ClangFormat: apply to source, most of intern
[blender.git] / intern / cycles / util / util_math_matrix.h
1 /*
2  * Copyright 2011-2017 Blender Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 #ifndef __UTIL_MATH_MATRIX_H__
18 #define __UTIL_MATH_MATRIX_H__
19
20 CCL_NAMESPACE_BEGIN
21
22 #define MAT(A, size, row, col) A[(row) * (size) + (col)]
23
24 /* Variants that use a constant stride on GPUS. */
25 #ifdef __KERNEL_GPU__
26 #  define MATS(A, n, r, c, s) A[((r) * (n) + (c)) * (s)]
27 /* Element access when only the lower-triangular elements are stored. */
28 #  define MATHS(A, r, c, s) A[((r) * ((r) + 1) / 2 + (c)) * (s)]
29 #  define VECS(V, i, s) V[(i) * (s)]
30 #else
31 #  define MATS(A, n, r, c, s) MAT(A, n, r, c)
32 #  define MATHS(A, r, c, s) A[(r) * ((r) + 1) / 2 + (c)]
33 #  define VECS(V, i, s) V[i]
34 #endif
35
36 /* Zeroing helpers. */
37
38 ccl_device_inline void math_vector_zero(float *v, int n)
39 {
40   for (int i = 0; i < n; i++) {
41     v[i] = 0.0f;
42   }
43 }
44
45 ccl_device_inline void math_matrix_zero(float *A, int n)
46 {
47   for (int row = 0; row < n; row++) {
48     for (int col = 0; col <= row; col++) {
49       MAT(A, n, row, col) = 0.0f;
50     }
51   }
52 }
53
54 /* Elementary vector operations. */
55
56 ccl_device_inline void math_vector_add(float *a, const float *ccl_restrict b, int n)
57 {
58   for (int i = 0; i < n; i++) {
59     a[i] += b[i];
60   }
61 }
62
63 ccl_device_inline void math_vector_mul(float *a, const float *ccl_restrict b, int n)
64 {
65   for (int i = 0; i < n; i++) {
66     a[i] *= b[i];
67   }
68 }
69
70 ccl_device_inline void math_vector_mul_strided(ccl_global float *a,
71                                                const float *ccl_restrict b,
72                                                int astride,
73                                                int n)
74 {
75   for (int i = 0; i < n; i++) {
76     a[i * astride] *= b[i];
77   }
78 }
79
80 ccl_device_inline void math_vector_scale(float *a, float b, int n)
81 {
82   for (int i = 0; i < n; i++) {
83     a[i] *= b;
84   }
85 }
86
87 ccl_device_inline void math_vector_max(float *a, const float *ccl_restrict b, int n)
88 {
89   for (int i = 0; i < n; i++) {
90     a[i] = max(a[i], b[i]);
91   }
92 }
93
94 ccl_device_inline void math_vec3_add(float3 *v, int n, float *x, float3 w)
95 {
96   for (int i = 0; i < n; i++) {
97     v[i] += w * x[i];
98   }
99 }
100
101 ccl_device_inline void math_vec3_add_strided(
102     ccl_global float3 *v, int n, float *x, float3 w, int stride)
103 {
104   for (int i = 0; i < n; i++) {
105     ccl_global float *elem = (ccl_global float *)(v + i * stride);
106     atomic_add_and_fetch_float(elem + 0, w.x * x[i]);
107     atomic_add_and_fetch_float(elem + 1, w.y * x[i]);
108     atomic_add_and_fetch_float(elem + 2, w.z * x[i]);
109   }
110 }
111
112 /* Elementary matrix operations.
113  * Note: TriMatrix refers to a square matrix that is symmetric, and therefore its upper-triangular part isn't stored. */
114
115 ccl_device_inline void math_trimatrix_add_diagonal(ccl_global float *A,
116                                                    int n,
117                                                    float val,
118                                                    int stride)
119 {
120   for (int row = 0; row < n; row++) {
121     MATHS(A, row, row, stride) += val;
122   }
123 }
124
125 /* Add Gramian matrix of v to A.
126  * The Gramian matrix of v is vt*v, so element (i,j) is v[i]*v[j]. */
127 ccl_device_inline void math_matrix_add_gramian(float *A,
128                                                int n,
129                                                const float *ccl_restrict v,
130                                                float weight)
131 {
132   for (int row = 0; row < n; row++) {
133     for (int col = 0; col <= row; col++) {
134       MAT(A, n, row, col) += v[row] * v[col] * weight;
135     }
136   }
137 }
138
139 /* Add Gramian matrix of v to A.
140  * The Gramian matrix of v is vt*v, so element (i,j) is v[i]*v[j]. */
141 ccl_device_inline void math_trimatrix_add_gramian_strided(
142     ccl_global float *A, int n, const float *ccl_restrict v, float weight, int stride)
143 {
144   for (int row = 0; row < n; row++) {
145     for (int col = 0; col <= row; col++) {
146       atomic_add_and_fetch_float(&MATHS(A, row, col, stride), v[row] * v[col] * weight);
147     }
148   }
149 }
150
151 ccl_device_inline void math_trimatrix_add_gramian(ccl_global float *A,
152                                                   int n,
153                                                   const float *ccl_restrict v,
154                                                   float weight)
155 {
156   for (int row = 0; row < n; row++) {
157     for (int col = 0; col <= row; col++) {
158       MATHS(A, row, col, 1) += v[row] * v[col] * weight;
159     }
160   }
161 }
162
163 /* Transpose matrix A inplace. */
164 ccl_device_inline void math_matrix_transpose(ccl_global float *A, int n, int stride)
165 {
166   for (int i = 0; i < n; i++) {
167     for (int j = 0; j < i; j++) {
168       float temp = MATS(A, n, i, j, stride);
169       MATS(A, n, i, j, stride) = MATS(A, n, j, i, stride);
170       MATS(A, n, j, i, stride) = temp;
171     }
172   }
173 }
174
175 /* Solvers for matrix problems */
176
177 /* In-place Cholesky-Banachiewicz decomposition of the square, positive-definite matrix A
178  * into a lower triangular matrix L so that A = L*L^T. A is being overwritten by L.
179  * Also, only the lower triangular part of A is ever accessed. */
180 ccl_device void math_trimatrix_cholesky(ccl_global float *A, int n, int stride)
181 {
182   for (int row = 0; row < n; row++) {
183     for (int col = 0; col <= row; col++) {
184       float sum_col = MATHS(A, row, col, stride);
185       for (int k = 0; k < col; k++) {
186         sum_col -= MATHS(A, row, k, stride) * MATHS(A, col, k, stride);
187       }
188       if (row == col) {
189         sum_col = sqrtf(max(sum_col, 0.0f));
190       }
191       else {
192         sum_col /= MATHS(A, col, col, stride);
193       }
194       MATHS(A, row, col, stride) = sum_col;
195     }
196   }
197 }
198
199 /* Solve A*S=y for S given A and y, where A is symmetrical positive-semidefinite and both inputs are destroyed in the process.
200  *
201  * We can apply Cholesky decomposition to find a lower triangular L so that L*Lt = A.
202  * With that we get (L*Lt)*S = L*(Lt*S) = L*b = y, defining b as Lt*S.
203  * Since L is lower triangular, finding b is relatively easy since y is known.
204  * Then, the remaining problem is Lt*S = b, which again can be solved easily.
205  *
206  * This is useful for solving the normal equation S=inv(Xt*W*X)*Xt*W*y, since Xt*W*X is
207  * symmetrical positive-semidefinite by construction, so we can just use this function with A=Xt*W*X and y=Xt*W*y. */
208 ccl_device_inline void math_trimatrix_vec3_solve(ccl_global float *A,
209                                                  ccl_global float3 *y,
210                                                  int n,
211                                                  int stride)
212 {
213   /* Since the first entry of the design row is always 1, the upper-left element of XtWX is a good
214    * heuristic for the amount of pixels considered (with weighting), therefore the amount of correction
215    * is scaled based on it. */
216   math_trimatrix_add_diagonal(A, n, 3e-7f * A[0], stride); /* Improve the numerical stability. */
217   math_trimatrix_cholesky(A, n, stride);                   /* Replace A with L so that L*Lt = A. */
218
219   /* Use forward substitution to solve L*b = y, replacing y by b. */
220   for (int row = 0; row < n; row++) {
221     float3 sum = VECS(y, row, stride);
222     for (int col = 0; col < row; col++)
223       sum -= MATHS(A, row, col, stride) * VECS(y, col, stride);
224     VECS(y, row, stride) = sum / MATHS(A, row, row, stride);
225   }
226
227   /* Use backward substitution to solve Lt*S = b, replacing b by S. */
228   for (int row = n - 1; row >= 0; row--) {
229     float3 sum = VECS(y, row, stride);
230     for (int col = row + 1; col < n; col++)
231       sum -= MATHS(A, col, row, stride) * VECS(y, col, stride);
232     VECS(y, row, stride) = sum / MATHS(A, row, row, stride);
233   }
234 }
235
236 /* Perform the Jacobi Eigenvalue Methon on matrix A.
237  * A is assumed to be a symmetrical matrix, therefore only the lower-triangular part is ever accessed.
238  * The algorithm overwrites the contents of A.
239  *
240  * After returning, A will be overwritten with D, which is (almost) diagonal,
241  * and V will contain the eigenvectors of the original A in its rows (!),
242  * so that A = V^T*D*V. Therefore, the diagonal elements of D are the (sorted) eigenvalues of A.
243  */
244 ccl_device void math_matrix_jacobi_eigendecomposition(float *A,
245                                                       ccl_global float *V,
246                                                       int n,
247                                                       int v_stride)
248 {
249   const float singular_epsilon = 1e-9f;
250
251   for (int row = 0; row < n; row++) {
252     for (int col = 0; col < n; col++) {
253       MATS(V, n, row, col, v_stride) = (col == row) ? 1.0f : 0.0f;
254     }
255   }
256
257   for (int sweep = 0; sweep < 8; sweep++) {
258     float off_diagonal = 0.0f;
259     for (int row = 1; row < n; row++) {
260       for (int col = 0; col < row; col++) {
261         off_diagonal += fabsf(MAT(A, n, row, col));
262       }
263     }
264     if (off_diagonal < 1e-7f) {
265       /* The matrix has nearly reached diagonal form.
266        * Since the eigenvalues are only used to determine truncation, their exact values aren't required - a relative error of a few ULPs won't matter at all. */
267       break;
268     }
269
270     /* Set the threshold for the small element rotation skip in the first sweep:
271      * Skip all elements that are less than a tenth of the average off-diagonal element. */
272     float threshold = 0.2f * off_diagonal / (n * n);
273
274     for (int row = 1; row < n; row++) {
275       for (int col = 0; col < row; col++) {
276         /* Perform a Jacobi rotation on this element that reduces it to zero. */
277         float element = MAT(A, n, row, col);
278         float abs_element = fabsf(element);
279
280         /* If we're in a later sweep and the element already is very small, just set it to zero and skip the rotation. */
281         if (sweep > 3 && abs_element <= singular_epsilon * fabsf(MAT(A, n, row, row)) &&
282             abs_element <= singular_epsilon * fabsf(MAT(A, n, col, col))) {
283           MAT(A, n, row, col) = 0.0f;
284           continue;
285         }
286
287         if (element == 0.0f) {
288           continue;
289         }
290
291         /* If we're in one of the first sweeps and the element is smaller than the threshold, skip it. */
292         if (sweep < 3 && (abs_element < threshold)) {
293           continue;
294         }
295
296         /* Determine rotation: The rotation is characterized by its angle phi - or, in the actual implementation, sin(phi) and cos(phi).
297          * To find those, we first compute their ratio - that might be unstable if the angle approaches 90°, so there's a fallback for that case.
298          * Then, we compute sin(phi) and cos(phi) themselves. */
299         float singular_diff = MAT(A, n, row, row) - MAT(A, n, col, col);
300         float ratio;
301         if (abs_element > singular_epsilon * fabsf(singular_diff)) {
302           float cot_2phi = 0.5f * singular_diff / element;
303           ratio = 1.0f / (fabsf(cot_2phi) + sqrtf(1.0f + cot_2phi * cot_2phi));
304           if (cot_2phi < 0.0f)
305             ratio = -ratio; /* Copy sign. */
306         }
307         else {
308           ratio = element / singular_diff;
309         }
310
311         float c = 1.0f / sqrtf(1.0f + ratio * ratio);
312         float s = ratio * c;
313         /* To improve numerical stability by avoiding cancellation, the update equations are reformulized to use sin(phi) and tan(phi/2) instead. */
314         float tan_phi_2 = s / (1.0f + c);
315
316         /* Update the singular values in the diagonal. */
317         float singular_delta = ratio * element;
318         MAT(A, n, row, row) += singular_delta;
319         MAT(A, n, col, col) -= singular_delta;
320
321         /* Set the element itself to zero. */
322         MAT(A, n, row, col) = 0.0f;
323
324         /* Perform the actual rotations on the matrices. */
325 #define ROT(M, r1, c1, r2, c2, stride) \
326   { \
327     float M1 = MATS(M, n, r1, c1, stride); \
328     float M2 = MATS(M, n, r2, c2, stride); \
329     MATS(M, n, r1, c1, stride) -= s * (M2 + tan_phi_2 * M1); \
330     MATS(M, n, r2, c2, stride) += s * (M1 - tan_phi_2 * M2); \
331   }
332
333         /* Split into three parts to ensure correct accesses since we only store the lower-triangular part of A. */
334         for (int i = 0; i < col; i++)
335           ROT(A, col, i, row, i, 1);
336         for (int i = col + 1; i < row; i++)
337           ROT(A, i, col, row, i, 1);
338         for (int i = row + 1; i < n; i++)
339           ROT(A, i, col, i, row, 1);
340
341         for (int i = 0; i < n; i++)
342           ROT(V, col, i, row, i, v_stride);
343 #undef ROT
344       }
345     }
346   }
347
348   /* Sort eigenvalues and the associated eigenvectors. */
349   for (int i = 0; i < n - 1; i++) {
350     float v = MAT(A, n, i, i);
351     int k = i;
352     for (int j = i; j < n; j++) {
353       if (MAT(A, n, j, j) >= v) {
354         v = MAT(A, n, j, j);
355         k = j;
356       }
357     }
358     if (k != i) {
359       /* Swap eigenvalues. */
360       MAT(A, n, k, k) = MAT(A, n, i, i);
361       MAT(A, n, i, i) = v;
362       /* Swap eigenvectors. */
363       for (int j = 0; j < n; j++) {
364         float v = MATS(V, n, i, j, v_stride);
365         MATS(V, n, i, j, v_stride) = MATS(V, n, k, j, v_stride);
366         MATS(V, n, k, j, v_stride) = v;
367       }
368     }
369   }
370 }
371
372 #ifdef __KERNEL_SSE3__
373 ccl_device_inline void math_vector_zero_sse(float4 *A, int n)
374 {
375   for (int i = 0; i < n; i++) {
376     A[i] = make_float4(0.0f);
377   }
378 }
379
380 ccl_device_inline void math_matrix_zero_sse(float4 *A, int n)
381 {
382   for (int row = 0; row < n; row++) {
383     for (int col = 0; col <= row; col++) {
384       MAT(A, n, row, col) = make_float4(0.0f);
385     }
386   }
387 }
388
389 /* Add Gramian matrix of v to A.
390  * The Gramian matrix of v is v^T*v, so element (i,j) is v[i]*v[j]. */
391 ccl_device_inline void math_matrix_add_gramian_sse(float4 *A,
392                                                    int n,
393                                                    const float4 *ccl_restrict v,
394                                                    float4 weight)
395 {
396   for (int row = 0; row < n; row++) {
397     for (int col = 0; col <= row; col++) {
398       MAT(A, n, row, col) = MAT(A, n, row, col) + v[row] * v[col] * weight;
399     }
400   }
401 }
402
403 ccl_device_inline void math_vector_add_sse(float4 *V, int n, const float4 *ccl_restrict a)
404 {
405   for (int i = 0; i < n; i++) {
406     V[i] += a[i];
407   }
408 }
409
410 ccl_device_inline void math_vector_mul_sse(float4 *V, int n, const float4 *ccl_restrict a)
411 {
412   for (int i = 0; i < n; i++) {
413     V[i] *= a[i];
414   }
415 }
416
417 ccl_device_inline void math_vector_max_sse(float4 *a, const float4 *ccl_restrict b, int n)
418 {
419   for (int i = 0; i < n; i++) {
420     a[i] = max(a[i], b[i]);
421   }
422 }
423
424 ccl_device_inline void math_matrix_hsum(float *A, int n, const float4 *ccl_restrict B)
425 {
426   for (int row = 0; row < n; row++) {
427     for (int col = 0; col <= row; col++) {
428       MAT(A, n, row, col) = reduce_add(MAT(B, n, row, col))[0];
429     }
430   }
431 }
432 #endif
433
434 #undef MAT
435
436 CCL_NAMESPACE_END
437
438 #endif /* __UTIL_MATH_MATRIX_H__ */