Blender V5.3
math_matrix_c.cc
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
8
11#include "BLI_math_solvers.hh"
12#include "BLI_math_vector_c.hh"
13#include "BLI_simd.hh"
14
15#ifndef MATH_STANDALONE
16# include "eigen_capi.h"
17#endif
18
19#include <algorithm>
20#include <cstring>
21
22#include "BLI_strict_flags.hh" /* IWYU pragma: keep. Keep last. */
23
24namespace blender {
25
26/* -------------------------------------------------------------------- */
29
30void zero_m3(float m[3][3])
31{
32 memset(m, 0, sizeof(float[3][3]));
33}
34
35void zero_m4(float m[4][4])
36{
37 memset(m, 0, sizeof(float[4][4]));
38}
39
40void unit_m2(float m[2][2])
41{
42 m[0][0] = m[1][1] = 1.0f;
43 m[0][1] = 0.0f;
44 m[1][0] = 0.0f;
45}
46
47void unit_m3(float m[3][3])
48{
49 m[0][0] = m[1][1] = m[2][2] = 1.0f;
50 m[0][1] = m[0][2] = 0.0f;
51 m[1][0] = m[1][2] = 0.0f;
52 m[2][0] = m[2][1] = 0.0f;
53}
54
55void unit_m4(float m[4][4])
56{
57 m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
58 m[0][1] = m[0][2] = m[0][3] = 0.0f;
59 m[1][0] = m[1][2] = m[1][3] = 0.0f;
60 m[2][0] = m[2][1] = m[2][3] = 0.0f;
61 m[3][0] = m[3][1] = m[3][2] = 0.0f;
62}
63
64void unit_m4_db(double m[4][4])
65{
66 m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
67 m[0][1] = m[0][2] = m[0][3] = 0.0f;
68 m[1][0] = m[1][2] = m[1][3] = 0.0f;
69 m[2][0] = m[2][1] = m[2][3] = 0.0f;
70 m[3][0] = m[3][1] = m[3][2] = 0.0f;
71}
72
73void copy_m2_m2(float m1[2][2], const float m2[2][2])
74{
75 memcpy(m1, m2, sizeof(float[2][2]));
76}
77
78void copy_m3_m3(float m1[3][3], const float m2[3][3])
79{
80 /* destination comes first: */
81 memcpy(m1, m2, sizeof(float[3][3]));
82}
83
84void copy_m4_m4(float m1[4][4], const float m2[4][4])
85{
86 memcpy(m1, m2, sizeof(float[4][4]));
87}
88
89void copy_m4_m4_db(double m1[4][4], const double m2[4][4])
90{
91 memcpy(m1, m2, sizeof(double[4][4]));
92}
93
94void copy_m3_m4(float m1[3][3], const float m2[4][4])
95{
96 m1[0][0] = m2[0][0];
97 m1[0][1] = m2[0][1];
98 m1[0][2] = m2[0][2];
99
100 m1[1][0] = m2[1][0];
101 m1[1][1] = m2[1][1];
102 m1[1][2] = m2[1][2];
103
104 m1[2][0] = m2[2][0];
105 m1[2][1] = m2[2][1];
106 m1[2][2] = m2[2][2];
107}
108
109void copy_m4_m3(float m1[4][4], const float m2[3][3]) /* no clear */
110{
111 m1[0][0] = m2[0][0];
112 m1[0][1] = m2[0][1];
113 m1[0][2] = m2[0][2];
114
115 m1[1][0] = m2[1][0];
116 m1[1][1] = m2[1][1];
117 m1[1][2] = m2[1][2];
118
119 m1[2][0] = m2[2][0];
120 m1[2][1] = m2[2][1];
121 m1[2][2] = m2[2][2];
122
123 m1[0][3] = 0.0f;
124 m1[1][3] = 0.0f;
125 m1[2][3] = 0.0f;
126
127 m1[3][0] = 0.0f;
128 m1[3][1] = 0.0f;
129 m1[3][2] = 0.0f;
130 m1[3][3] = 1.0f;
131}
132
133void copy_m3d_m3(double m1[3][3], const float m2[3][3])
134{
135 m1[0][0] = m2[0][0];
136 m1[0][1] = m2[0][1];
137 m1[0][2] = m2[0][2];
138
139 m1[1][0] = m2[1][0];
140 m1[1][1] = m2[1][1];
141 m1[1][2] = m2[1][2];
142
143 m1[2][0] = m2[2][0];
144 m1[2][1] = m2[2][1];
145 m1[2][2] = m2[2][2];
146}
147
148void copy_m4d_m4(double m1[4][4], const float m2[4][4])
149{
150 m1[0][0] = m2[0][0];
151 m1[0][1] = m2[0][1];
152 m1[0][2] = m2[0][2];
153 m1[0][3] = m2[0][3];
154
155 m1[1][0] = m2[1][0];
156 m1[1][1] = m2[1][1];
157 m1[1][2] = m2[1][2];
158 m1[1][3] = m2[1][3];
159
160 m1[2][0] = m2[2][0];
161 m1[2][1] = m2[2][1];
162 m1[2][2] = m2[2][2];
163 m1[2][3] = m2[2][3];
164
165 m1[3][0] = m2[3][0];
166 m1[3][1] = m2[3][1];
167 m1[3][2] = m2[3][2];
168 m1[3][3] = m2[3][3];
169}
170
171void copy_m3_m3d(float m1[3][3], const double m2[3][3])
172{
173 /* Keep it stupid simple for better data flow in CPU. */
174 m1[0][0] = float(m2[0][0]);
175 m1[0][1] = float(m2[0][1]);
176 m1[0][2] = float(m2[0][2]);
177
178 m1[1][0] = float(m2[1][0]);
179 m1[1][1] = float(m2[1][1]);
180 m1[1][2] = float(m2[1][2]);
181
182 m1[2][0] = float(m2[2][0]);
183 m1[2][1] = float(m2[2][1]);
184 m1[2][2] = float(m2[2][2]);
185}
186
187void swap_m4m4(float m1[4][4], float m2[4][4])
188{
189 float t;
190 int i, j;
191
192 for (i = 0; i < 4; i++) {
193 for (j = 0; j < 4; j++) {
194 t = m1[i][j];
195 m1[i][j] = m2[i][j];
196 m2[i][j] = t;
197 }
198 }
199}
200
201void shuffle_m4(float R[4][4], const int index[4])
202{
203 zero_m4(R);
204 for (int k = 0; k < 4; k++) {
205 if (index[k] >= 0) {
206 R[index[k]][k] = 1.0f;
207 }
208 }
209}
210
212
213/* -------------------------------------------------------------------- */
216
217void mul_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
218{
219 if (ELEM(R, A, B)) {
220 float T[4][4];
221 mul_m4_m4m4(T, A, B);
222 copy_m4_m4(R, T);
223 return;
224 }
225
226 /* Matrix product: `R[j][k] = B[j][i] . A[i][k]`. */
227#if BLI_HAVE_SSE2
228 __m128 A0 = _mm_loadu_ps(A[0]);
229 __m128 A1 = _mm_loadu_ps(A[1]);
230 __m128 A2 = _mm_loadu_ps(A[2]);
231 __m128 A3 = _mm_loadu_ps(A[3]);
232
233 for (int i = 0; i < 4; i++) {
234 __m128 B0 = _mm_set1_ps(B[i][0]);
235 __m128 B1 = _mm_set1_ps(B[i][1]);
236 __m128 B2 = _mm_set1_ps(B[i][2]);
237 __m128 B3 = _mm_set1_ps(B[i][3]);
238
239 __m128 sum = _mm_add_ps(_mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)),
240 _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3)));
241
242 _mm_storeu_ps(R[i], sum);
243 }
244#else
245 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0] + B[0][3] * A[3][0];
246 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1] + B[0][3] * A[3][1];
247 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2] + B[0][3] * A[3][2];
248 R[0][3] = B[0][0] * A[0][3] + B[0][1] * A[1][3] + B[0][2] * A[2][3] + B[0][3] * A[3][3];
249
250 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0] + B[1][3] * A[3][0];
251 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1] + B[1][3] * A[3][1];
252 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2] + B[1][3] * A[3][2];
253 R[1][3] = B[1][0] * A[0][3] + B[1][1] * A[1][3] + B[1][2] * A[2][3] + B[1][3] * A[3][3];
254
255 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0] + B[2][3] * A[3][0];
256 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1] + B[2][3] * A[3][1];
257 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2] + B[2][3] * A[3][2];
258 R[2][3] = B[2][0] * A[0][3] + B[2][1] * A[1][3] + B[2][2] * A[2][3] + B[2][3] * A[3][3];
259
260 R[3][0] = B[3][0] * A[0][0] + B[3][1] * A[1][0] + B[3][2] * A[2][0] + B[3][3] * A[3][0];
261 R[3][1] = B[3][0] * A[0][1] + B[3][1] * A[1][1] + B[3][2] * A[2][1] + B[3][3] * A[3][1];
262 R[3][2] = B[3][0] * A[0][2] + B[3][1] * A[1][2] + B[3][2] * A[2][2] + B[3][3] * A[3][2];
263 R[3][3] = B[3][0] * A[0][3] + B[3][1] * A[1][3] + B[3][2] * A[2][3] + B[3][3] * A[3][3];
264#endif
265}
266
267void mul_m4db_m4db_m4fl(double R[4][4], const double A[4][4], const float B[4][4])
268{
269 if (R == A) {
270 double T[4][4];
272 copy_m4_m4_db(R, T);
273 return;
274 }
275
276 /* Matrix product: `R[j][k] = B[j][i] . A[i][k]`. */
277
278 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0] + B[0][3] * A[3][0];
279 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1] + B[0][3] * A[3][1];
280 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2] + B[0][3] * A[3][2];
281 R[0][3] = B[0][0] * A[0][3] + B[0][1] * A[1][3] + B[0][2] * A[2][3] + B[0][3] * A[3][3];
282
283 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0] + B[1][3] * A[3][0];
284 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1] + B[1][3] * A[3][1];
285 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2] + B[1][3] * A[3][2];
286 R[1][3] = B[1][0] * A[0][3] + B[1][1] * A[1][3] + B[1][2] * A[2][3] + B[1][3] * A[3][3];
287
288 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0] + B[2][3] * A[3][0];
289 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1] + B[2][3] * A[3][1];
290 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2] + B[2][3] * A[3][2];
291 R[2][3] = B[2][0] * A[0][3] + B[2][1] * A[1][3] + B[2][2] * A[2][3] + B[2][3] * A[3][3];
292
293 R[3][0] = B[3][0] * A[0][0] + B[3][1] * A[1][0] + B[3][2] * A[2][0] + B[3][3] * A[3][0];
294 R[3][1] = B[3][0] * A[0][1] + B[3][1] * A[1][1] + B[3][2] * A[2][1] + B[3][3] * A[3][1];
295 R[3][2] = B[3][0] * A[0][2] + B[3][1] * A[1][2] + B[3][2] * A[2][2] + B[3][3] * A[3][2];
296 R[3][3] = B[3][0] * A[0][3] + B[3][1] * A[1][3] + B[3][2] * A[2][3] + B[3][3] * A[3][3];
297}
298
299void mul_m4_m4_pre(float R[4][4], const float A[4][4])
300{
301 mul_m4_m4m4(R, A, R);
302}
303
304void mul_m4_m4_post(float R[4][4], const float B[4][4])
305{
306 mul_m4_m4m4(R, R, B);
307}
308
309void mul_m3_m3_pre(float R[3][3], const float A[3][3])
310{
311 mul_m3_m3m3(R, A, R);
312}
313
314void mul_m3_m3_post(float R[3][3], const float B[3][3])
315{
316 mul_m3_m3m3(R, R, B);
317}
318
319void mul_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
320{
321 if (ELEM(R, A, B)) {
322 float T[3][3];
323 mul_m3_m3m3(T, A, B);
324 copy_m3_m3(R, T);
325 return;
326 }
327 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
328 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
329 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
330
331 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
332 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
333 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
334
335 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
336 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
337 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
338}
339
340void mul_m4_m4m3(float R[4][4], const float A[4][4], const float B[3][3])
341{
342 if (R == A) {
343 float T[4][4];
344 /* The mul_m4_m4m3 only writes to the upper-left 3x3 block, so make it so the rest of the
345 * matrix is copied from the input to the output.
346 *
347 * TODO(sergey): It does sound a bit redundant from the number of copy operations, so there is
348 * a potential for optimization. */
349 copy_m4_m4(T, A);
350 mul_m4_m4m3(T, A, B);
351 copy_m4_m4(R, T);
352 return;
353 }
354
355 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
356 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
357 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
358 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
359 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
360 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
361 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
362 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
363 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
364}
365
366void mul_m3_m3m4(float R[3][3], const float A[3][3], const float B[4][4])
367{
368 if (R == A) {
369 float T[3][3];
370 mul_m3_m3m4(T, A, B);
371 copy_m3_m3(R, T);
372 return;
373 }
374
375 /* Matrix product: `R[j][k] = B[j][i] . A[i][k]`. */
376
377 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
378 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
379 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
380
381 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
382 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
383 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
384
385 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
386 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
387 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
388}
389
390void mul_m3_m4m3(float R[3][3], const float A[4][4], const float B[3][3])
391{
392 if (R == B) {
393 float T[3][3];
394 mul_m3_m4m3(T, A, B);
395 copy_m3_m3(R, T);
396 return;
397 }
398
399 /* Matrix product: `R[j][k] = B[j][i] . A[i][k]`. */
400
401 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
402 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
403 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
404
405 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
406 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
407 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
408
409 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
410 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
411 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
412}
413
414void mul_m4_m3m4(float R[4][4], const float A[3][3], const float B[4][4])
415{
416 if (R == B) {
417 float T[4][4];
418 /* The mul_m4_m4m3 only writes to the upper-left 3x3 block, so make it so the rest of the
419 * matrix is copied from the input to the output.
420 *
421 * TODO(sergey): It does sound a bit redundant from the number of copy operations, so there is
422 * a potential for optimization. */
423 copy_m4_m4(T, B);
424 mul_m4_m3m4(T, A, B);
425 copy_m4_m4(R, T);
426 return;
427 }
428
429 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
430 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
431 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
432 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
433 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
434 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
435 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
436 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
437 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
438}
439
440void mul_m3_m4m4(float R[3][3], const float A[4][4], const float B[4][4])
441{
442 R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
443 R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
444 R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
445 R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
446 R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
447 R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
448 R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
449 R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
450 R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
451}
452
454
455/* -------------------------------------------------------------------- */
458
459void _va_mul_m3_series_3(float r[3][3], const float m1[3][3], const float m2[3][3])
460{
461 mul_m3_m3m3(r, m1, m2);
462}
463void _va_mul_m3_series_4(float r[3][3],
464 const float m1[3][3],
465 const float m2[3][3],
466 const float m3[3][3])
467{
468 float s[3][3];
469 mul_m3_m3m3(s, m1, m2);
470 mul_m3_m3m3(r, s, m3);
471}
472void _va_mul_m3_series_5(float r[3][3],
473 const float m1[3][3],
474 const float m2[3][3],
475 const float m3[3][3],
476 const float m4[3][3])
477{
478 float s[3][3];
479 float t[3][3];
480 mul_m3_m3m3(s, m1, m2);
481 mul_m3_m3m3(t, s, m3);
482 mul_m3_m3m3(r, t, m4);
483}
484void _va_mul_m3_series_6(float r[3][3],
485 const float m1[3][3],
486 const float m2[3][3],
487 const float m3[3][3],
488 const float m4[3][3],
489 const float m5[3][3])
490{
491 float s[3][3];
492 float t[3][3];
493 mul_m3_m3m3(s, m1, m2);
494 mul_m3_m3m3(t, s, m3);
495 mul_m3_m3m3(s, t, m4);
496 mul_m3_m3m3(r, s, m5);
497}
498void _va_mul_m3_series_7(float r[3][3],
499 const float m1[3][3],
500 const float m2[3][3],
501 const float m3[3][3],
502 const float m4[3][3],
503 const float m5[3][3],
504 const float m6[3][3])
505{
506 float s[3][3];
507 float t[3][3];
508 mul_m3_m3m3(s, m1, m2);
509 mul_m3_m3m3(t, s, m3);
510 mul_m3_m3m3(s, t, m4);
511 mul_m3_m3m3(t, s, m5);
512 mul_m3_m3m3(r, t, m6);
513}
514void _va_mul_m3_series_8(float r[3][3],
515 const float m1[3][3],
516 const float m2[3][3],
517 const float m3[3][3],
518 const float m4[3][3],
519 const float m5[3][3],
520 const float m6[3][3],
521 const float m7[3][3])
522{
523 float s[3][3];
524 float t[3][3];
525 mul_m3_m3m3(s, m1, m2);
526 mul_m3_m3m3(t, s, m3);
527 mul_m3_m3m3(s, t, m4);
528 mul_m3_m3m3(t, s, m5);
529 mul_m3_m3m3(s, t, m6);
530 mul_m3_m3m3(r, s, m7);
531}
532void _va_mul_m3_series_9(float r[3][3],
533 const float m1[3][3],
534 const float m2[3][3],
535 const float m3[3][3],
536 const float m4[3][3],
537 const float m5[3][3],
538 const float m6[3][3],
539 const float m7[3][3],
540 const float m8[3][3])
541{
542 float s[3][3];
543 float t[3][3];
544 mul_m3_m3m3(s, m1, m2);
545 mul_m3_m3m3(t, s, m3);
546 mul_m3_m3m3(s, t, m4);
547 mul_m3_m3m3(t, s, m5);
548 mul_m3_m3m3(s, t, m6);
549 mul_m3_m3m3(t, s, m7);
550 mul_m3_m3m3(r, t, m8);
551}
552
554
555/* -------------------------------------------------------------------- */
558
559void _va_mul_m4_series_3(float r[4][4], const float m1[4][4], const float m2[4][4])
560{
561 mul_m4_m4m4(r, m1, m2);
562}
563void _va_mul_m4_series_4(float r[4][4],
564 const float m1[4][4],
565 const float m2[4][4],
566 const float m3[4][4])
567{
568 float s[4][4];
569 mul_m4_m4m4(s, m1, m2);
570 mul_m4_m4m4(r, s, m3);
571}
572void _va_mul_m4_series_5(float r[4][4],
573 const float m1[4][4],
574 const float m2[4][4],
575 const float m3[4][4],
576 const float m4[4][4])
577{
578 float s[4][4];
579 float t[4][4];
580 mul_m4_m4m4(s, m1, m2);
581 mul_m4_m4m4(t, s, m3);
582 mul_m4_m4m4(r, t, m4);
583}
584void _va_mul_m4_series_6(float r[4][4],
585 const float m1[4][4],
586 const float m2[4][4],
587 const float m3[4][4],
588 const float m4[4][4],
589 const float m5[4][4])
590{
591 float s[4][4];
592 float t[4][4];
593 mul_m4_m4m4(s, m1, m2);
594 mul_m4_m4m4(t, s, m3);
595 mul_m4_m4m4(s, t, m4);
596 mul_m4_m4m4(r, s, m5);
597}
598void _va_mul_m4_series_7(float r[4][4],
599 const float m1[4][4],
600 const float m2[4][4],
601 const float m3[4][4],
602 const float m4[4][4],
603 const float m5[4][4],
604 const float m6[4][4])
605{
606 float s[4][4];
607 float t[4][4];
608 mul_m4_m4m4(s, m1, m2);
609 mul_m4_m4m4(t, s, m3);
610 mul_m4_m4m4(s, t, m4);
611 mul_m4_m4m4(t, s, m5);
612 mul_m4_m4m4(r, t, m6);
613}
614void _va_mul_m4_series_8(float r[4][4],
615 const float m1[4][4],
616 const float m2[4][4],
617 const float m3[4][4],
618 const float m4[4][4],
619 const float m5[4][4],
620 const float m6[4][4],
621 const float m7[4][4])
622{
623 float s[4][4];
624 float t[4][4];
625 mul_m4_m4m4(s, m1, m2);
626 mul_m4_m4m4(t, s, m3);
627 mul_m4_m4m4(s, t, m4);
628 mul_m4_m4m4(t, s, m5);
629 mul_m4_m4m4(s, t, m6);
630 mul_m4_m4m4(r, s, m7);
631}
632void _va_mul_m4_series_9(float r[4][4],
633 const float m1[4][4],
634 const float m2[4][4],
635 const float m3[4][4],
636 const float m4[4][4],
637 const float m5[4][4],
638 const float m6[4][4],
639 const float m7[4][4],
640 const float m8[4][4])
641{
642 float s[4][4];
643 float t[4][4];
644 mul_m4_m4m4(s, m1, m2);
645 mul_m4_m4m4(t, s, m3);
646 mul_m4_m4m4(s, t, m4);
647 mul_m4_m4m4(t, s, m5);
648 mul_m4_m4m4(s, t, m6);
649 mul_m4_m4m4(t, s, m7);
650 mul_m4_m4m4(r, t, m8);
651}
652
654
655/* -------------------------------------------------------------------- */
658
659void mul_v2_m3v2(float r[2], const float m[3][3], const float v[2])
660{
661 float temp[3], warped[3];
662
663 copy_v2_v2(temp, v);
664 temp[2] = 1.0f;
665
666 mul_v3_m3v3(warped, m, temp);
667
668 r[0] = warped[0] / warped[2];
669 r[1] = warped[1] / warped[2];
670}
671
672void mul_m3_v2(const float m[3][3], float r[2])
673{
674 mul_v2_m3v2(r, m, r);
675}
676
677void mul_m4_v3(const float M[4][4], float r[3])
678{
679 const float x = r[0];
680 const float y = r[1];
681
682 r[0] = x * M[0][0] + y * M[1][0] + M[2][0] * r[2] + M[3][0];
683 r[1] = x * M[0][1] + y * M[1][1] + M[2][1] * r[2] + M[3][1];
684 r[2] = x * M[0][2] + y * M[1][2] + M[2][2] * r[2] + M[3][2];
685}
686
687void mul_v3_m4v3(float r[3], const float mat[4][4], const float vec[3])
688{
689 const float x = vec[0];
690 const float y = vec[1];
691
692 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
693 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
694 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
695}
696
697void mul_v3_m4v3_db(double r[3], const double mat[4][4], const double vec[3])
698{
699 const double x = vec[0];
700 const double y = vec[1];
701
702 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
703 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
704 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
705}
706void mul_v4_m4v3_db(double r[4], const double mat[4][4], const double vec[3])
707{
708 const double x = vec[0];
709 const double y = vec[1];
710
711 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
712 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
713 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
714 r[3] = x * mat[0][3] + y * mat[1][3] + mat[2][3] * vec[2] + mat[3][3];
715}
716
717void mul_v2_m4v3(float r[2], const float mat[4][4], const float vec[3])
718{
719 const float x = vec[0];
720
721 r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
722 r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
723}
724
725void mul_v2_m2v2(float r[2], const float mat[2][2], const float vec[2])
726{
727 const float x = vec[0];
728
729 r[0] = mat[0][0] * x + mat[1][0] * vec[1];
730 r[1] = mat[0][1] * x + mat[1][1] * vec[1];
731}
732
733void mul_m2_v2(const float mat[2][2], float vec[2])
734{
735 mul_v2_m2v2(vec, mat, vec);
736}
737
738void mul_mat3_m4_v3(const float mat[4][4], float r[3])
739{
740 const float x = r[0];
741 const float y = r[1];
742
743 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * r[2];
744 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * r[2];
745 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * r[2];
746}
747
748void mul_v3_mat3_m4v3(float r[3], const float mat[4][4], const float vec[3])
749{
750 const float x = vec[0];
751 const float y = vec[1];
752
753 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
754 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
755 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
756}
757
758void mul_v3_mat3_m4v3_db(double r[3], const double mat[4][4], const double vec[3])
759{
760 const double x = vec[0];
761 const double y = vec[1];
762
763 r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
764 r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
765 r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
766}
767
768void mul_project_m4_v3(const float mat[4][4], float vec[3])
769{
770 /* absolute value to not flip the frustum upside down behind the camera */
771 const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
772 mul_m4_v3(mat, vec);
773
774 vec[0] /= w;
775 vec[1] /= w;
776 vec[2] /= w;
777}
778
779void mul_v3_project_m4_v3(float r[3], const float mat[4][4], const float vec[3])
780{
781 const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
782 mul_v3_m4v3(r, mat, vec);
783
784 r[0] /= w;
785 r[1] /= w;
786 r[2] /= w;
787}
788
789void mul_v2_project_m4_v3(float r[2], const float mat[4][4], const float vec[3])
790{
791 const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
792 mul_v2_m4v3(r, mat, vec);
793
794 r[0] /= w;
795 r[1] /= w;
796}
797
798void mul_v4_m4v4(float r[4], const float mat[4][4], const float v[4])
799{
800 const float x = v[0];
801 const float y = v[1];
802 const float z = v[2];
803
804 r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
805 r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
806 r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3];
807 r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3];
808}
809
810void mul_m4_v4(const float mat[4][4], float r[4])
811{
812 mul_v4_m4v4(r, mat, r);
813}
814
815void mul_v4_m4v3(float r[4], const float M[4][4], const float v[3])
816{
817 /* v has implicit w = 1.0f */
818 r[0] = v[0] * M[0][0] + v[1] * M[1][0] + M[2][0] * v[2] + M[3][0];
819 r[1] = v[0] * M[0][1] + v[1] * M[1][1] + M[2][1] * v[2] + M[3][1];
820 r[2] = v[0] * M[0][2] + v[1] * M[1][2] + M[2][2] * v[2] + M[3][2];
821 r[3] = v[0] * M[0][3] + v[1] * M[1][3] + M[2][3] * v[2] + M[3][3];
822}
823
824void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3])
825{
826 float t[3];
827 copy_v3_v3(t, a);
828
829 r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
830 r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
831 r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2];
832}
833
834void mul_v3_m3v3_db(double r[3], const double M[3][3], const double a[3])
835{
836 double t[3];
837 copy_v3_v3_db(t, a);
838
839 r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
840 r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
841 r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2];
842}
843
844void mul_v2_m3v3(float r[2], const float M[3][3], const float a[3])
845{
846 float t[3];
847 copy_v3_v3(t, a);
848
849 r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
850 r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
851}
852
853void mul_m3_v3(const float M[3][3], float r[3])
854{
855 mul_v3_m3v3(r, M, r);
856}
857
858void mul_m3_v3_db(const double M[3][3], double r[3])
859{
860 mul_v3_m3v3_db(r, M, r);
861}
862
863void mul_transposed_m3_v3(const float M[3][3], float r[3])
864{
865 const float x = r[0];
866 const float y = r[1];
867
868 r[0] = x * M[0][0] + y * M[0][1] + M[0][2] * r[2];
869 r[1] = x * M[1][0] + y * M[1][1] + M[1][2] * r[2];
870 r[2] = x * M[2][0] + y * M[2][1] + M[2][2] * r[2];
871}
872
873void mul_transposed_mat3_m4_v3(const float M[4][4], float r[3])
874{
875 const float x = r[0];
876 const float y = r[1];
877
878 r[0] = x * M[0][0] + y * M[0][1] + M[0][2] * r[2];
879 r[1] = x * M[1][0] + y * M[1][1] + M[1][2] * r[2];
880 r[2] = x * M[2][0] + y * M[2][1] + M[2][2] * r[2];
881}
882
883void mul_m3_fl(float R[3][3], float f)
884{
885 int i, j;
886
887 for (i = 0; i < 3; i++) {
888 for (j = 0; j < 3; j++) {
889 R[i][j] *= f;
890 }
891 }
892}
893
894void mul_m4_fl(float R[4][4], float f)
895{
896 int i, j;
897
898 for (i = 0; i < 4; i++) {
899 for (j = 0; j < 4; j++) {
900 R[i][j] *= f;
901 }
902 }
903}
904
905void mul_mat3_m4_fl(float R[4][4], float f)
906{
907 int i, j;
908
909 for (i = 0; i < 3; i++) {
910 for (j = 0; j < 3; j++) {
911 R[i][j] *= f;
912 }
913 }
914}
915
916void negate_m3(float R[3][3])
917{
918 int i, j;
919
920 for (i = 0; i < 3; i++) {
921 for (j = 0; j < 3; j++) {
922 R[i][j] *= -1.0f;
923 }
924 }
925}
926
927void negate_mat3_m4(float R[4][4])
928{
929 int i, j;
930
931 for (i = 0; i < 3; i++) {
932 for (j = 0; j < 3; j++) {
933 R[i][j] *= -1.0f;
934 }
935 }
936}
937
938void negate_m4(float R[4][4])
939{
940 int i, j;
941
942 for (i = 0; i < 4; i++) {
943 for (j = 0; j < 4; j++) {
944 R[i][j] *= -1.0f;
945 }
946 }
947}
948
949void add_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
950{
951 int i, j;
952
953 for (i = 0; i < 3; i++) {
954 for (j = 0; j < 3; j++) {
955 R[i][j] = A[i][j] + B[i][j];
956 }
957 }
958}
959
960void add_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
961{
962 int i, j;
963
964 for (i = 0; i < 4; i++) {
965 for (j = 0; j < 4; j++) {
966 R[i][j] = A[i][j] + B[i][j];
967 }
968 }
969}
970
971void madd_m3_m3m3fl(float R[3][3], const float A[3][3], const float B[3][3], const float f)
972{
973 int i, j;
974
975 for (i = 0; i < 3; i++) {
976 for (j = 0; j < 3; j++) {
977 R[i][j] = A[i][j] + B[i][j] * f;
978 }
979 }
980}
981
982void madd_m4_m4m4fl(float R[4][4], const float A[4][4], const float B[4][4], const float f)
983{
984 int i, j;
985
986 for (i = 0; i < 4; i++) {
987 for (j = 0; j < 4; j++) {
988 R[i][j] = A[i][j] + B[i][j] * f;
989 }
990 }
991}
992
993void sub_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
994{
995 int i, j;
996
997 for (i = 0; i < 3; i++) {
998 for (j = 0; j < 3; j++) {
999 R[i][j] = A[i][j] - B[i][j];
1000 }
1001 }
1002}
1003
1004float determinant_m3_array(const float m[3][3])
1005{
1006 return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
1007 m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
1008 m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
1009}
1010
1011float determinant_m4_mat3_array(const float m[4][4])
1012{
1013 return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
1014 m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
1015 m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
1016}
1017
1018bool invert_m2_m2(float inverse[2][2], const float mat[2][2])
1019{
1020 const float det = determinant_m2(mat[0][0], mat[1][0], mat[0][1], mat[1][1]);
1021 adjoint_m2_m2(inverse, mat);
1022
1023 bool success = (det != 0.0f);
1024 if (success) {
1025 inverse[0][0] /= det;
1026 inverse[1][0] /= det;
1027 inverse[0][1] /= det;
1028 inverse[1][1] /= det;
1029 }
1030
1031 return success;
1032}
1033
1034bool invert_m3(float mat[3][3])
1035{
1036 float mat_tmp[3][3];
1037 const bool success = invert_m3_m3(mat_tmp, mat);
1038
1039 copy_m3_m3(mat, mat_tmp);
1040 return success;
1041}
1042
1043bool invert_m3_m3(float inverse[3][3], const float mat[3][3])
1044{
1045 float det;
1046 int a, b;
1047 bool success;
1048
1049 /* calc adjoint */
1050 adjoint_m3_m3(inverse, mat);
1051
1052 /* then determinant old matrix! */
1053 det = determinant_m3_array(mat);
1054
1055 success = (det != 0.0f);
1056
1057 if (det != 0.0f) [[likely]] {
1058 det = 1.0f / det;
1059 for (a = 0; a < 3; a++) {
1060 for (b = 0; b < 3; b++) {
1061 inverse[a][b] *= det;
1062 }
1063 }
1064 }
1065
1066 return success;
1067}
1068
1069bool invert_m4(float mat[4][4])
1070{
1071 float mat_tmp[4][4];
1072 const bool success = invert_m4_m4(mat_tmp, mat);
1073
1074 copy_m4_m4(mat, mat_tmp);
1075 return success;
1076}
1077
1078bool invert_m4_m4_fallback(float inverse[4][4], const float mat[4][4])
1079{
1080#ifndef MATH_STANDALONE
1081 if (EIG_invert_m4_m4(inverse, mat)) {
1082 return true;
1083 }
1084#endif
1085
1086 int i, j, k;
1087 double temp;
1088 float tempmat[4][4];
1089 float max;
1090 int maxj;
1091
1092 BLI_assert(inverse != mat);
1093
1094 /* Set inverse to identity */
1095 for (i = 0; i < 4; i++) {
1096 for (j = 0; j < 4; j++) {
1097 inverse[i][j] = 0;
1098 }
1099 }
1100 for (i = 0; i < 4; i++) {
1101 inverse[i][i] = 1;
1102 }
1103
1104 /* Copy original matrix so we don't mess it up */
1105 for (i = 0; i < 4; i++) {
1106 for (j = 0; j < 4; j++) {
1107 tempmat[i][j] = mat[i][j];
1108 }
1109 }
1110
1111 for (i = 0; i < 4; i++) {
1112 /* Look for row with max pivot */
1113 max = fabsf(tempmat[i][i]);
1114 maxj = i;
1115 for (j = i + 1; j < 4; j++) {
1116 if (fabsf(tempmat[j][i]) > max) {
1117 max = fabsf(tempmat[j][i]);
1118 maxj = j;
1119 }
1120 }
1121 /* Swap rows if necessary */
1122 if (maxj != i) {
1123 for (k = 0; k < 4; k++) {
1124 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
1125 SWAP(float, inverse[i][k], inverse[maxj][k]);
1126 }
1127 }
1128
1129 if (tempmat[i][i] == 0.0f) [[unlikely]] {
1130 return false; /* No non-zero pivot */
1131 }
1132 temp = double(tempmat[i][i]);
1133 for (k = 0; k < 4; k++) {
1134 tempmat[i][k] = float(double(tempmat[i][k]) / temp);
1135 inverse[i][k] = float(double(inverse[i][k]) / temp);
1136 }
1137 for (j = 0; j < 4; j++) {
1138 if (j != i) {
1139 temp = tempmat[j][i];
1140 for (k = 0; k < 4; k++) {
1141 tempmat[j][k] -= float(double(tempmat[i][k]) * temp);
1142 inverse[j][k] -= float(double(inverse[i][k]) * temp);
1143 }
1144 }
1145 }
1146 }
1147 return true;
1148}
1149
1150bool invert_m4_m4(float inverse[4][4], const float mat[4][4])
1151{
1152#ifndef MATH_STANDALONE
1153 /* Use optimized matrix inverse from Eigen, since performance
1154 * impact of this function is significant in complex rigs. */
1155 return EIG_invert_m4_m4(inverse, mat);
1156#else
1157 return invert_m4_m4_fallback(inverse, mat);
1158#endif
1159}
1160
1161void mul_m4_m4m4_aligned_scale(float R[4][4], const float A[4][4], const float B[4][4])
1162{
1163 float loc_a[3], rot_a[3][3], size_a[3];
1164 float loc_b[3], rot_b[3][3], size_b[3];
1165 float loc_r[3], rot_r[3][3], size_r[3];
1166
1167 mat4_to_loc_rot_size(loc_a, rot_a, size_a, A);
1168 mat4_to_loc_rot_size(loc_b, rot_b, size_b, B);
1169
1170 mul_v3_m4v3(loc_r, A, loc_b);
1171 mul_m3_m3m3(rot_r, rot_a, rot_b);
1172 mul_v3_v3v3(size_r, size_a, size_b);
1173
1174 loc_rot_size_to_mat4(R, loc_r, rot_r, size_r);
1175}
1176
1177void mul_m4_m4m4_split_channels(float R[4][4], const float A[4][4], const float B[4][4])
1178{
1179 float loc_a[3], rot_a[3][3], size_a[3];
1180 float loc_b[3], rot_b[3][3], size_b[3];
1181 float loc_r[3], rot_r[3][3], size_r[3];
1182
1183 mat4_to_loc_rot_size(loc_a, rot_a, size_a, A);
1184 mat4_to_loc_rot_size(loc_b, rot_b, size_b, B);
1185
1186 add_v3_v3v3(loc_r, loc_a, loc_b);
1187 mul_m3_m3m3(rot_r, rot_a, rot_b);
1188 mul_v3_v3v3(size_r, size_a, size_b);
1189
1190 loc_rot_size_to_mat4(R, loc_r, rot_r, size_r);
1191}
1192
1194
1195/* -------------------------------------------------------------------- */
1198
1199void transpose_m3(float R[3][3])
1200{
1201 float t;
1202
1203 t = R[0][1];
1204 R[0][1] = R[1][0];
1205 R[1][0] = t;
1206 t = R[0][2];
1207 R[0][2] = R[2][0];
1208 R[2][0] = t;
1209 t = R[1][2];
1210 R[1][2] = R[2][1];
1211 R[2][1] = t;
1212}
1213
1214void transpose_m3_m3(float R[3][3], const float M[3][3])
1215{
1216 BLI_assert(R != M);
1217
1218 R[0][0] = M[0][0];
1219 R[0][1] = M[1][0];
1220 R[0][2] = M[2][0];
1221 R[1][0] = M[0][1];
1222 R[1][1] = M[1][1];
1223 R[1][2] = M[2][1];
1224 R[2][0] = M[0][2];
1225 R[2][1] = M[1][2];
1226 R[2][2] = M[2][2];
1227}
1228
1229void transpose_m3_m4(float R[3][3], const float M[4][4])
1230{
1231 BLI_assert(&R[0][0] != &M[0][0]);
1232
1233 R[0][0] = M[0][0];
1234 R[0][1] = M[1][0];
1235 R[0][2] = M[2][0];
1236 R[1][0] = M[0][1];
1237 R[1][1] = M[1][1];
1238 R[1][2] = M[2][1];
1239 R[2][0] = M[0][2];
1240 R[2][1] = M[1][2];
1241 R[2][2] = M[2][2];
1242}
1243
1244void transpose_m4(float R[4][4])
1245{
1246 float t;
1247
1248 t = R[0][1];
1249 R[0][1] = R[1][0];
1250 R[1][0] = t;
1251 t = R[0][2];
1252 R[0][2] = R[2][0];
1253 R[2][0] = t;
1254 t = R[0][3];
1255 R[0][3] = R[3][0];
1256 R[3][0] = t;
1257
1258 t = R[1][2];
1259 R[1][2] = R[2][1];
1260 R[2][1] = t;
1261 t = R[1][3];
1262 R[1][3] = R[3][1];
1263 R[3][1] = t;
1264
1265 t = R[2][3];
1266 R[2][3] = R[3][2];
1267 R[3][2] = t;
1268}
1269
1270void transpose_m4_m4(float R[4][4], const float M[4][4])
1271{
1272 BLI_assert(R != M);
1273
1274 R[0][0] = M[0][0];
1275 R[0][1] = M[1][0];
1276 R[0][2] = M[2][0];
1277 R[0][3] = M[3][0];
1278 R[1][0] = M[0][1];
1279 R[1][1] = M[1][1];
1280 R[1][2] = M[2][1];
1281 R[1][3] = M[3][1];
1282 R[2][0] = M[0][2];
1283 R[2][1] = M[1][2];
1284 R[2][2] = M[2][2];
1285 R[2][3] = M[3][2];
1286 R[3][0] = M[0][3];
1287 R[3][1] = M[1][3];
1288 R[3][2] = M[2][3];
1289 R[3][3] = M[3][3];
1290}
1291
1292bool compare_m4m4(const float mat1[4][4], const float mat2[4][4], float limit)
1293{
1294 if (compare_v4v4(mat1[0], mat2[0], limit)) {
1295 if (compare_v4v4(mat1[1], mat2[1], limit)) {
1296 if (compare_v4v4(mat1[2], mat2[2], limit)) {
1297 if (compare_v4v4(mat1[3], mat2[3], limit)) {
1298 return true;
1299 }
1300 }
1301 }
1302 }
1303 return false;
1304}
1305
1306void orthogonalize_m3(float R[3][3], int axis)
1307{
1308 float size[3];
1309 mat3_to_size(size, R);
1310 normalize_v3(R[axis]);
1311 switch (axis) {
1312 case 0:
1313 if (dot_v3v3(R[0], R[1]) < 1) {
1314 cross_v3_v3v3(R[2], R[0], R[1]);
1315 normalize_v3(R[2]);
1316 cross_v3_v3v3(R[1], R[2], R[0]);
1317 }
1318 else if (dot_v3v3(R[0], R[2]) < 1) {
1319 cross_v3_v3v3(R[1], R[2], R[0]);
1320 normalize_v3(R[1]);
1321 cross_v3_v3v3(R[2], R[0], R[1]);
1322 }
1323 else {
1324 float vec[3];
1325
1326 vec[0] = R[0][1];
1327 vec[1] = R[0][2];
1328 vec[2] = R[0][0];
1329
1330 cross_v3_v3v3(R[2], R[0], vec);
1331 normalize_v3(R[2]);
1332 cross_v3_v3v3(R[1], R[2], R[0]);
1333 }
1334 break;
1335 case 1:
1336 if (dot_v3v3(R[1], R[0]) < 1) {
1337 cross_v3_v3v3(R[2], R[0], R[1]);
1338 normalize_v3(R[2]);
1339 cross_v3_v3v3(R[0], R[1], R[2]);
1340 }
1341 else if (dot_v3v3(R[0], R[2]) < 1) {
1342 cross_v3_v3v3(R[0], R[1], R[2]);
1343 normalize_v3(R[0]);
1344 cross_v3_v3v3(R[2], R[0], R[1]);
1345 }
1346 else {
1347 float vec[3];
1348
1349 vec[0] = R[1][1];
1350 vec[1] = R[1][2];
1351 vec[2] = R[1][0];
1352
1353 cross_v3_v3v3(R[0], R[1], vec);
1354 normalize_v3(R[0]);
1355 cross_v3_v3v3(R[2], R[0], R[1]);
1356 }
1357 break;
1358 case 2:
1359 if (dot_v3v3(R[2], R[0]) < 1) {
1360 cross_v3_v3v3(R[1], R[2], R[0]);
1361 normalize_v3(R[1]);
1362 cross_v3_v3v3(R[0], R[1], R[2]);
1363 }
1364 else if (dot_v3v3(R[2], R[1]) < 1) {
1365 cross_v3_v3v3(R[0], R[1], R[2]);
1366 normalize_v3(R[0]);
1367 cross_v3_v3v3(R[1], R[2], R[0]);
1368 }
1369 else {
1370 float vec[3];
1371
1372 vec[0] = R[2][1];
1373 vec[1] = R[2][2];
1374 vec[2] = R[2][0];
1375
1376 cross_v3_v3v3(R[0], vec, R[2]);
1377 normalize_v3(R[0]);
1378 cross_v3_v3v3(R[1], R[2], R[0]);
1379 }
1380 break;
1381 default:
1383 break;
1384 }
1385 mul_v3_fl(R[0], size[0]);
1386 mul_v3_fl(R[1], size[1]);
1387 mul_v3_fl(R[2], size[2]);
1388}
1389
1390void orthogonalize_m4(float R[4][4], int axis)
1391{
1392 float size[3];
1393 mat4_to_size(size, R);
1394 normalize_v3(R[axis]);
1395 switch (axis) {
1396 case 0:
1397 if (dot_v3v3(R[0], R[1]) < 1) {
1398 cross_v3_v3v3(R[2], R[0], R[1]);
1399 normalize_v3(R[2]);
1400 cross_v3_v3v3(R[1], R[2], R[0]);
1401 }
1402 else if (dot_v3v3(R[0], R[2]) < 1) {
1403 cross_v3_v3v3(R[1], R[2], R[0]);
1404 normalize_v3(R[1]);
1405 cross_v3_v3v3(R[2], R[0], R[1]);
1406 }
1407 else {
1408 float vec[3];
1409
1410 vec[0] = R[0][1];
1411 vec[1] = R[0][2];
1412 vec[2] = R[0][0];
1413
1414 cross_v3_v3v3(R[2], R[0], vec);
1415 normalize_v3(R[2]);
1416 cross_v3_v3v3(R[1], R[2], R[0]);
1417 }
1418 break;
1419 case 1:
1420 if (dot_v3v3(R[1], R[0]) < 1) {
1421 cross_v3_v3v3(R[2], R[0], R[1]);
1422 normalize_v3(R[2]);
1423 cross_v3_v3v3(R[0], R[1], R[2]);
1424 }
1425 else if (dot_v3v3(R[0], R[2]) < 1) {
1426 cross_v3_v3v3(R[0], R[1], R[2]);
1427 normalize_v3(R[0]);
1428 cross_v3_v3v3(R[2], R[0], R[1]);
1429 }
1430 else {
1431 float vec[3];
1432
1433 vec[0] = R[1][1];
1434 vec[1] = R[1][2];
1435 vec[2] = R[1][0];
1436
1437 cross_v3_v3v3(R[0], R[1], vec);
1438 normalize_v3(R[0]);
1439 cross_v3_v3v3(R[2], R[0], R[1]);
1440 }
1441 break;
1442 case 2:
1443 if (dot_v3v3(R[2], R[0]) < 1) {
1444 cross_v3_v3v3(R[1], R[2], R[0]);
1445 normalize_v3(R[1]);
1446 cross_v3_v3v3(R[0], R[1], R[2]);
1447 }
1448 else if (dot_v3v3(R[2], R[1]) < 1) {
1449 cross_v3_v3v3(R[0], R[1], R[2]);
1450 normalize_v3(R[0]);
1451 cross_v3_v3v3(R[1], R[2], R[0]);
1452 }
1453 else {
1454 float vec[3];
1455
1456 vec[0] = R[2][1];
1457 vec[1] = R[2][2];
1458 vec[2] = R[2][0];
1459
1460 cross_v3_v3v3(R[0], vec, R[2]);
1461 normalize_v3(R[0]);
1462 cross_v3_v3v3(R[1], R[2], R[0]);
1463 }
1464 break;
1465 default:
1467 break;
1468 }
1469 mul_v3_fl(R[0], size[0]);
1470 mul_v3_fl(R[1], size[1]);
1471 mul_v3_fl(R[2], size[2]);
1472}
1473
1475static void orthogonalize_stable(float v1[3], float v2[3], float v3[3], bool normalize)
1476{
1477 /* Make secondary axis vectors orthogonal to the primary via
1478 * plane projection, which preserves the determinant. */
1479 float len_sq_v1 = len_squared_v3(v1);
1480
1481 if (len_sq_v1 > 0.0f) {
1482 madd_v3_v3fl(v2, v1, -dot_v3v3(v2, v1) / len_sq_v1);
1483 madd_v3_v3fl(v3, v1, -dot_v3v3(v3, v1) / len_sq_v1);
1484
1485 if (normalize) {
1486 mul_v3_fl(v1, 1.0f / sqrtf(len_sq_v1));
1487 }
1488 }
1489
1490 /* Make secondary axis vectors orthogonal relative to each other. */
1491 float norm_v2[3], norm_v3[3], tmp[3];
1492 float length_v2 = normalize_v3_v3(norm_v2, v2);
1493 float length_v3 = normalize_v3_v3(norm_v3, v3);
1494 float cos_angle = dot_v3v3(norm_v2, norm_v3);
1495 float abs_cos_angle = fabsf(cos_angle);
1496
1497 /* Apply correction if the shear angle is significant, and not degenerate. */
1498 if (abs_cos_angle > 1e-4f && abs_cos_angle < 1.0f - FLT_EPSILON) {
1499 /* Adjust v2 by half of the necessary angle correction.
1500 * Thus the angle change is the same for both axis directions. */
1501 float angle = acosf(cos_angle);
1502 float target_angle = angle + (float(M_PI_2) - angle) / 2;
1503
1504 madd_v3_v3fl(norm_v2, norm_v3, -cos_angle);
1505 mul_v3_fl(norm_v2, sinf(target_angle) / len_v3(norm_v2));
1506 madd_v3_v3fl(norm_v2, norm_v3, cosf(target_angle));
1507
1508 /* Make v3 orthogonal. */
1509 cross_v3_v3v3(tmp, norm_v2, norm_v3);
1510 cross_v3_v3v3(norm_v3, tmp, norm_v2);
1511 normalize_v3(norm_v3);
1512
1513 /* Re-apply scale, preserving area and proportion. */
1514 if (!normalize) {
1515 float scale_fac = sqrtf(sinf(angle));
1516 mul_v3_v3fl(v2, norm_v2, length_v2 * scale_fac);
1517 mul_v3_v3fl(v3, norm_v3, length_v3 * scale_fac);
1518 }
1519 }
1520
1521 if (normalize) {
1522 copy_v3_v3(v2, norm_v2);
1523 copy_v3_v3(v3, norm_v3);
1524 }
1525}
1526
1527void orthogonalize_m4_stable(float R[4][4], int axis, bool normalize)
1528{
1529 switch (axis) {
1530 case 0:
1531 orthogonalize_stable(R[0], R[1], R[2], normalize);
1532 break;
1533 case 1:
1534 orthogonalize_stable(R[1], R[0], R[2], normalize);
1535 break;
1536 case 2:
1537 orthogonalize_stable(R[2], R[0], R[1], normalize);
1538 break;
1539 default:
1541 break;
1542 }
1543}
1544
1546
1547/* -------------------------------------------------------------------- */
1558
1562static bool orthogonalize_m3_zero_axes_impl(float *mat[3], const float unit_length)
1563{
1564 enum { X = 1 << 0, Y = 1 << 1, Z = 1 << 2 };
1565 int flag = 0;
1566 for (int i = 0; i < 3; i++) {
1567 flag |= (len_squared_v3(mat[i]) == 0.0f) ? (1 << i) : 0;
1568 }
1569
1570 /* Either all or none are zero, either way we can't properly resolve this
1571 * since we need to fill invalid axes from valid ones. */
1572 if (ELEM(flag, 0, X | Y | Z)) {
1573 return false;
1574 }
1575
1576 switch (flag) {
1577 case X | Y: {
1578 ortho_v3_v3(mat[1], mat[2]);
1580 }
1581 case X: {
1582 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1583 break;
1584 }
1585
1586 case Y | Z: {
1587 ortho_v3_v3(mat[2], mat[0]);
1589 }
1590 case Y: {
1591 cross_v3_v3v3(mat[1], mat[0], mat[2]);
1592 break;
1593 }
1594
1595 case Z | X: {
1596 ortho_v3_v3(mat[0], mat[1]);
1598 }
1599 case Z: {
1600 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1601 break;
1602 }
1603 default: {
1605 }
1606 }
1607
1608 for (int i = 0; i < 3; i++) {
1609 if (flag & (1 << i)) {
1610 if (normalize_v3_length(mat[i], unit_length) == 0.0f) [[unlikely]] {
1611 mat[i][i] = unit_length;
1612 }
1613 }
1614 }
1615
1616 return true;
1617}
1618
1619bool orthogonalize_m3_zero_axes(float m[3][3], const float unit_length)
1620{
1621 float *unpacked[3] = {m[0], m[1], m[2]};
1622 return orthogonalize_m3_zero_axes_impl(unpacked, unit_length);
1623}
1624bool orthogonalize_m4_zero_axes(float m[4][4], const float unit_length)
1625{
1626 float *unpacked[3] = {m[0], m[1], m[2]};
1627 return orthogonalize_m3_zero_axes_impl(unpacked, unit_length);
1628}
1629
1631
1632/* -------------------------------------------------------------------- */
1635
1636bool is_orthogonal_m3(const float m[3][3])
1637{
1638 int i, j;
1639
1640 for (i = 0; i < 3; i++) {
1641 for (j = 0; j < i; j++) {
1642 if (fabsf(dot_v3v3(m[i], m[j])) > 1e-5f) {
1643 return false;
1644 }
1645 }
1646 }
1647
1648 return true;
1649}
1650
1651bool is_orthogonal_m4(const float m[4][4])
1652{
1653 int i, j;
1654
1655 for (i = 0; i < 4; i++) {
1656 for (j = 0; j < i; j++) {
1657 if (fabsf(dot_v4v4(m[i], m[j])) > 1e-5f) {
1658 return false;
1659 }
1660 }
1661 }
1662
1663 return true;
1664}
1665
1666bool is_orthonormal_m3(const float m[3][3])
1667{
1668 if (is_orthogonal_m3(m)) {
1669 int i;
1670
1671 for (i = 0; i < 3; i++) {
1672 if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1e-5f) {
1673 return false;
1674 }
1675 }
1676
1677 return true;
1678 }
1679
1680 return false;
1681}
1682
1683bool is_orthonormal_m4(const float m[4][4])
1684{
1685 if (is_orthogonal_m4(m)) {
1686 int i;
1687
1688 for (i = 0; i < 4; i++) {
1689 if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1e-5f) {
1690 return false;
1691 }
1692 }
1693
1694 return true;
1695 }
1696
1697 return false;
1698}
1699
1700bool is_identity_m4(const float m[4][4])
1701{
1702 for (int row = 0; row < 4; row++) {
1703 for (int col = 0; col < 4; col++) {
1704 if (m[row][col] != (row == col ? 1.0f : 0.0f)) {
1705 return false;
1706 }
1707 }
1708 }
1709
1710 return true;
1711}
1712
1713bool is_uniform_scaled_m3(const float m[3][3])
1714{
1715 const float eps = 1e-7f;
1716 float t[3][3];
1717 float l1, l2, l3, l4, l5, l6;
1718
1719 transpose_m3_m3(t, m);
1720
1721 l1 = len_squared_v3(m[0]);
1722 l2 = len_squared_v3(m[1]);
1723 l3 = len_squared_v3(m[2]);
1724
1725 l4 = len_squared_v3(t[0]);
1726 l5 = len_squared_v3(t[1]);
1727 l6 = len_squared_v3(t[2]);
1728
1729 if (fabsf(l2 - l1) <= eps && fabsf(l3 - l1) <= eps && fabsf(l4 - l1) <= eps &&
1730 fabsf(l5 - l1) <= eps && fabsf(l6 - l1) <= eps)
1731 {
1732 return true;
1733 }
1734
1735 return false;
1736}
1737
1738bool is_uniform_scaled_m4(const float m[4][4])
1739{
1740 float t[3][3];
1741 copy_m3_m4(t, m);
1742 return is_uniform_scaled_m3(t);
1743}
1744
1745void normalize_m2_m2(float R[2][2], const float M[2][2])
1746{
1747 int i;
1748 for (i = 0; i < 2; i++) {
1749 normalize_v2_v2(R[i], M[i]);
1750 }
1751}
1752
1753void normalize_m3(float R[3][3])
1754{
1755 int i;
1756 for (i = 0; i < 3; i++) {
1757 normalize_v3(R[i]);
1758 }
1759}
1760
1761void normalize_m3_m3(float R[3][3], const float M[3][3])
1762{
1763 int i;
1764 for (i = 0; i < 3; i++) {
1765 normalize_v3_v3(R[i], M[i]);
1766 }
1767}
1768
1769void normalize_m4_ex(float R[4][4], float r_scale[3])
1770{
1771 int i;
1772 for (i = 0; i < 3; i++) {
1773 r_scale[i] = normalize_v3(R[i]);
1774 if (r_scale[i] != 0.0f) {
1775 R[i][3] /= r_scale[i];
1776 }
1777 }
1778}
1779void normalize_m4(float R[4][4])
1780{
1781 int i;
1782 for (i = 0; i < 3; i++) {
1783 float len = normalize_v3(R[i]);
1784 if (len != 0.0f) {
1785 R[i][3] /= len;
1786 }
1787 }
1788}
1789
1790void normalize_m4_m4(float rmat[4][4], const float mat[4][4])
1791{
1792 int i;
1793 for (i = 0; i < 3; i++) {
1794 float len = normalize_v3_v3(rmat[i], mat[i]);
1795 rmat[i][3] = (len != 0.0f) ? (mat[i][3] / len) : mat[i][3];
1796 }
1797 copy_v4_v4(rmat[3], mat[3]);
1798}
1799
1800void adjoint_m2_m2(float R[2][2], const float M[2][2])
1801{
1802 const float r00 = M[1][1];
1803 const float r01 = -M[0][1];
1804 const float r10 = -M[1][0];
1805 const float r11 = M[0][0];
1806
1807 R[0][0] = r00;
1808 R[0][1] = r01;
1809 R[1][0] = r10;
1810 R[1][1] = r11;
1811}
1812
1813void adjoint_m3_m3(float R[3][3], const float M[3][3])
1814{
1815 const float m00 = M[0][0], m01 = M[0][1], m02 = M[0][2];
1816 const float m10 = M[1][0], m11 = M[1][1], m12 = M[1][2];
1817 const float m20 = M[2][0], m21 = M[2][1], m22 = M[2][2];
1818
1819 R[0][0] = m11 * m22 - m12 * m21;
1820 R[0][1] = -m01 * m22 + m02 * m21;
1821 R[0][2] = m01 * m12 - m02 * m11;
1822
1823 R[1][0] = -m10 * m22 + m12 * m20;
1824 R[1][1] = m00 * m22 - m02 * m20;
1825 R[1][2] = -m00 * m12 + m02 * m10;
1826
1827 R[2][0] = m10 * m21 - m11 * m20;
1828 R[2][1] = -m00 * m21 + m01 * m20;
1829 R[2][2] = m00 * m11 - m01 * m10;
1830}
1831
1832void adjoint_m4_m4(float R[4][4], const float M[4][4]) /* out = ADJ(in) */
1833{
1834 float a1, a2, a3, a4, b1, b2, b3, b4;
1835 float c1, c2, c3, c4, d1, d2, d3, d4;
1836
1837 a1 = M[0][0];
1838 b1 = M[0][1];
1839 c1 = M[0][2];
1840 d1 = M[0][3];
1841
1842 a2 = M[1][0];
1843 b2 = M[1][1];
1844 c2 = M[1][2];
1845 d2 = M[1][3];
1846
1847 a3 = M[2][0];
1848 b3 = M[2][1];
1849 c3 = M[2][2];
1850 d3 = M[2][3];
1851
1852 a4 = M[3][0];
1853 b4 = M[3][1];
1854 c4 = M[3][2];
1855 d4 = M[3][3];
1856
1857 R[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1858 R[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1859 R[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1860 R[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1861
1862 R[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1863 R[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1864 R[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1865 R[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1866
1867 R[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1868 R[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1869 R[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1870 R[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1871
1872 R[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1873 R[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1874 R[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1875 R[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1876}
1877
1878float determinant_m2(const float a, const float b, const float c, const float d)
1879{
1880 return a * d - b * c;
1881}
1882
1884 float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3)
1885{
1886 float ans;
1887
1888 ans = (a1 * determinant_m2(b2, b3, c2, c3) - b1 * determinant_m2(a2, a3, c2, c3) +
1889 c1 * determinant_m2(a2, a3, b2, b3));
1890
1891 return ans;
1892}
1893
1894float determinant_m4(const float m[4][4])
1895{
1896 float ans;
1897 float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1898
1899 a1 = m[0][0];
1900 b1 = m[0][1];
1901 c1 = m[0][2];
1902 d1 = m[0][3];
1903
1904 a2 = m[1][0];
1905 b2 = m[1][1];
1906 c2 = m[1][2];
1907 d2 = m[1][3];
1908
1909 a3 = m[2][0];
1910 b3 = m[2][1];
1911 c3 = m[2][2];
1912 d3 = m[2][3];
1913
1914 a4 = m[3][0];
1915 b4 = m[3][1];
1916 c4 = m[3][2];
1917 d4 = m[3][3];
1918
1919 ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1920 b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1921 c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1922 d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1923
1924 return ans;
1925}
1926
1928
1929/* -------------------------------------------------------------------- */
1932
1933void size_to_mat3(float R[3][3], const float size[3])
1934{
1935 R[0][0] = size[0];
1936 R[0][1] = 0.0f;
1937 R[0][2] = 0.0f;
1938 R[1][1] = size[1];
1939 R[1][0] = 0.0f;
1940 R[1][2] = 0.0f;
1941 R[2][2] = size[2];
1942 R[2][1] = 0.0f;
1943 R[2][0] = 0.0f;
1944}
1945
1946void size_to_mat4(float R[4][4], const float size[3])
1947{
1948 R[0][0] = size[0];
1949 R[0][1] = 0.0f;
1950 R[0][2] = 0.0f;
1951 R[0][3] = 0.0f;
1952 R[1][0] = 0.0f;
1953 R[1][1] = size[1];
1954 R[1][2] = 0.0f;
1955 R[1][3] = 0.0f;
1956 R[2][0] = 0.0f;
1957 R[2][1] = 0.0f;
1958 R[2][2] = size[2];
1959 R[2][3] = 0.0f;
1960 R[3][0] = 0.0f;
1961 R[3][1] = 0.0f;
1962 R[3][2] = 0.0f;
1963 R[3][3] = 1.0f;
1964}
1965
1966void mat3_to_size(float size[3], const float M[3][3])
1967{
1968 size[0] = len_v3(M[0]);
1969 size[1] = len_v3(M[1]);
1970 size[2] = len_v3(M[2]);
1971}
1972
1973void mat4_to_size(float size[3], const float M[4][4])
1974{
1975 size[0] = len_v3(M[0]);
1976 size[1] = len_v3(M[1]);
1977 size[2] = len_v3(M[2]);
1978}
1979
1980float mat4_to_size_max_axis(const float M[4][4])
1981{
1982 return sqrtf(std::max({len_squared_v3(M[0]), len_squared_v3(M[1]), len_squared_v3(M[2])}));
1983}
1984
1985void mat4_to_size_fix_shear(float size[3], const float M[4][4])
1986{
1987 mat4_to_size(size, M);
1988
1989 float volume = size[0] * size[1] * size[2];
1990
1991 if (volume != 0.0f) {
1992 mul_v3_fl(size, cbrtf(fabsf(mat4_to_volume_scale(M) / volume)));
1993 }
1994}
1995
1996float mat4_to_volume_scale(const float mat[4][4])
1997{
1998 return determinant_m4_mat3_array(mat);
1999}
2000
2001float mat3_to_scale(const float mat[3][3])
2002{
2003 /* unit length vector */
2004 float unit_vec[3];
2005 copy_v3_fl(unit_vec, float(M_SQRT1_3));
2006 mul_m3_v3(mat, unit_vec);
2007 return len_v3(unit_vec);
2008}
2009
2010float mat4_to_scale(const float mat[4][4])
2011{
2012 /* unit length vector */
2013 float unit_vec[3];
2014 copy_v3_fl(unit_vec, float(M_SQRT1_3));
2015 mul_mat3_m4_v3(mat, unit_vec);
2016 return len_v3(unit_vec);
2017}
2018
2019void mat3_to_rot_size(float rot[3][3], float size[3], const float mat3[3][3])
2020{
2021 /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
2022 size[0] = normalize_v3_v3(rot[0], mat3[0]);
2023 size[1] = normalize_v3_v3(rot[1], mat3[1]);
2024 size[2] = normalize_v3_v3(rot[2], mat3[2]);
2025 if (is_negative_m3(rot)) [[unlikely]] {
2026 negate_m3(rot);
2027 negate_v3(size);
2028 }
2029}
2030
2031void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], const float wmat[4][4])
2032{
2033 float mat3[3][3]; /* wmat -> 3x3 */
2034
2035 copy_m3_m4(mat3, wmat);
2036 mat3_to_rot_size(rot, size, mat3);
2037
2038 /* location */
2039 copy_v3_v3(loc, wmat[3]);
2040}
2041
2042void mat4_to_loc_quat(float loc[3], float quat[4], const float wmat[4][4])
2043{
2044 float mat3[3][3];
2045 float mat3_n[3][3]; /* normalized mat3 */
2046
2047 copy_m3_m4(mat3, wmat);
2048 normalize_m3_m3(mat3_n, mat3);
2049
2050 mat3_normalized_to_quat(quat, mat3_n);
2051 copy_v3_v3(loc, wmat[3]);
2052}
2053
2054void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat[4][4])
2055{
2056 float rot[3][3];
2057 mat4_to_loc_rot_size(loc, rot, size, wmat);
2059}
2060
2070#ifndef MATH_STANDALONE
2071void mat3_polar_decompose(const float mat3[3][3], float r_U[3][3], float r_P[3][3])
2072{
2073 /* From svd decomposition (M = WSV*), we have:
2074 * U = WV*
2075 * P = VSV*
2076 */
2077 float W[3][3], S[3][3], V[3][3], Vt[3][3];
2078 float sval[3];
2079
2080 BLI_svd_m3(mat3, W, sval, V);
2081
2082 size_to_mat3(S, sval);
2083
2084 transpose_m3_m3(Vt, V);
2085 mul_m3_m3m3(r_U, W, Vt);
2086 mul_m3_series(r_P, V, S, Vt);
2087}
2088#endif
2089
2090void scale_m3_fl(float R[3][3], float scale)
2091{
2092 R[0][0] = R[1][1] = R[2][2] = scale;
2093 R[0][1] = R[0][2] = 0.0;
2094 R[1][0] = R[1][2] = 0.0;
2095 R[2][0] = R[2][1] = 0.0;
2096}
2097
2098void scale_m4_fl(float R[4][4], float scale)
2099{
2100 R[0][0] = R[1][1] = R[2][2] = scale;
2101 R[3][3] = 1.0;
2102 R[0][1] = R[0][2] = R[0][3] = 0.0;
2103 R[1][0] = R[1][2] = R[1][3] = 0.0;
2104 R[2][0] = R[2][1] = R[2][3] = 0.0;
2105 R[3][0] = R[3][1] = R[3][2] = 0.0;
2106}
2107
2108void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
2109{
2110 mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
2111 mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
2112 mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
2113}
2114
2115void rotate_m4(float mat[4][4], const char axis, const float angle)
2116{
2117 const float angle_cos = cosf(angle);
2118 const float angle_sin = sinf(angle);
2119
2120 BLI_assert(axis >= 'X' && axis <= 'Z');
2121
2122 switch (axis) {
2123 case 'X':
2124 for (int col = 0; col < 4; col++) {
2125 float temp = angle_cos * mat[1][col] + angle_sin * mat[2][col];
2126 mat[2][col] = -angle_sin * mat[1][col] + angle_cos * mat[2][col];
2127 mat[1][col] = temp;
2128 }
2129 break;
2130
2131 case 'Y':
2132 for (int col = 0; col < 4; col++) {
2133 float temp = angle_cos * mat[0][col] - angle_sin * mat[2][col];
2134 mat[2][col] = angle_sin * mat[0][col] + angle_cos * mat[2][col];
2135 mat[0][col] = temp;
2136 }
2137 break;
2138
2139 case 'Z':
2140 for (int col = 0; col < 4; col++) {
2141 float temp = angle_cos * mat[0][col] + angle_sin * mat[1][col];
2142 mat[1][col] = -angle_sin * mat[0][col] + angle_cos * mat[1][col];
2143 mat[0][col] = temp;
2144 }
2145 break;
2146 default:
2148 break;
2149 }
2150}
2151
2152void rescale_m4(float mat[4][4], const float scale[3])
2153{
2154 mul_v3_fl(mat[0], scale[0]);
2155 mul_v3_fl(mat[1], scale[1]);
2156 mul_v3_fl(mat[2], scale[2]);
2157}
2158
2159void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
2160{
2161 float tmat[4][4];
2162
2163 unit_m4(tmat);
2164
2165 copy_v3_v3(tmat[3], pivot);
2166 mul_m4_m4m4(mat, tmat, mat);
2167
2168 /* invert the matrix */
2169 negate_v3(tmat[3]);
2170 mul_m4_m4m4(mat, mat, tmat);
2171}
2172
2173void blend_m3_m3m3(float out[3][3],
2174 const float dst[3][3],
2175 const float src[3][3],
2176 const float srcweight)
2177{
2178 float srot[3][3], drot[3][3];
2179 float squat[4], dquat[4], fquat[4];
2180 float sscale[3], dscale[3], fsize[3];
2181 float rmat[3][3], smat[3][3];
2182
2183 mat3_to_rot_size(drot, dscale, dst);
2184 mat3_to_rot_size(srot, sscale, src);
2185
2186 mat3_normalized_to_quat_fast(dquat, drot);
2187 mat3_normalized_to_quat_fast(squat, srot);
2188
2189 /* do blending */
2190 interp_qt_qtqt(fquat, dquat, squat, srcweight);
2191 interp_v3_v3v3(fsize, dscale, sscale, srcweight);
2192
2193 /* compose new matrix */
2194 quat_to_mat3(rmat, fquat);
2195 size_to_mat3(smat, fsize);
2196 mul_m3_m3m3(out, rmat, smat);
2197}
2198
2199void blend_m4_m4m4(float out[4][4],
2200 const float dst[4][4],
2201 const float src[4][4],
2202 const float srcweight)
2203{
2204 float sloc[3], dloc[3], floc[3];
2205 float srot[3][3], drot[3][3];
2206 float squat[4], dquat[4], fquat[4];
2207 float sscale[3], dscale[3], fsize[3];
2208
2209 mat4_to_loc_rot_size(dloc, drot, dscale, dst);
2210 mat4_to_loc_rot_size(sloc, srot, sscale, src);
2211
2212 mat3_normalized_to_quat_fast(dquat, drot);
2213 mat3_normalized_to_quat_fast(squat, srot);
2214
2215 /* do blending */
2216 interp_v3_v3v3(floc, dloc, sloc, srcweight);
2217 interp_qt_qtqt(fquat, dquat, squat, srcweight);
2218 interp_v3_v3v3(fsize, dscale, sscale, srcweight);
2219
2220 /* compose new matrix */
2221 loc_quat_size_to_mat4(out, floc, fquat, fsize);
2222}
2223
2224/* for builds without Eigen */
2225#ifndef MATH_STANDALONE
2226void interp_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3], const float t)
2227{
2228 /* 'Rotation' component ('U' part of polar decomposition,
2229 * the closest orthogonal matrix to M3 rot/scale
2230 * transformation matrix), spherically interpolated. */
2231 float U_A[3][3], U_B[3][3], U[3][3];
2232 float quat_A[4], quat_B[4], quat[4];
2233 /* 'Scaling' component ('P' part of polar decomposition, i.e. scaling in U-defined space),
2234 * linearly interpolated. */
2235 float P_A[3][3], P_B[3][3], P[3][3];
2236
2237 int i;
2238
2239 mat3_polar_decompose(A, U_A, P_A);
2240 mat3_polar_decompose(B, U_B, P_B);
2241
2242 /* Quaternions cannot represent an axis flip. If such a singularity is detected, choose a
2243 * different decomposition of the matrix that still satisfies A = U_A * P_A but which has a
2244 * positive determinant and thus no axis flips. This resolves #77154.
2245 *
2246 * Note that a flip of two axes is just a rotation of 180 degrees around the third axis, and
2247 * three flipped axes are just an 180 degree rotation + a single axis flip. It is thus sufficient
2248 * to solve this problem for single axis flips. */
2249 if (is_negative_m3(U_A)) {
2250 mul_m3_fl(U_A, -1.0f);
2251 mul_m3_fl(P_A, -1.0f);
2252 }
2253 if (is_negative_m3(U_B)) {
2254 mul_m3_fl(U_B, -1.0f);
2255 mul_m3_fl(P_B, -1.0f);
2256 }
2257
2258 mat3_to_quat(quat_A, U_A);
2259 mat3_to_quat(quat_B, U_B);
2260 interp_qt_qtqt(quat, quat_A, quat_B, t);
2261 quat_to_mat3(U, quat);
2262
2263 for (i = 0; i < 3; i++) {
2264 interp_v3_v3v3(P[i], P_A[i], P_B[i], t);
2265 }
2266
2267 /* And we reconstruct rot/scale matrix from interpolated polar components */
2268 mul_m3_m3m3(R, U, P);
2269}
2270
2271void interp_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4], const float t)
2272{
2273 float A3[3][3], B3[3][3], R3[3][3];
2274
2275 /* Location component, linearly interpolated. */
2276 float loc_A[3], loc_B[3], loc[3];
2277
2278 copy_v3_v3(loc_A, A[3]);
2279 copy_v3_v3(loc_B, B[3]);
2280 interp_v3_v3v3(loc, loc_A, loc_B, t);
2281
2282 copy_m3_m4(A3, A);
2283 copy_m3_m4(B3, B);
2284
2285 interp_m3_m3m3(R3, A3, B3, t);
2286
2287 copy_m4_m3(R, R3);
2288 copy_v3_v3(R[3], loc);
2289}
2290#endif /* MATH_STANDALONE */
2291
2292bool is_negative_m3(const float mat[3][3])
2293{
2294 return determinant_m3_array(mat) < 0.0f;
2295}
2296
2297bool is_negative_m4(const float mat[4][4])
2298{
2299 /* Don't use #determinant_m4 as only the 3x3 components are needed
2300 * when the matrix is used as a transformation to represent location/scale/rotation. */
2301 return determinant_m4_mat3_array(mat) < 0.0f;
2302}
2303
2304bool is_zero_m4(const float mat[4][4])
2305{
2306 return (is_zero_v4(mat[0]) && is_zero_v4(mat[1]) && is_zero_v4(mat[2]) && is_zero_v4(mat[3]));
2307}
2308
2309bool equals_m3m3(const float mat1[3][3], const float mat2[3][3])
2310{
2311 return (equals_v3v3(mat1[0], mat2[0]) && equals_v3v3(mat1[1], mat2[1]) &&
2312 equals_v3v3(mat1[2], mat2[2]));
2313}
2314
2315bool equals_m4m4(const float mat1[4][4], const float mat2[4][4])
2316{
2317 return (equals_v4v4(mat1[0], mat2[0]) && equals_v4v4(mat1[1], mat2[1]) &&
2318 equals_v4v4(mat1[2], mat2[2]) && equals_v4v4(mat1[3], mat2[3]));
2319}
2320
2321void loc_rot_size_to_mat4(float R[4][4],
2322 const float loc[3],
2323 const float rot[3][3],
2324 const float size[3])
2325{
2326 copy_m4_m3(R, rot);
2327 rescale_m4(R, size);
2328 copy_v3_v3(R[3], loc);
2329}
2330
2331void loc_eul_size_to_mat4(float R[4][4],
2332 const float loc[3],
2333 const float eul[3],
2334 const float size[3])
2335{
2336 float rmat[3][3], smat[3][3], tmat[3][3];
2337
2338 /* initialize new matrix */
2339 unit_m4(R);
2340
2341 /* make rotation + scaling part */
2342 eul_to_mat3(rmat, eul);
2343 size_to_mat3(smat, size);
2344 mul_m3_m3m3(tmat, rmat, smat);
2345
2346 /* Copy rot/scale part to output matrix. */
2347 copy_m4_m3(R, tmat);
2348
2349 /* copy location to matrix */
2350 R[3][0] = loc[0];
2351 R[3][1] = loc[1];
2352 R[3][2] = loc[2];
2353}
2354
2356 float R[4][4], const float loc[3], const float eul[3], const float size[3], const short order)
2357{
2358 float rmat[3][3], smat[3][3], tmat[3][3];
2359
2360 /* Initialize new matrix. */
2361 unit_m4(R);
2362
2363 /* Make rotation + scaling part. */
2364 eulO_to_mat3(rmat, eul, order);
2365 size_to_mat3(smat, size);
2366 mul_m3_m3m3(tmat, rmat, smat);
2367
2368 /* Copy rot/scale part to output matrix. */
2369 copy_m4_m3(R, tmat);
2370
2371 /* Copy location to matrix. */
2372 R[3][0] = loc[0];
2373 R[3][1] = loc[1];
2374 R[3][2] = loc[2];
2375}
2376
2377void loc_quat_size_to_mat4(float R[4][4],
2378 const float loc[3],
2379 const float quat[4],
2380 const float size[3])
2381{
2382 float rmat[3][3], smat[3][3], tmat[3][3];
2383
2384 /* initialize new matrix */
2385 unit_m4(R);
2386
2387 /* make rotation + scaling part */
2388 quat_to_mat3(rmat, quat);
2389 size_to_mat3(smat, size);
2390 mul_m3_m3m3(tmat, rmat, smat);
2391
2392 /* Copy rot/scale part to output matrix. */
2393 copy_m4_m3(R, tmat);
2394
2395 /* copy location to matrix */
2396 R[3][0] = loc[0];
2397 R[3][1] = loc[1];
2398 R[3][2] = loc[2];
2399}
2400
2402
2403/* -------------------------------------------------------------------- */
2406
2407void print_m3(const char *str, const float m[3][3])
2408{
2409 printf("%s\n", str);
2410 printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
2411 printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
2412 printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
2413 printf("\n");
2414}
2415
2416void print_m4(const char *str, const float m[4][4])
2417{
2418 printf("%s\n", str);
2419 printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
2420 printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
2421 printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
2422 printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
2423 printf("\n");
2424}
2425
2426void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
2427{
2428 /* NOTE: originally from TNT (template numeric toolkit) matrix library.
2429 * https://math.nist.gov/tnt */
2430
2431 float A[4][4];
2432 float work1[4], work2[4];
2433 int m = 4;
2434 int n = 4;
2435 int maxiter = 200;
2436 int nu = min_ii(m, n);
2437
2438 float *work = work1;
2439 float *e = work2;
2440 float eps;
2441
2442 int i = 0, j = 0, k = 0, p, pp, iter;
2443
2444 /* Reduce A to bidiagonal form, storing the diagonal elements
2445 * in s and the super-diagonal elements in e. */
2446
2447 int nct = min_ii(m - 1, n);
2448 int nrt = max_ii(0, min_ii(n - 2, m));
2449
2450 copy_m4_m4(A, A_);
2451 zero_m4(U);
2452 zero_v4(s);
2453
2454 for (k = 0; k < max_ii(nct, nrt); k++) {
2455 if (k < nct) {
2456
2457 /* Compute the transformation for the k-th column and
2458 * place the k-th diagonal in s[k].
2459 * Compute 2-norm of k-th column without under/overflow. */
2460 s[k] = 0;
2461 for (i = k; i < m; i++) {
2462 s[k] = hypotf(s[k], A[i][k]);
2463 }
2464 if (s[k] != 0.0f) {
2465 float invsk;
2466 if (A[k][k] < 0.0f) {
2467 s[k] = -s[k];
2468 }
2469 invsk = 1.0f / s[k];
2470 for (i = k; i < m; i++) {
2471 A[i][k] *= invsk;
2472 }
2473 A[k][k] += 1.0f;
2474 }
2475 s[k] = -s[k];
2476 }
2477 for (j = k + 1; j < n; j++) {
2478 if ((k < nct) && (s[k] != 0.0f)) {
2479
2480 /* Apply the transformation. */
2481
2482 float t = 0;
2483 for (i = k; i < m; i++) {
2484 t += A[i][k] * A[i][j];
2485 }
2486 t = -t / A[k][k];
2487 for (i = k; i < m; i++) {
2488 A[i][j] += t * A[i][k];
2489 }
2490 }
2491
2492 /* Place the k-th row of A into e for the */
2493 /* subsequent calculation of the row transformation. */
2494
2495 e[j] = A[k][j];
2496 }
2497 if (k < nct) {
2498
2499 /* Place the transformation in U for subsequent back
2500 * multiplication. */
2501
2502 for (i = k; i < m; i++) {
2503 U[i][k] = A[i][k];
2504 }
2505 }
2506 if (k < nrt) {
2507
2508 /* Compute the k-th row transformation and place the
2509 * k-th super-diagonal in e[k].
2510 * Compute 2-norm without under/overflow. */
2511 e[k] = 0;
2512 for (i = k + 1; i < n; i++) {
2513 e[k] = hypotf(e[k], e[i]);
2514 }
2515 if (e[k] != 0.0f) {
2516 float invek;
2517 if (e[k + 1] < 0.0f) {
2518 e[k] = -e[k];
2519 }
2520 invek = 1.0f / e[k];
2521 for (i = k + 1; i < n; i++) {
2522 e[i] *= invek;
2523 }
2524 e[k + 1] += 1.0f;
2525 }
2526 e[k] = -e[k];
2527 if ((k + 1 < m) && (e[k] != 0.0f)) {
2528 float invek1;
2529
2530 /* Apply the transformation. */
2531
2532 for (i = k + 1; i < m; i++) {
2533 work[i] = 0.0f;
2534 }
2535 for (j = k + 1; j < n; j++) {
2536 for (i = k + 1; i < m; i++) {
2537 work[i] += e[j] * A[i][j];
2538 }
2539 }
2540 invek1 = 1.0f / e[k + 1];
2541 for (j = k + 1; j < n; j++) {
2542 float t = -e[j] * invek1;
2543 for (i = k + 1; i < m; i++) {
2544 A[i][j] += t * work[i];
2545 }
2546 }
2547 }
2548
2549 /* Place the transformation in V for subsequent
2550 * back multiplication. */
2551
2552 for (i = k + 1; i < n; i++) {
2553 V[i][k] = e[i];
2554 }
2555 }
2556 }
2557
2558 /* Set up the final bidiagonal matrix or order p. */
2559
2560 p = min_ii(n, m + 1);
2561 if (nct < n) {
2562 s[nct] = A[nct][nct];
2563 }
2564 if (m < p) {
2565 s[p - 1] = 0.0f;
2566 }
2567 if (nrt + 1 < p) {
2568 e[nrt] = A[nrt][p - 1];
2569 }
2570 e[p - 1] = 0.0f;
2571
2572 /* If required, generate U. */
2573
2574 for (j = nct; j < nu; j++) {
2575 for (i = 0; i < m; i++) {
2576 U[i][j] = 0.0f;
2577 }
2578 U[j][j] = 1.0f;
2579 }
2580 for (k = nct - 1; k >= 0; k--) {
2581 if (s[k] != 0.0f) {
2582 for (j = k + 1; j < nu; j++) {
2583 float t = 0;
2584 for (i = k; i < m; i++) {
2585 t += U[i][k] * U[i][j];
2586 }
2587 t = -t / U[k][k];
2588 for (i = k; i < m; i++) {
2589 U[i][j] += t * U[i][k];
2590 }
2591 }
2592 for (i = k; i < m; i++) {
2593 U[i][k] = -U[i][k];
2594 }
2595 U[k][k] = 1.0f + U[k][k];
2596 for (i = 0; i < k - 1; i++) {
2597 U[i][k] = 0.0f;
2598 }
2599 }
2600 else {
2601 for (i = 0; i < m; i++) {
2602 U[i][k] = 0.0f;
2603 }
2604 U[k][k] = 1.0f;
2605 }
2606 }
2607
2608 /* If required, generate V. */
2609
2610 for (k = n - 1; k >= 0; k--) {
2611 if ((k < nrt) && (e[k] != 0.0f)) {
2612 for (j = k + 1; j < nu; j++) {
2613 float t = 0;
2614 for (i = k + 1; i < n; i++) {
2615 t += V[i][k] * V[i][j];
2616 }
2617 t = -t / V[k + 1][k];
2618 for (i = k + 1; i < n; i++) {
2619 V[i][j] += t * V[i][k];
2620 }
2621 }
2622 }
2623 for (i = 0; i < n; i++) {
2624 V[i][k] = 0.0f;
2625 }
2626 V[k][k] = 1.0f;
2627 }
2628
2629 /* Main iteration loop for the singular values. */
2630
2631 pp = p - 1;
2632 iter = 0;
2633 eps = powf(2.0f, -52.0f);
2634 while (p > 0) {
2635 int kase = 0;
2636
2637 /* Test for maximum iterations to avoid infinite loop */
2638 if (maxiter == 0) {
2639 break;
2640 }
2641 maxiter--;
2642
2643 /* This section of the program inspects for
2644 * negligible elements in the s and e arrays. On
2645 * completion the variables kase and k are set as follows.
2646 *
2647 * kase = 1: if s(p) and e[k - 1] are negligible and k<p
2648 * kase = 2: if s(k) is negligible and k<p
2649 * kase = 3: if e[k - 1] is negligible, k<p, and
2650 * s(k), ..., s(p) are not negligible (qr step).
2651 * kase = 4: if e(p - 1) is negligible (convergence). */
2652
2653 for (k = p - 2; k >= -1; k--) {
2654 if (k == -1) {
2655 break;
2656 }
2657 if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
2658 e[k] = 0.0f;
2659 break;
2660 }
2661 }
2662 if (k == p - 2) {
2663 kase = 4;
2664 }
2665 else {
2666 int ks;
2667 for (ks = p - 1; ks >= k; ks--) {
2668 float t;
2669 if (ks == k) {
2670 break;
2671 }
2672 t = (ks != p ? fabsf(e[ks]) : 0.0f) + (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
2673 if (fabsf(s[ks]) <= eps * t) {
2674 s[ks] = 0.0f;
2675 break;
2676 }
2677 }
2678 if (ks == k) {
2679 kase = 3;
2680 }
2681 else if (ks == p - 1) {
2682 kase = 1;
2683 }
2684 else {
2685 kase = 2;
2686 k = ks;
2687 }
2688 }
2689 k++;
2690
2691 /* Perform the task indicated by kase. */
2692
2693 switch (kase) {
2694
2695 /* Deflate negligible s(p). */
2696
2697 case 1: {
2698 float f = e[p - 2];
2699 e[p - 2] = 0.0f;
2700 for (j = p - 2; j >= k; j--) {
2701 float t = hypotf(s[j], f);
2702 float invt = 1.0f / t;
2703 float cs = s[j] * invt;
2704 float sn = f * invt;
2705 s[j] = t;
2706 if (j != k) {
2707 f = -sn * e[j - 1];
2708 e[j - 1] = cs * e[j - 1];
2709 }
2710
2711 for (i = 0; i < n; i++) {
2712 t = cs * V[i][j] + sn * V[i][p - 1];
2713 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
2714 V[i][j] = t;
2715 }
2716 }
2717 break;
2718 }
2719
2720 /* Split at negligible s(k). */
2721
2722 case 2: {
2723 float f = e[k - 1];
2724 e[k - 1] = 0.0f;
2725 for (j = k; j < p; j++) {
2726 float t = hypotf(s[j], f);
2727 float invt = 1.0f / t;
2728 float cs = s[j] * invt;
2729 float sn = f * invt;
2730 s[j] = t;
2731 f = -sn * e[j];
2732 e[j] = cs * e[j];
2733
2734 for (i = 0; i < m; i++) {
2735 t = cs * U[i][j] + sn * U[i][k - 1];
2736 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
2737 U[i][j] = t;
2738 }
2739 }
2740 break;
2741 }
2742
2743 /* Perform one qr step. */
2744
2745 case 3: {
2746
2747 /* Calculate the shift. */
2748
2749 float scale = max_ff(
2750 max_ff(max_ff(max_ff(fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])), fabsf(s[k])),
2751 fabsf(e[k]));
2752 float invscale = 1.0f / scale;
2753 float sp = s[p - 1] * invscale;
2754 float spm1 = s[p - 2] * invscale;
2755 float epm1 = e[p - 2] * invscale;
2756 float sk = s[k] * invscale;
2757 float ek = e[k] * invscale;
2758 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
2759 float c = (sp * epm1) * (sp * epm1);
2760 float shift = 0.0f;
2761 float f, g;
2762 if ((b != 0.0f) || (c != 0.0f)) {
2763 shift = sqrtf(b * b + c);
2764 if (b < 0.0f) {
2765 shift = -shift;
2766 }
2767 shift = c / (b + shift);
2768 }
2769 f = (sk + sp) * (sk - sp) + shift;
2770 g = sk * ek;
2771
2772 /* Chase zeros. */
2773
2774 for (j = k; j < p - 1; j++) {
2775 float t = hypotf(f, g);
2776 /* NOTE(@brecht): division by zero checks added to avoid NaN. */
2777 float cs = (t == 0.0f) ? 0.0f : f / t;
2778 float sn = (t == 0.0f) ? 0.0f : g / t;
2779 if (j != k) {
2780 e[j - 1] = t;
2781 }
2782 f = cs * s[j] + sn * e[j];
2783 e[j] = cs * e[j] - sn * s[j];
2784 g = sn * s[j + 1];
2785 s[j + 1] = cs * s[j + 1];
2786
2787 for (i = 0; i < n; i++) {
2788 t = cs * V[i][j] + sn * V[i][j + 1];
2789 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
2790 V[i][j] = t;
2791 }
2792
2793 t = hypotf(f, g);
2794 /* NOTE(@brecht): division by zero checks added to avoid NaN. */
2795 cs = (t == 0.0f) ? 0.0f : f / t;
2796 sn = (t == 0.0f) ? 0.0f : g / t;
2797 s[j] = t;
2798 f = cs * e[j] + sn * s[j + 1];
2799 s[j + 1] = -sn * e[j] + cs * s[j + 1];
2800 g = sn * e[j + 1];
2801 e[j + 1] = cs * e[j + 1];
2802 if (j < m - 1) {
2803 for (i = 0; i < m; i++) {
2804 t = cs * U[i][j] + sn * U[i][j + 1];
2805 U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
2806 U[i][j] = t;
2807 }
2808 }
2809 }
2810 e[p - 2] = f;
2811 iter = iter + 1;
2812 break;
2813 }
2814 /* Convergence. */
2815
2816 case 4: {
2817
2818 /* Make the singular values positive. */
2819
2820 if (s[k] <= 0.0f) {
2821 s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
2822
2823 for (i = 0; i <= pp; i++) {
2824 V[i][k] = -V[i][k];
2825 }
2826 }
2827
2828 /* Order the singular values. */
2829
2830 while (k < pp) {
2831 float t;
2832 if (s[k] >= s[k + 1]) {
2833 break;
2834 }
2835 t = s[k];
2836 s[k] = s[k + 1];
2837 s[k + 1] = t;
2838 if (k < n - 1) {
2839 for (i = 0; i < n; i++) {
2840 t = V[i][k + 1];
2841 V[i][k + 1] = V[i][k];
2842 V[i][k] = t;
2843 }
2844 }
2845 if (k < m - 1) {
2846 for (i = 0; i < m; i++) {
2847 t = U[i][k + 1];
2848 U[i][k + 1] = U[i][k];
2849 U[i][k] = t;
2850 }
2851 }
2852 k++;
2853 }
2854 iter = 0;
2855 p--;
2856 break;
2857 }
2858 }
2859 }
2860}
2861
2862void pseudoinverse_m4_m4(float inverse[4][4], const float mat[4][4], float epsilon)
2863{
2864 /* compute Moore-Penrose pseudo inverse of matrix, singular values
2865 * below epsilon are ignored for stability (truncated SVD) */
2866 float A[4][4], V[4][4], W[4], Wm[4][4], U[4][4];
2867 int i;
2868
2869 transpose_m4_m4(A, mat);
2870 svd_m4(V, W, U, A);
2871 transpose_m4(U);
2872 transpose_m4(V);
2873
2874 zero_m4(Wm);
2875 for (i = 0; i < 4; i++) {
2876 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
2877 }
2878
2879 transpose_m4(V);
2880
2881 mul_m4_series(inverse, U, Wm, V);
2882}
2883
2884void pseudoinverse_m3_m3(float inverse[3][3], const float mat[3][3], float epsilon)
2885{
2886 /* try regular inverse when possible, otherwise fall back to slow svd */
2887 if (!invert_m3_m3(inverse, mat)) {
2888 float mat_tmp[4][4], tmpinv[4][4];
2889
2890 copy_m4_m3(mat_tmp, mat);
2891 pseudoinverse_m4_m4(tmpinv, mat_tmp, epsilon);
2892 copy_m3_m4(inverse, tmpinv);
2893 }
2894}
2895
2896bool has_zero_axis_m4(const float matrix[4][4])
2897{
2898 return len_squared_v3(matrix[0]) < FLT_EPSILON || len_squared_v3(matrix[1]) < FLT_EPSILON ||
2899 len_squared_v3(matrix[2]) < FLT_EPSILON;
2900}
2901
2902void invert_m4_m4_safe(float inverse[4][4], const float mat[4][4])
2903{
2904 if (!invert_m4_m4(inverse, mat)) {
2905 float mat_tmp[4][4];
2906
2907 copy_m4_m4(mat_tmp, mat);
2908
2909 /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should
2910 * never be in this situation, but try to invert it anyway with tweak.
2911 */
2912 mat_tmp[0][0] += 1e-8f;
2913 mat_tmp[1][1] += 1e-8f;
2914 mat_tmp[2][2] += 1e-8f;
2915
2916 if (!invert_m4_m4(inverse, mat_tmp)) {
2918 }
2919 }
2920}
2921
2923
2924/* -------------------------------------------------------------------- */
2938
2939void invert_m4_m4_safe_ortho(float inverse[4][4], const float mat[4][4])
2940{
2941 if (!invert_m4_m4(inverse, mat)) [[unlikely]] {
2942 float mat_tmp[4][4];
2943 copy_m4_m4(mat_tmp, mat);
2944 if (!(orthogonalize_m4_zero_axes(mat_tmp, 1.0f) && invert_m4_m4(inverse, mat_tmp)))
2945 [[unlikely]]
2946 {
2948 }
2949 }
2950}
2951
2952void invert_m3_m3_safe_ortho(float inverse[3][3], const float mat[3][3])
2953{
2954 if (!invert_m3_m3(inverse, mat)) [[unlikely]] {
2955 float mat_tmp[3][3];
2956 copy_m3_m3(mat_tmp, mat);
2957 if (!(orthogonalize_m3_zero_axes(mat_tmp, 1.0f) && invert_m3_m3(inverse, mat_tmp)))
2958 [[unlikely]]
2959 {
2961 }
2962 }
2963}
2964
2966
2967/* -------------------------------------------------------------------- */
2970
2972 const float local[4][4],
2973 const float target[4][4])
2974{
2975 float itarget[4][4];
2976 invert_m4_m4(itarget, target);
2977 mul_m4_m4m4(data->local2target, itarget, local);
2978 invert_m4_m4(data->target2local, data->local2target);
2979}
2980
2982 const float local[4][4],
2983 const float target[4][4])
2984{
2985 float ilocal[4][4];
2986 invert_m4_m4(ilocal, local);
2987 mul_m4_m4m4(data->local2target, target, ilocal);
2988 invert_m4_m4(data->target2local, data->local2target);
2989}
2990
2992{
2993 mul_v3_m4v3(co, (const_cast<SpaceTransform *>(data))->local2target, co);
2994}
2995
2997{
2998 mul_v3_m4v3(co, (const_cast<SpaceTransform *>(data))->target2local, co);
2999}
3000
3002{
3003 mul_mat3_m4_v3((const_cast<SpaceTransform *>(data))->local2target, no);
3004 normalize_v3(no);
3005}
3006
3008{
3009 mul_transposed_mat3_m4_v3(data->local2target, no);
3010 normalize_v3(no);
3011}
3012
3014
3015} // namespace blender
#define ATTR_FALLTHROUGH
#define M_SQRT1_3
#define M_PI_2
#define mul_m4_series(...)
#define mul_m3_series(...)
#define Z
#define Y
#define A0
Definition RandGen.cpp:26
#define A2
Definition RandGen.cpp:28
#define A1
Definition RandGen.cpp:27
#define BLI_assert_unreachable()
#define SWAP(type, a, b)
#define ELEM(...)
#define A
btScalar max
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition btQuadWord.h:117
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition btQuadWord.h:119
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition btVector3.h:356
#define powf(x, y)
#define hypotf(x, y)
#define acosf(x)
#define rot(x, k)
#define str(s)
#define FLT_EPSILON
MatBase< C, R > inverse(MatBase< C, R >) RET
VecBase< float, D > normalize(VecOp< float, D >) RET
float3x3 scale(float3x3 mat, float2 scale)
ccl_device_inline dual1 len(const ccl_private dual< T > &a)
Definition math_dual.h:263
#define M
bool EIG_invert_m4_m4(float inverse[4][4], const float matrix[4][4])
Definition matrix.cc:29
#define T
#define B
#define R
BMesh Iterators.
void transpose_m3_m4(float R[3][3], const float M[4][4])
UserDef U
Definition blender.cc:58
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
void loc_quat_size_to_mat4(float R[4][4], const float loc[3], const float quat[4], const float size[3])
void _va_mul_m4_series_3(float r[4][4], const float m1[4][4], const float m2[4][4]) ATTR_NONNULL()
void mul_m4_m4m4_split_channels(float R[4][4], const float A[4][4], const float B[4][4])
void mul_m3_v3(const float M[3][3], float r[3])
void orthogonalize_m3(float R[3][3], int axis)
float determinant_m4_mat3_array(const float m[4][4])
void mat4_to_loc_quat(float loc[3], float quat[4], const float wmat[4][4])
bool is_uniform_scaled_m4(const float m[4][4])
void eul_to_mat3(float mat[3][3], const float eul[3])
void mul_m3_m3_pre(float R[3][3], const float A[3][3])
void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], const float wmat[4][4])
void orthogonalize_m4(float R[4][4], int axis)
void negate_m4(float R[4][4])
static bool orthogonalize_m3_zero_axes_impl(float *mat[3], const float unit_length)
bool is_identity_m4(const float m[4][4])
void _va_mul_m3_series_7(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], const float m5[3][3], const float m6[3][3]) ATTR_NONNULL()
MINLINE void copy_v3_v3(float r[3], const float a[3])
void mul_m4_fl(float R[4][4], float f)
void negate_m3(float R[3][3])
void copy_m3_m4(float m1[3][3], const float m2[4][4])
void mul_m4_m4_pre(float R[4][4], const float A[4][4])
MINLINE void add_v3_v3v3(float r[3], const float a[3], const float b[3])
void _va_mul_m4_series_8(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], const float m5[4][4], const float m6[4][4], const float m7[4][4]) ATTR_NONNULL()
void copy_m3_m3(float m1[3][3], const float m2[3][3])
void _va_mul_m3_series_4(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3]) ATTR_NONNULL()
bool invert_m4_m4_fallback(float inverse[4][4], const float mat[4][4])
void negate_mat3_m4(float R[4][4])
void copy_m4_m3(float m1[4][4], const float m2[3][3])
void BLI_svd_m3(const float m3[3][3], float r_U[3][3], float r_S[3], float r_V[3][3])
Compute the SVD (Singular Values Decomposition) of given 3D matrix (m3 = USV*).
void mat3_normalized_to_quat(float q[4], const float mat[3][3])
float mat4_to_scale(const float mat[4][4])
bool invert_m3_m3(float inverse[3][3], const float mat[3][3])
void mul_v3_m4v3(float r[3], const float mat[4][4], const float vec[3])
void mul_m3_m4m3(float R[3][3], const float A[4][4], const float B[3][3])
MINLINE void copy_v2_v2(float r[2], const float a[2])
void mul_m3_m3_post(float R[3][3], const float B[3][3])
void mul_project_m4_v3(const float mat[4][4], float vec[3])
MINLINE float mul_project_m4_v3_zfac(const float mat[4][4], const float co[3]) ATTR_WARN_UNUSED_RESULT
void normalize_m3(float R[3][3]) ATTR_NONNULL()
bool compare_m4m4(const float mat1[4][4], const float mat2[4][4], float limit)
void BLI_space_transform_from_matrices(struct SpaceTransform *data, const float local[4][4], const float target[4][4])
bool invert_m4_m4(float inverse[4][4], const float mat[4][4])
void mul_v2_m3v3(float r[2], const float M[3][3], const float a[3])
bool is_uniform_scaled_m3(const float m[3][3])
void interp_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4], float t)
void _va_mul_m3_series_3(float r[3][3], const float m1[3][3], const float m2[3][3]) ATTR_NONNULL()
void mul_mat3_m4_fl(float R[4][4], float f)
float determinant_m2(float a, float b, float c, float d)
void _va_mul_m3_series_9(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], const float m5[3][3], const float m6[3][3], const float m7[3][3], const float m8[3][3]) ATTR_NONNULL()
void BLI_space_transform_invert(const struct SpaceTransform *data, float co[3])
void mul_v3_mat3_m4v3(float r[3], const float mat[4][4], const float vec[3])
void blend_m4_m4m4(float out[4][4], const float dst[4][4], const float src[4][4], float srcweight)
void copy_m2_m2(float m1[2][2], const float m2[2][2])
bool is_negative_m4(const float mat[4][4])
void mul_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
void _va_mul_m3_series_8(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], const float m5[3][3], const float m6[3][3], const float m7[3][3]) ATTR_NONNULL()
bool invert_m4(float mat[4][4])
bool orthogonalize_m3_zero_axes(float m[3][3], float unit_length)
void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
MINLINE int max_ii(int a, int b)
void mul_m3_v2(const float m[3][3], float r[2])
ATTR_WARN_UNUSED_RESULT const BMVert * v
void mul_m3_m4m4(float R[3][3], const float A[4][4], const float B[4][4])
void unit_m2(float m[2][2])
bool has_zero_axis_m4(const float matrix[4][4])
MINLINE void copy_v3_v3_db(double r[3], const double a[3])
float determinant_m3_array(const float m[3][3])
void mat3_polar_decompose(const float mat3[3][3], float r_U[3][3], float r_P[3][3])
void normalize_m2_m2(float R[2][2], const float M[2][2]) ATTR_NONNULL()
void mat3_to_rot_size(float rot[3][3], float size[3], const float mat3[3][3])
MINLINE void mul_v3_fl(float r[3], float f)
MINLINE bool is_zero_v4(const float v[4]) ATTR_WARN_UNUSED_RESULT
void normalize_m4(float R[4][4]) ATTR_NONNULL()
MINLINE bool compare_v4v4(const float v1[4], const float v2[4], float limit) ATTR_WARN_UNUSED_RESULT
void mul_m4_m4m4_aligned_scale(float R[4][4], const float A[4][4], const float B[4][4])
void _va_mul_m4_series_5(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4]) ATTR_NONNULL()
MINLINE void zero_v4(float r[4])
void eulO_to_mat3(float M[3][3], const float e[3], short order)
void copy_m3_m3d(float m1[3][3], const double m2[3][3])
void mul_transposed_m3_v3(const float M[3][3], float r[3])
void mul_m4_v3(const float M[4][4], float r[3])
void quat_to_mat3(float m[3][3], const float q[4])
void mul_v3_project_m4_v3(float r[3], const float mat[4][4], const float vec[3])
void transpose_m3_m3(float R[3][3], const float M[3][3])
MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
MINLINE bool equals_v3v3(const float v1[3], const float v2[3]) ATTR_WARN_UNUSED_RESULT
bool is_orthogonal_m3(const float m[3][3])
MINLINE bool equals_v4v4(const float v1[4], const float v2[4]) ATTR_WARN_UNUSED_RESULT
void add_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
void size_to_mat4(float R[4][4], const float size[3])
void adjoint_m3_m3(float R[3][3], const float M[3][3])
void mul_v4_m4v3_db(double r[4], const double mat[4][4], const double vec[3])
void transpose_m3(float R[3][3])
void copy_m4_m4_db(double m1[4][4], const double m2[4][4])
MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
void mat3_normalized_to_quat_fast(float q[4], const float mat[3][3])
void loc_eul_size_to_mat4(float R[4][4], const float loc[3], const float eul[3], const float size[3])
void mul_m4_m4m3(float R[4][4], const float A[4][4], const float B[3][3])
BMesh const char void * data
void interp_qt_qtqt(float q[4], const float a[4], const float b[4], float t)
bool is_orthonormal_m3(const float m[3][3])
void adjoint_m2_m2(float R[2][2], const float M[2][2])
void BLI_space_transform_global_from_matrices(struct SpaceTransform *data, const float local[4][4], const float target[4][4])
void blend_m3_m3m3(float out[3][3], const float dst[3][3], const float src[3][3], float srcweight)
void _va_mul_m4_series_7(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], const float m5[4][4], const float m6[4][4]) ATTR_NONNULL()
MINLINE void negate_v3(float r[3])
void mul_v2_m2v2(float r[2], const float mat[2][2], const float vec[2])
bool equals_m4m4(const float mat1[4][4], const float mat2[4][4])
void loc_eulO_size_to_mat4(float R[4][4], const float loc[3], const float eul[3], const float size[3], short order)
void mul_m3_m3m4(float R[3][3], const float A[3][3], const float B[4][4])
void unit_m4(float m[4][4])
void interp_v3_v3v3(float r[3], const float a[3], const float b[3], float t)
bool orthogonalize_m4_zero_axes(float m[4][4], float unit_length)
void copy_m3d_m3(double m1[3][3], const float m2[3][3])
void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
void mul_m2_v2(const float mat[2][2], float vec[2])
void mul_v3_m4v3_db(double r[3], const double mat[4][4], const double vec[3])
void mul_v3_m3v3_db(double r[3], const double M[3][3], const double a[3])
void normalize_m4_ex(float R[4][4], float r_scale[3]) ATTR_NONNULL()
MINLINE float len_v3(const float a[3]) ATTR_WARN_UNUSED_RESULT
void _va_mul_m3_series_6(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], const float m5[3][3]) ATTR_NONNULL()
void mat4_to_size_fix_shear(float size[3], const float M[4][4])
void mul_m4_v4(const float mat[4][4], float r[4])
void invert_m3_m3_safe_ortho(float inverse[3][3], const float mat[3][3])
bool is_zero_m4(const float mat[4][4])
static void orthogonalize_stable(float v1[3], float v2[3], float v3[3], bool normalize)
void mul_m3_v3_db(const double M[3][3], double r[3])
void loc_rot_size_to_mat4(float R[4][4], const float loc[3], const float rot[3][3], const float size[3])
void unit_m4_db(double m[4][4])
void ortho_v3_v3(float out[3], const float v[3])
void print_m3(const char *str, const float m[3][3])
void normalize_m3_m3(float R[3][3], const float M[3][3]) ATTR_NONNULL()
void mul_mat3_m4_v3(const float mat[4][4], float r[3])
void copy_m4d_m4(double m1[4][4], const float m2[4][4])
MINLINE float normalize_v3_v3(float r[3], const float a[3])
void mat4_to_size(float size[3], const float M[4][4])
bool invert_m3(float mat[3][3])
void invert_m4_m4_safe(float inverse[4][4], const float mat[4][4])
void mul_v4_m4v3(float r[4], const float M[4][4], const float v[3])
void mul_v2_m3v2(float r[2], const float m[3][3], const float v[2])
MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
void transpose_m4_m4(float R[4][4], const float M[4][4])
void scale_m3_fl(float R[3][3], float scale)
MINLINE float normalize_v3(float n[3])
void pseudoinverse_m4_m4(float inverse[4][4], const float mat[4][4], float epsilon)
void mul_m3_fl(float R[3][3], float f)
void rotate_m4(float mat[4][4], char axis, float angle)
void transpose_m4(float R[4][4])
void interp_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3], float t)
void unit_m3(float m[3][3])
ATTR_WARN_UNUSED_RESULT const BMVert * v2
bool is_orthonormal_m4(const float m[4][4])
MINLINE void copy_v3_fl(float r[3], float f)
void _va_mul_m3_series_5(float r[3][3], const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3]) ATTR_NONNULL()
MINLINE float max_ff(float a, float b)
MINLINE float normalize_v3_length(float n[3], float unit_length)
void mul_m4_m4_post(float R[4][4], const float B[4][4])
MINLINE float len_squared_v3(const float v[3]) ATTR_WARN_UNUSED_RESULT
void swap_m4m4(float m1[4][4], float m2[4][4])
bool is_orthogonal_m4(const float m[4][4])
void rescale_m4(float mat[4][4], const float scale[3])
void pseudoinverse_m3_m3(float inverse[3][3], const float mat[3][3], float epsilon)
void mul_v2_m4v3(float r[2], const float mat[4][4], const float vec[3])
MINLINE float dot_v4v4(const float a[4], const float b[4]) ATTR_WARN_UNUSED_RESULT
void size_to_mat3(float R[3][3], const float size[3])
void zero_m4(float m[4][4])
void zero_m3(float m[3][3])
void _va_mul_m4_series_4(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4]) ATTR_NONNULL()
void madd_m3_m3m3fl(float R[3][3], const float A[3][3], const float B[3][3], float f)
MINLINE void copy_v4_v4(float r[4], const float a[4])
void mul_m4db_m4db_m4fl(double R[4][4], const double A[4][4], const float B[4][4])
float mat4_to_size_max_axis(const float M[4][4])
void print_m4(const char *str, const float m[4][4])
void _va_mul_m4_series_9(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], const float m5[4][4], const float m6[4][4], const float m7[4][4], const float m8[4][4]) ATTR_NONNULL()
void BLI_space_transform_apply(const struct SpaceTransform *data, float co[3])
void mul_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
void _va_mul_m4_series_6(float r[4][4], const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], const float m5[4][4]) ATTR_NONNULL()
void copy_m4_m4(float m1[4][4], const float m2[4][4])
void scale_m4_fl(float R[4][4], float scale)
void mat3_to_size(float size[3], const float M[3][3])
void shuffle_m4(float R[4][4], const int index[4])
float mat4_to_volume_scale(const float mat[4][4])
void BLI_space_transform_apply_normal(const struct SpaceTransform *data, float no[3])
void normalize_m4_m4(float rmat[4][4], const float mat[4][4]) ATTR_NONNULL()
MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3])
void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3])
void madd_m4_m4m4fl(float R[4][4], const float A[4][4], const float B[4][4], float f)
bool is_negative_m3(const float mat[3][3])
bool invert_m2_m2(float inverse[2][2], const float mat[2][2])
MINLINE int min_ii(int a, int b)
void sub_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
MINLINE float normalize_v2_v2(float r[2], const float a[2])
MINLINE float dot_v3v3(const float a[3], const float b[3]) ATTR_WARN_UNUSED_RESULT
void mul_v2_project_m4_v3(float r[2], const float mat[4][4], const float vec[3])
void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat[4][4])
void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
void orthogonalize_m4_stable(float R[4][4], int axis, bool normalize)
float determinant_m3(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3)
float mat3_to_scale(const float mat[3][3])
void mul_v3_mat3_m4v3_db(double r[3], const double mat[4][4], const double vec[3])
void mat3_to_quat(float q[4], const float mat[3][3])
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
void mul_m4_m3m4(float R[4][4], const float A[3][3], const float B[4][4])
void invert_m4_m4_safe_ortho(float inverse[4][4], const float mat[4][4])
void mul_v4_m4v4(float r[4], const float mat[4][4], const float v[4])
void BLI_space_transform_invert_normal(const struct SpaceTransform *data, float no[3])
void add_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
void adjoint_m4_m4(float R[4][4], const float M[4][4])
bool equals_m3m3(const float mat1[3][3], const float mat2[3][3])
void mul_transposed_mat3_m4_v3(const float M[4][4], float r[3])
float determinant_m4(const float m[4][4])
#define X(TYPE)
const btScalar eps
Definition poly34.cpp:11
#define fabsf
#define sqrtf
#define sinf
#define cosf
KERNEL_FEATURE_LIGHT_PASSES KERNEL_FEATURE_PATH_GUIDING float
CCL_NAMESPACE_BEGIN struct Window V