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