doxygen: add newline after \file
[blender.git] / source / blender / blenlib / intern / math_matrix.c
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
27 #include <assert.h>
28 #include "BLI_math.h"
29
30 #include "BLI_strict_flags.h"
31
32 #include "eigen_capi.h"
33
34 /********************************* Init **************************************/
35
36 void zero_m2(float m[2][2])
37 {
38         memset(m, 0, sizeof(float[2][2]));
39 }
40
41 void zero_m3(float m[3][3])
42 {
43         memset(m, 0, sizeof(float[3][3]));
44 }
45
46 void zero_m4(float m[4][4])
47 {
48         memset(m, 0, sizeof(float[4][4]));
49 }
50
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
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
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
75 void copy_m2_m2(float m1[2][2], const float m2[2][2])
76 {
77         memcpy(m1, m2, sizeof(float[2][2]));
78 }
79
80 void copy_m3_m3(float m1[3][3], const float m2[3][3])
81 {
82         /* destination comes first: */
83         memcpy(m1, m2, sizeof(float[3][3]));
84 }
85
86 void copy_m4_m4(float m1[4][4], const float m2[4][4])
87 {
88         memcpy(m1, m2, sizeof(float[4][4]));
89 }
90
91 void copy_m3_m4(float m1[3][3], const float m2[4][4])
92 {
93         m1[0][0] = m2[0][0];
94         m1[0][1] = m2[0][1];
95         m1[0][2] = m2[0][2];
96
97         m1[1][0] = m2[1][0];
98         m1[1][1] = m2[1][1];
99         m1[1][2] = m2[1][2];
100
101         m1[2][0] = m2[2][0];
102         m1[2][1] = m2[2][1];
103         m1[2][2] = m2[2][2];
104 }
105
106 void copy_m4_m3(float m1[4][4], const float m2[3][3]) /* no clear */
107 {
108         m1[0][0] = m2[0][0];
109         m1[0][1] = m2[0][1];
110         m1[0][2] = m2[0][2];
111
112         m1[1][0] = m2[1][0];
113         m1[1][1] = m2[1][1];
114         m1[1][2] = m2[1][2];
115
116         m1[2][0] = m2[2][0];
117         m1[2][1] = m2[2][1];
118         m1[2][2] = m2[2][2];
119
120         /*      Reevan's Bugfix */
121         m1[0][3] = 0.0f;
122         m1[1][3] = 0.0f;
123         m1[2][3] = 0.0f;
124
125         m1[3][0] = 0.0f;
126         m1[3][1] = 0.0f;
127         m1[3][2] = 0.0f;
128         m1[3][3] = 1.0f;
129
130 }
131
132 void copy_m3_m3d(float R[3][3], const double A[3][3])
133 {
134         /* Keep it stupid simple for better data flow in CPU. */
135         R[0][0] = (float)A[0][0];
136         R[0][1] = (float)A[0][1];
137         R[0][2] = (float)A[0][2];
138
139         R[1][0] = (float)A[1][0];
140         R[1][1] = (float)A[1][1];
141         R[1][2] = (float)A[1][2];
142
143         R[2][0] = (float)A[2][0];
144         R[2][1] = (float)A[2][1];
145         R[2][2] = (float)A[2][2];
146 }
147
148 void swap_m3m3(float m1[3][3], float m2[3][3])
149 {
150         float t;
151         int i, j;
152
153         for (i = 0; i < 3; i++) {
154                 for (j = 0; j < 3; j++) {
155                         t = m1[i][j];
156                         m1[i][j] = m2[i][j];
157                         m2[i][j] = t;
158                 }
159         }
160 }
161
162 void swap_m4m4(float m1[4][4], float m2[4][4])
163 {
164         float t;
165         int i, j;
166
167         for (i = 0; i < 4; i++) {
168                 for (j = 0; j < 4; j++) {
169                         t = m1[i][j];
170                         m1[i][j] = m2[i][j];
171                         m2[i][j] = t;
172                 }
173         }
174 }
175
176 /******************************** Arithmetic *********************************/
177
178 void mul_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
179 {
180         if (A == R)
181                 mul_m4_m4_post(R, B);
182         else if (B == R)
183                 mul_m4_m4_pre(R, A);
184         else
185                 mul_m4_m4m4_uniq(R, A, B);
186 }
187
188 void mul_m4_m4m4_uniq(float R[4][4], const float A[4][4], const float B[4][4])
189 {
190         BLI_assert(R != A && R != B);
191
192         /* matrix product: R[j][k] = A[j][i] . B[i][k] */
193 #ifdef __SSE2__
194         __m128 A0 = _mm_loadu_ps(A[0]);
195         __m128 A1 = _mm_loadu_ps(A[1]);
196         __m128 A2 = _mm_loadu_ps(A[2]);
197         __m128 A3 = _mm_loadu_ps(A[3]);
198
199         for (int i = 0; i < 4; i++) {
200                 __m128 B0 = _mm_set1_ps(B[i][0]);
201                 __m128 B1 = _mm_set1_ps(B[i][1]);
202                 __m128 B2 = _mm_set1_ps(B[i][2]);
203                 __m128 B3 = _mm_set1_ps(B[i][3]);
204
205                 __m128 sum = _mm_add_ps(
206                         _mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)),
207                         _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3)));
208
209                 _mm_storeu_ps(R[i], sum);
210         }
211 #else
212         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];
213         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];
214         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];
215         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];
216
217         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];
218         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];
219         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];
220         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];
221
222         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];
223         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];
224         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];
225         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];
226
227         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];
228         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];
229         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];
230         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];
231 #endif
232 }
233
234 void mul_m4_m4_pre(float R[4][4], const float A[4][4])
235 {
236         BLI_assert(A != R);
237         float B[4][4];
238         copy_m4_m4(B, R);
239         mul_m4_m4m4_uniq(R, A, B);
240 }
241
242 void mul_m4_m4_post(float R[4][4], const float B[4][4])
243 {
244         BLI_assert(B != R);
245         float A[4][4];
246         copy_m4_m4(A, R);
247         mul_m4_m4m4_uniq(R, A, B);
248 }
249
250 void mul_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3])
251 {
252         if (A == R)
253                 mul_m3_m3_post(R, B);
254         else if (B == R)
255                 mul_m3_m3_pre(R, A);
256         else
257                 mul_m3_m3m3_uniq(R, A, B);
258 }
259
260 void mul_m3_m3_pre(float R[3][3], const float A[3][3])
261 {
262         BLI_assert(A != R);
263         float B[3][3];
264         copy_m3_m3(B, R);
265         mul_m3_m3m3_uniq(R, A, B);
266 }
267
268 void mul_m3_m3_post(float R[3][3], const float B[3][3])
269 {
270         BLI_assert(B != R);
271         float A[3][3];
272         copy_m3_m3(A, R);
273         mul_m3_m3m3_uniq(R, A, B);
274 }
275
276 void mul_m3_m3m3_uniq(float R[3][3], const float A[3][3], const float B[3][3])
277 {
278         BLI_assert(R != A && R != B);
279
280         R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0];
281         R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1];
282         R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2];
283
284         R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0];
285         R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1];
286         R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2];
287
288         R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0];
289         R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1];
290         R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2];
291 }
292
293 void mul_m4_m4m3(float m1[4][4], const float m3_[4][4], const float m2_[3][3])
294 {
295         float m2[3][3], m3[4][4];
296
297         /* copy so it works when m1 is the same pointer as m2 or m3 */
298         /* TODO: avoid copying when matrices are different */
299         copy_m3_m3(m2, m2_);
300         copy_m4_m4(m3, m3_);
301
302         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
303         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
304         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
305         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
306         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
307         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
308         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
309         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
310         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
311 }
312
313 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */
314 void mul_m3_m3m4(float m1[3][3], const float m3_[4][4], const float m2_[3][3])
315 {
316         float m2[3][3], m3[4][4];
317
318         /* copy so it works when m1 is the same pointer as m2 or m3 */
319         /* TODO: avoid copying when matrices are different */
320         copy_m3_m3(m2, m2_);
321         copy_m4_m4(m3, m3_);
322
323         /* m1[i][j] = m2[i][k] * m3[k][j] */
324         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
325         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
326         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
327
328         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
329         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
330         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
331
332         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
333         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
334         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
335 }
336
337 void mul_m4_m3m4(float m1[4][4], const float m3_[3][3], const float m2_[4][4])
338 {
339         float m2[4][4], m3[3][3];
340
341         /* copy so it works when m1 is the same pointer as m2 or m3 */
342         /* TODO: avoid copying when matrices are different */
343         copy_m4_m4(m2, m2_);
344         copy_m3_m3(m3, m3_);
345
346         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
347         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
348         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
349         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
350         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
351         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
352         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
353         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
354         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
355 }
356
357
358 /** \name Macro helpers for: mul_m3_series
359  * \{ */
360 void _va_mul_m3_series_3(
361         float r[3][3],
362         const float m1[3][3], const float m2[3][3])
363 {
364         mul_m3_m3m3(r, m1, m2);
365 }
366 void _va_mul_m3_series_4(
367         float r[3][3],
368         const float m1[3][3], const float m2[3][3], const float m3[3][3])
369 {
370         mul_m3_m3m3(r, m1, m2);
371         mul_m3_m3m3(r, r, m3);
372 }
373 void _va_mul_m3_series_5(
374         float r[3][3],
375         const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3])
376 {
377         mul_m3_m3m3(r, m1, m2);
378         mul_m3_m3m3(r, r, m3);
379         mul_m3_m3m3(r, r, m4);
380 }
381 void _va_mul_m3_series_6(
382         float r[3][3],
383         const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3],
384         const float m5[3][3])
385 {
386         mul_m3_m3m3(r, m1, m2);
387         mul_m3_m3m3(r, r, m3);
388         mul_m3_m3m3(r, r, m4);
389         mul_m3_m3m3(r, r, m5);
390 }
391 void _va_mul_m3_series_7(
392         float r[3][3],
393         const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3],
394         const float m5[3][3], const float m6[3][3])
395 {
396         mul_m3_m3m3(r, m1, m2);
397         mul_m3_m3m3(r, r, m3);
398         mul_m3_m3m3(r, r, m4);
399         mul_m3_m3m3(r, r, m5);
400         mul_m3_m3m3(r, r, m6);
401 }
402 void _va_mul_m3_series_8(
403         float r[3][3],
404         const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3],
405         const float m5[3][3], const float m6[3][3], const float m7[3][3])
406 {
407         mul_m3_m3m3(r, m1, m2);
408         mul_m3_m3m3(r, r, m3);
409         mul_m3_m3m3(r, r, m4);
410         mul_m3_m3m3(r, r, m5);
411         mul_m3_m3m3(r, r, m6);
412         mul_m3_m3m3(r, r, m7);
413 }
414 void _va_mul_m3_series_9(
415         float r[3][3],
416         const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3],
417         const float m5[3][3], const float m6[3][3], const float m7[3][3], const float m8[3][3])
418 {
419         mul_m3_m3m3(r, m1, m2);
420         mul_m3_m3m3(r, r, m3);
421         mul_m3_m3m3(r, r, m4);
422         mul_m3_m3m3(r, r, m5);
423         mul_m3_m3m3(r, r, m6);
424         mul_m3_m3m3(r, r, m7);
425         mul_m3_m3m3(r, r, m8);
426 }
427 /** \} */
428
429 /** \name Macro helpers for: mul_m4_series
430  * \{ */
431 void _va_mul_m4_series_3(
432         float r[4][4],
433         const float m1[4][4], const float m2[4][4])
434 {
435         mul_m4_m4m4(r, m1, m2);
436 }
437 void _va_mul_m4_series_4(
438         float r[4][4],
439         const float m1[4][4], const float m2[4][4], const float m3[4][4])
440 {
441         mul_m4_m4m4(r, m1, m2);
442         mul_m4_m4m4(r, r, m3);
443 }
444 void _va_mul_m4_series_5(
445         float r[4][4],
446         const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4])
447 {
448         mul_m4_m4m4(r, m1, m2);
449         mul_m4_m4m4(r, r, m3);
450         mul_m4_m4m4(r, r, m4);
451 }
452 void _va_mul_m4_series_6(
453         float r[4][4],
454         const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4],
455         const float m5[4][4])
456 {
457         mul_m4_m4m4(r, m1, m2);
458         mul_m4_m4m4(r, r, m3);
459         mul_m4_m4m4(r, r, m4);
460         mul_m4_m4m4(r, r, m5);
461 }
462 void _va_mul_m4_series_7(
463         float r[4][4],
464         const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4],
465         const float m5[4][4], const float m6[4][4])
466 {
467         mul_m4_m4m4(r, m1, m2);
468         mul_m4_m4m4(r, r, m3);
469         mul_m4_m4m4(r, r, m4);
470         mul_m4_m4m4(r, r, m5);
471         mul_m4_m4m4(r, r, m6);
472 }
473 void _va_mul_m4_series_8(
474         float r[4][4],
475         const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4],
476         const float m5[4][4], const float m6[4][4], const float m7[4][4])
477 {
478         mul_m4_m4m4(r, m1, m2);
479         mul_m4_m4m4(r, r, m3);
480         mul_m4_m4m4(r, r, m4);
481         mul_m4_m4m4(r, r, m5);
482         mul_m4_m4m4(r, r, m6);
483         mul_m4_m4m4(r, r, m7);
484 }
485 void _va_mul_m4_series_9(
486         float r[4][4],
487         const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4],
488         const float m5[4][4], const float m6[4][4], const float m7[4][4], const float m8[4][4])
489 {
490         mul_m4_m4m4(r, m1, m2);
491         mul_m4_m4m4(r, r, m3);
492         mul_m4_m4m4(r, r, m4);
493         mul_m4_m4m4(r, r, m5);
494         mul_m4_m4m4(r, r, m6);
495         mul_m4_m4m4(r, r, m7);
496         mul_m4_m4m4(r, r, m8);
497 }
498 /** \} */
499
500 void mul_v2_m3v2(float r[2], const float m[3][3], const float v[2])
501 {
502         float temp[3], warped[3];
503
504         copy_v2_v2(temp, v);
505         temp[2] = 1.0f;
506
507         mul_v3_m3v3(warped, m, temp);
508
509         r[0] = warped[0] / warped[2];
510         r[1] = warped[1] / warped[2];
511 }
512
513 void mul_m3_v2(const float m[3][3], float r[2])
514 {
515         mul_v2_m3v2(r, m, r);
516 }
517
518 void mul_m4_v3(const float mat[4][4], float vec[3])
519 {
520         const float x = vec[0];
521         const float y = vec[1];
522
523         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
524         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
525         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
526 }
527
528 void mul_v3_m4v3(float r[3], const float mat[4][4], const float vec[3])
529 {
530         const float x = vec[0];
531         const float y = vec[1];
532
533         r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
534         r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
535         r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
536 }
537
538 void mul_v2_m4v3(float r[2], const float mat[4][4], const float vec[3])
539 {
540         const float x = vec[0];
541
542         r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
543         r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
544 }
545
546 void mul_v2_m2v2(float r[2], const float mat[2][2], const float vec[2])
547 {
548         const float x = vec[0];
549
550         r[0] = mat[0][0] * x + mat[1][0] * vec[1];
551         r[1] = mat[0][1] * x + mat[1][1] * vec[1];
552 }
553
554 void mul_m2v2(const float mat[2][2], float vec[2])
555 {
556         mul_v2_m2v2(vec, mat, vec);
557 }
558
559 /* same as mul_m4_v3() but doesnt apply translation component */
560 void mul_mat3_m4_v3(const float mat[4][4], float vec[3])
561 {
562         const float x = vec[0];
563         const float y = vec[1];
564
565         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
566         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
567         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
568 }
569
570 void mul_v3_mat3_m4v3(float r[3], const float mat[4][4], const float vec[3])
571 {
572         const float x = vec[0];
573         const float y = vec[1];
574
575         r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
576         r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
577         r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
578 }
579
580 void mul_project_m4_v3(const float mat[4][4], float vec[3])
581 {
582         /* absolute value to not flip the frustum upside down behind the camera */
583         const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
584         mul_m4_v3(mat, vec);
585
586         vec[0] /= w;
587         vec[1] /= w;
588         vec[2] /= w;
589 }
590
591 void mul_v3_project_m4_v3(float r[3], const float mat[4][4], const float vec[3])
592 {
593         const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
594         mul_v3_m4v3(r, mat, vec);
595
596         r[0] /= w;
597         r[1] /= w;
598         r[2] /= w;
599 }
600
601 void mul_v2_project_m4_v3(float r[2], const float mat[4][4], const float vec[3])
602 {
603         const float w = fabsf(mul_project_m4_v3_zfac(mat, vec));
604         mul_v2_m4v3(r, mat, vec);
605
606         r[0] /= w;
607         r[1] /= w;
608 }
609
610 void mul_v4_m4v4(float r[4], const float mat[4][4], const float v[4])
611 {
612         const float x = v[0];
613         const float y = v[1];
614         const float z = v[2];
615
616         r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
617         r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
618         r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3];
619         r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3];
620 }
621
622 void mul_m4_v4(const float mat[4][4], float r[4])
623 {
624         mul_v4_m4v4(r, mat, r);
625 }
626
627 void mul_v4d_m4v4d(double r[4], const float mat[4][4], const double v[4])
628 {
629         const double x = v[0];
630         const double y = v[1];
631         const double z = v[2];
632
633         r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
634         r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
635         r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3];
636         r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3];
637 }
638
639 void mul_m4_v4d(const float mat[4][4], double r[4])
640 {
641         mul_v4d_m4v4d(r, mat, r);
642 }
643
644 void mul_v4_m4v3(float r[4], const float M[4][4], const float v[3])
645 {
646         /* v has implicit w = 1.0f */
647         r[0] = v[0] * M[0][0] + v[1] * M[1][0] + M[2][0] * v[2] + M[3][0];
648         r[1] = v[0] * M[0][1] + v[1] * M[1][1] + M[2][1] * v[2] + M[3][1];
649         r[2] = v[0] * M[0][2] + v[1] * M[1][2] + M[2][2] * v[2] + M[3][2];
650         r[3] = v[0] * M[0][3] + v[1] * M[1][3] + M[2][3] * v[2] + M[3][3];
651 }
652
653 void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3])
654 {
655         float t[3];
656         copy_v3_v3(t, a);
657
658         r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
659         r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
660         r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2];
661 }
662
663 void mul_v3_m3v3_db(double r[3], const double M[3][3], const double a[3])
664 {
665         double t[3];
666         copy_v3_v3_db(t, a);
667
668         r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
669         r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
670         r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2];
671 }
672
673 void mul_v2_m3v3(float r[2], const float M[3][3], const float a[3])
674 {
675         float t[3];
676         copy_v3_v3(t, a);
677
678         r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2];
679         r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2];
680 }
681
682 void mul_m3_v3(const float M[3][3], float r[3])
683 {
684         mul_v3_m3v3(r, M, (const float[3]){UNPACK3(r)});
685 }
686
687 void mul_m3_v3_db(const double M[3][3], double r[3])
688 {
689         mul_v3_m3v3_db(r, M, (const double[3]){UNPACK3(r)});
690 }
691
692 void mul_transposed_m3_v3(const float mat[3][3], float vec[3])
693 {
694         const float x = vec[0];
695         const float y = vec[1];
696
697         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
698         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
699         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
700 }
701
702 void mul_transposed_mat3_m4_v3(const float mat[4][4], float vec[3])
703 {
704         const float x = vec[0];
705         const float y = vec[1];
706
707         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
708         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
709         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
710 }
711
712 void mul_m3_fl(float m[3][3], float f)
713 {
714         int i, j;
715
716         for (i = 0; i < 3; i++)
717                 for (j = 0; j < 3; j++)
718                         m[i][j] *= f;
719 }
720
721 void mul_m4_fl(float m[4][4], float f)
722 {
723         int i, j;
724
725         for (i = 0; i < 4; i++)
726                 for (j = 0; j < 4; j++)
727                         m[i][j] *= f;
728 }
729
730 void mul_mat3_m4_fl(float m[4][4], float f)
731 {
732         int i, j;
733
734         for (i = 0; i < 3; i++)
735                 for (j = 0; j < 3; j++)
736                         m[i][j] *= f;
737 }
738
739 void negate_m3(float m[3][3])
740 {
741         int i, j;
742
743         for (i = 0; i < 3; i++)
744                 for (j = 0; j < 3; j++)
745                         m[i][j] *= -1.0f;
746 }
747
748 void negate_mat3_m4(float m[4][4])
749 {
750         int i, j;
751
752         for (i = 0; i < 3; i++)
753                 for (j = 0; j < 3; j++)
754                         m[i][j] *= -1.0f;
755 }
756
757 void negate_m4(float m[4][4])
758 {
759         int i, j;
760
761         for (i = 0; i < 4; i++)
762                 for (j = 0; j < 4; j++)
763                         m[i][j] *= -1.0f;
764 }
765
766 void mul_m3_v3_double(const float mat[3][3], double vec[3])
767 {
768         const double x = vec[0];
769         const double y = vec[1];
770
771         vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
772         vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
773         vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
774 }
775
776 void add_m3_m3m3(float m1[3][3], const float m2[3][3], const float m3[3][3])
777 {
778         int i, j;
779
780         for (i = 0; i < 3; i++)
781                 for (j = 0; j < 3; j++)
782                         m1[i][j] = m2[i][j] + m3[i][j];
783 }
784
785 void add_m4_m4m4(float m1[4][4], const float m2[4][4], const float m3[4][4])
786 {
787         int i, j;
788
789         for (i = 0; i < 4; i++)
790                 for (j = 0; j < 4; j++)
791                         m1[i][j] = m2[i][j] + m3[i][j];
792 }
793
794 void sub_m3_m3m3(float m1[3][3], const float m2[3][3], const float m3[3][3])
795 {
796         int i, j;
797
798         for (i = 0; i < 3; i++)
799                 for (j = 0; j < 3; j++)
800                         m1[i][j] = m2[i][j] - m3[i][j];
801 }
802
803 void sub_m4_m4m4(float m1[4][4], const float m2[4][4], const float m3[4][4])
804 {
805         int i, j;
806
807         for (i = 0; i < 4; i++)
808                 for (j = 0; j < 4; j++)
809                         m1[i][j] = m2[i][j] - m3[i][j];
810 }
811
812 float determinant_m3_array(const float m[3][3])
813 {
814         return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
815                 m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
816                 m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
817 }
818
819 bool invert_m3_ex(float m[3][3], const float epsilon)
820 {
821         float tmp[3][3];
822         const bool success = invert_m3_m3_ex(tmp, m, epsilon);
823
824         copy_m3_m3(m, tmp);
825         return success;
826 }
827
828 bool invert_m3_m3_ex(float m1[3][3], const float m2[3][3], const float epsilon)
829 {
830         float det;
831         int a, b;
832         bool success;
833
834         BLI_assert(epsilon >= 0.0f);
835
836         /* calc adjoint */
837         adjoint_m3_m3(m1, m2);
838
839         /* then determinant old matrix! */
840         det = determinant_m3_array(m2);
841
842         success = (fabsf(det) > epsilon);
843
844         if (LIKELY(det != 0.0f)) {
845                 det = 1.0f / det;
846                 for (a = 0; a < 3; a++) {
847                         for (b = 0; b < 3; b++) {
848                                 m1[a][b] *= det;
849                         }
850                 }
851         }
852         return success;
853 }
854
855 bool invert_m3(float m[3][3])
856 {
857         float tmp[3][3];
858         const bool success = invert_m3_m3(tmp, m);
859
860         copy_m3_m3(m, tmp);
861         return success;
862 }
863
864 bool invert_m3_m3(float m1[3][3], const float m2[3][3])
865 {
866         float det;
867         int a, b;
868         bool success;
869
870         /* calc adjoint */
871         adjoint_m3_m3(m1, m2);
872
873         /* then determinant old matrix! */
874         det = determinant_m3_array(m2);
875
876         success = (det != 0.0f);
877
878         if (LIKELY(det != 0.0f)) {
879                 det = 1.0f / det;
880                 for (a = 0; a < 3; a++) {
881                         for (b = 0; b < 3; b++) {
882                                 m1[a][b] *= det;
883                         }
884                 }
885         }
886
887         return success;
888 }
889
890 bool invert_m4(float m[4][4])
891 {
892         float tmp[4][4];
893         const bool success = invert_m4_m4(tmp, m);
894
895         copy_m4_m4(m, tmp);
896         return success;
897 }
898
899 bool invert_m4_m4(float inverse[4][4], const float mat[4][4])
900 {
901         /* Use optimized matrix inverse from Eigen, since performance
902          * impact of this function is significant in complex rigs. */
903         return EIG_invert_m4_m4(inverse, mat);
904 }
905
906 /****************************** Linear Algebra *******************************/
907
908 void transpose_m3(float mat[3][3])
909 {
910         float t;
911
912         t = mat[0][1];
913         mat[0][1] = mat[1][0];
914         mat[1][0] = t;
915         t = mat[0][2];
916         mat[0][2] = mat[2][0];
917         mat[2][0] = t;
918         t = mat[1][2];
919         mat[1][2] = mat[2][1];
920         mat[2][1] = t;
921 }
922
923 void transpose_m3_m3(float rmat[3][3], const float mat[3][3])
924 {
925         BLI_assert(rmat != mat);
926
927         rmat[0][0] = mat[0][0];
928         rmat[0][1] = mat[1][0];
929         rmat[0][2] = mat[2][0];
930         rmat[1][0] = mat[0][1];
931         rmat[1][1] = mat[1][1];
932         rmat[1][2] = mat[2][1];
933         rmat[2][0] = mat[0][2];
934         rmat[2][1] = mat[1][2];
935         rmat[2][2] = mat[2][2];
936 }
937
938 /* seems obscure but in-fact a common operation */
939 void transpose_m3_m4(float rmat[3][3], const float mat[4][4])
940 {
941         BLI_assert(&rmat[0][0] != &mat[0][0]);
942
943         rmat[0][0] = mat[0][0];
944         rmat[0][1] = mat[1][0];
945         rmat[0][2] = mat[2][0];
946         rmat[1][0] = mat[0][1];
947         rmat[1][1] = mat[1][1];
948         rmat[1][2] = mat[2][1];
949         rmat[2][0] = mat[0][2];
950         rmat[2][1] = mat[1][2];
951         rmat[2][2] = mat[2][2];
952 }
953
954 void transpose_m4(float mat[4][4])
955 {
956         float t;
957
958         t = mat[0][1];
959         mat[0][1] = mat[1][0];
960         mat[1][0] = t;
961         t = mat[0][2];
962         mat[0][2] = mat[2][0];
963         mat[2][0] = t;
964         t = mat[0][3];
965         mat[0][3] = mat[3][0];
966         mat[3][0] = t;
967
968         t = mat[1][2];
969         mat[1][2] = mat[2][1];
970         mat[2][1] = t;
971         t = mat[1][3];
972         mat[1][3] = mat[3][1];
973         mat[3][1] = t;
974
975         t = mat[2][3];
976         mat[2][3] = mat[3][2];
977         mat[3][2] = t;
978 }
979
980 void transpose_m4_m4(float rmat[4][4], const float mat[4][4])
981 {
982         BLI_assert(rmat != mat);
983
984         rmat[0][0] = mat[0][0];
985         rmat[0][1] = mat[1][0];
986         rmat[0][2] = mat[2][0];
987         rmat[0][3] = mat[3][0];
988         rmat[1][0] = mat[0][1];
989         rmat[1][1] = mat[1][1];
990         rmat[1][2] = mat[2][1];
991         rmat[1][3] = mat[3][1];
992         rmat[2][0] = mat[0][2];
993         rmat[2][1] = mat[1][2];
994         rmat[2][2] = mat[2][2];
995         rmat[2][3] = mat[3][2];
996         rmat[3][0] = mat[0][3];
997         rmat[3][1] = mat[1][3];
998         rmat[3][2] = mat[2][3];
999         rmat[3][3] = mat[3][3];
1000 }
1001
1002 /* TODO: return bool */
1003 int compare_m4m4(const float mat1[4][4], const float mat2[4][4], float limit)
1004 {
1005         if (compare_v4v4(mat1[0], mat2[0], limit))
1006                 if (compare_v4v4(mat1[1], mat2[1], limit))
1007                         if (compare_v4v4(mat1[2], mat2[2], limit))
1008                                 if (compare_v4v4(mat1[3], mat2[3], limit))
1009                                         return 1;
1010         return 0;
1011 }
1012
1013 /**
1014  * Make an orthonormal matrix around the selected axis of the given matrix.
1015  *
1016  * \param axis: Axis to build the orthonormal basis around.
1017  */
1018 void orthogonalize_m3(float mat[3][3], int axis)
1019 {
1020         float size[3];
1021         mat3_to_size(size, mat);
1022         normalize_v3(mat[axis]);
1023         switch (axis) {
1024                 case 0:
1025                         if (dot_v3v3(mat[0], mat[1]) < 1) {
1026                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1027                                 normalize_v3(mat[2]);
1028                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1029                         }
1030                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1031                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1032                                 normalize_v3(mat[1]);
1033                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1034                         }
1035                         else {
1036                                 float vec[3];
1037
1038                                 vec[0] = mat[0][1];
1039                                 vec[1] = mat[0][2];
1040                                 vec[2] = mat[0][0];
1041
1042                                 cross_v3_v3v3(mat[2], mat[0], vec);
1043                                 normalize_v3(mat[2]);
1044                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1045                         }
1046                         break;
1047                 case 1:
1048                         if (dot_v3v3(mat[1], mat[0]) < 1) {
1049                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1050                                 normalize_v3(mat[2]);
1051                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1052                         }
1053                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1054                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1055                                 normalize_v3(mat[0]);
1056                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1057                         }
1058                         else {
1059                                 float vec[3];
1060
1061                                 vec[0] = mat[1][1];
1062                                 vec[1] = mat[1][2];
1063                                 vec[2] = mat[1][0];
1064
1065                                 cross_v3_v3v3(mat[0], mat[1], vec);
1066                                 normalize_v3(mat[0]);
1067                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1068                         }
1069                         break;
1070                 case 2:
1071                         if (dot_v3v3(mat[2], mat[0]) < 1) {
1072                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1073                                 normalize_v3(mat[1]);
1074                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1075                         }
1076                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
1077                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1078                                 normalize_v3(mat[0]);
1079                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1080                         }
1081                         else {
1082                                 float vec[3];
1083
1084                                 vec[0] = mat[2][1];
1085                                 vec[1] = mat[2][2];
1086                                 vec[2] = mat[2][0];
1087
1088                                 cross_v3_v3v3(mat[0], vec, mat[2]);
1089                                 normalize_v3(mat[0]);
1090                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1091                         }
1092                         break;
1093                 default:
1094                         BLI_assert(0);
1095                         break;
1096         }
1097         mul_v3_fl(mat[0], size[0]);
1098         mul_v3_fl(mat[1], size[1]);
1099         mul_v3_fl(mat[2], size[2]);
1100 }
1101
1102 /**
1103  * Make an orthonormal matrix around the selected axis of the given matrix.
1104  *
1105  * \param axis: Axis to build the orthonormal basis around.
1106  */
1107 void orthogonalize_m4(float mat[4][4], int axis)
1108 {
1109         float size[3];
1110         mat4_to_size(size, mat);
1111         normalize_v3(mat[axis]);
1112         switch (axis) {
1113                 case 0:
1114                         if (dot_v3v3(mat[0], mat[1]) < 1) {
1115                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1116                                 normalize_v3(mat[2]);
1117                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1118                         }
1119                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1120                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1121                                 normalize_v3(mat[1]);
1122                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1123                         }
1124                         else {
1125                                 float vec[3];
1126
1127                                 vec[0] = mat[0][1];
1128                                 vec[1] = mat[0][2];
1129                                 vec[2] = mat[0][0];
1130
1131                                 cross_v3_v3v3(mat[2], mat[0], vec);
1132                                 normalize_v3(mat[2]);
1133                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1134                         }
1135                         break;
1136                 case 1:
1137                         if (dot_v3v3(mat[1], mat[0]) < 1) {
1138                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1139                                 normalize_v3(mat[2]);
1140                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1141                         }
1142                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1143                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1144                                 normalize_v3(mat[0]);
1145                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1146                         }
1147                         else {
1148                                 float vec[3];
1149
1150                                 vec[0] = mat[1][1];
1151                                 vec[1] = mat[1][2];
1152                                 vec[2] = mat[1][0];
1153
1154                                 cross_v3_v3v3(mat[0], mat[1], vec);
1155                                 normalize_v3(mat[0]);
1156                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1157                         }
1158                         break;
1159                 case 2:
1160                         if (dot_v3v3(mat[2], mat[0]) < 1) {
1161                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1162                                 normalize_v3(mat[1]);
1163                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1164                         }
1165                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
1166                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1167                                 normalize_v3(mat[0]);
1168                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1169                         }
1170                         else {
1171                                 float vec[3];
1172
1173                                 vec[0] = mat[2][1];
1174                                 vec[1] = mat[2][2];
1175                                 vec[2] = mat[2][0];
1176
1177                                 cross_v3_v3v3(mat[0], vec, mat[2]);
1178                                 normalize_v3(mat[0]);
1179                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1180                         }
1181                         break;
1182                 default:
1183                         BLI_assert(0);
1184                         break;
1185         }
1186         mul_v3_fl(mat[0], size[0]);
1187         mul_v3_fl(mat[1], size[1]);
1188         mul_v3_fl(mat[2], size[2]);
1189 }
1190
1191 bool is_orthogonal_m3(const float m[3][3])
1192 {
1193         int i, j;
1194
1195         for (i = 0; i < 3; i++) {
1196                 for (j = 0; j < i; j++) {
1197                         if (fabsf(dot_v3v3(m[i], m[j])) > 1e-5f)
1198                                 return false;
1199                 }
1200         }
1201
1202         return true;
1203 }
1204
1205 bool is_orthogonal_m4(const float m[4][4])
1206 {
1207         int i, j;
1208
1209         for (i = 0; i < 4; i++) {
1210                 for (j = 0; j < i; j++) {
1211                         if (fabsf(dot_v4v4(m[i], m[j])) > 1e-5f)
1212                                 return false;
1213                 }
1214
1215         }
1216
1217         return true;
1218 }
1219
1220 bool is_orthonormal_m3(const float m[3][3])
1221 {
1222         if (is_orthogonal_m3(m)) {
1223                 int i;
1224
1225                 for (i = 0; i < 3; i++)
1226                         if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1e-5f)
1227                                 return false;
1228
1229                 return true;
1230         }
1231
1232         return false;
1233 }
1234
1235 bool is_orthonormal_m4(const float m[4][4])
1236 {
1237         if (is_orthogonal_m4(m)) {
1238                 int i;
1239
1240                 for (i = 0; i < 4; i++)
1241                         if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1e-5f)
1242                                 return false;
1243
1244                 return true;
1245         }
1246
1247         return false;
1248 }
1249
1250 bool is_uniform_scaled_m3(const float m[3][3])
1251 {
1252         const float eps = 1e-7f;
1253         float t[3][3];
1254         float l1, l2, l3, l4, l5, l6;
1255
1256         transpose_m3_m3(t, m);
1257
1258         l1 = len_squared_v3(m[0]);
1259         l2 = len_squared_v3(m[1]);
1260         l3 = len_squared_v3(m[2]);
1261
1262         l4 = len_squared_v3(t[0]);
1263         l5 = len_squared_v3(t[1]);
1264         l6 = len_squared_v3(t[2]);
1265
1266         if (fabsf(l2 - l1) <= eps &&
1267             fabsf(l3 - l1) <= eps &&
1268             fabsf(l4 - l1) <= eps &&
1269             fabsf(l5 - l1) <= eps &&
1270             fabsf(l6 - l1) <= eps)
1271         {
1272                 return true;
1273         }
1274
1275         return false;
1276 }
1277
1278 bool is_uniform_scaled_m4(const float m[4][4])
1279 {
1280         float t[3][3];
1281         copy_m3_m4(t, m);
1282         return is_uniform_scaled_m3(t);
1283 }
1284
1285 void normalize_m3_ex(float mat[3][3], float r_scale[3])
1286 {
1287         int i;
1288         for (i = 0; i < 3; i++) {
1289                 r_scale[i] = normalize_v3(mat[i]);
1290         }
1291 }
1292 void normalize_m3(float mat[3][3])
1293 {
1294         int i;
1295         for (i = 0; i < 3; i++) {
1296                 normalize_v3(mat[i]);
1297         }
1298 }
1299
1300 void normalize_m3_m3_ex(float rmat[3][3], const float mat[3][3], float r_scale[3])
1301 {
1302         int i;
1303         for (i = 0; i < 3; i++) {
1304                 r_scale[i] = normalize_v3_v3(rmat[i], mat[i]);
1305         }
1306 }
1307 void normalize_m3_m3(float rmat[3][3], const float mat[3][3])
1308 {
1309         int i;
1310         for (i = 0; i < 3; i++) {
1311                 normalize_v3_v3(rmat[i], mat[i]);
1312         }
1313 }
1314
1315 void normalize_m4_ex(float mat[4][4], float r_scale[3])
1316 {
1317         int i;
1318         for (i = 0; i < 3; i++) {
1319                 r_scale[i] = normalize_v3(mat[i]);
1320                 if (r_scale[i] != 0.0f) {
1321                         mat[i][3] /= r_scale[i];
1322                 }
1323         }
1324 }
1325 void normalize_m4(float mat[4][4])
1326 {
1327         int i;
1328         for (i = 0; i < 3; i++) {
1329                 float len = normalize_v3(mat[i]);
1330                 if (len != 0.0f) {
1331                         mat[i][3] /= len;
1332                 }
1333         }
1334 }
1335
1336 void normalize_m4_m4_ex(float rmat[4][4], const float mat[4][4], float r_scale[3])
1337 {
1338         int i;
1339         for (i = 0; i < 3; i++) {
1340                 r_scale[i] = normalize_v3_v3(rmat[i], mat[i]);
1341                 rmat[i][3] = (r_scale[i] != 0.0f) ? (mat[i][3] / r_scale[i]) : mat[i][3];
1342         }
1343         copy_v4_v4(rmat[3], mat[3]);
1344 }
1345 void normalize_m4_m4(float rmat[4][4], const float mat[4][4])
1346 {
1347         int i;
1348         for (i = 0; i < 3; i++) {
1349                 float len = normalize_v3_v3(rmat[i], mat[i]);
1350                 rmat[i][3] = (len != 0.0f) ? (mat[i][3] / len) : mat[i][3];
1351         }
1352         copy_v4_v4(rmat[3], mat[3]);
1353 }
1354
1355 void adjoint_m2_m2(float m1[2][2], const float m[2][2])
1356 {
1357         BLI_assert(m1 != m);
1358         m1[0][0] =  m[1][1];
1359         m1[0][1] = -m[0][1];
1360         m1[1][0] = -m[1][0];
1361         m1[1][1] =  m[0][0];
1362 }
1363
1364 void adjoint_m3_m3(float m1[3][3], const float m[3][3])
1365 {
1366         BLI_assert(m1 != m);
1367         m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
1368         m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
1369         m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
1370
1371         m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0];
1372         m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
1373         m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0];
1374
1375         m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
1376         m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0];
1377         m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
1378 }
1379
1380 void adjoint_m4_m4(float out[4][4], const float in[4][4]) /* out = ADJ(in) */
1381 {
1382         float a1, a2, a3, a4, b1, b2, b3, b4;
1383         float c1, c2, c3, c4, d1, d2, d3, d4;
1384
1385         a1 = in[0][0];
1386         b1 = in[0][1];
1387         c1 = in[0][2];
1388         d1 = in[0][3];
1389
1390         a2 = in[1][0];
1391         b2 = in[1][1];
1392         c2 = in[1][2];
1393         d2 = in[1][3];
1394
1395         a3 = in[2][0];
1396         b3 = in[2][1];
1397         c3 = in[2][2];
1398         d3 = in[2][3];
1399
1400         a4 = in[3][0];
1401         b4 = in[3][1];
1402         c4 = in[3][2];
1403         d4 = in[3][3];
1404
1405
1406         out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1407         out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1408         out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1409         out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1410
1411         out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1412         out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1413         out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1414         out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1415
1416         out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1417         out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1418         out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1419         out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1420
1421         out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1422         out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1423         out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1424         out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1425 }
1426
1427 float determinant_m2(float a, float b, float c, float d)
1428 {
1429
1430         return a * d - b * c;
1431 }
1432
1433 float determinant_m3(float a1, float a2, float a3,
1434                      float b1, float b2, float b3,
1435                      float c1, float c2, float c3)
1436 {
1437         float ans;
1438
1439         ans = (a1 * determinant_m2(b2, b3, c2, c3) -
1440                b1 * determinant_m2(a2, a3, c2, c3) +
1441                c1 * determinant_m2(a2, a3, b2, b3));
1442
1443         return ans;
1444 }
1445
1446 float determinant_m4(const float m[4][4])
1447 {
1448         float ans;
1449         float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1450
1451         a1 = m[0][0];
1452         b1 = m[0][1];
1453         c1 = m[0][2];
1454         d1 = m[0][3];
1455
1456         a2 = m[1][0];
1457         b2 = m[1][1];
1458         c2 = m[1][2];
1459         d2 = m[1][3];
1460
1461         a3 = m[2][0];
1462         b3 = m[2][1];
1463         c3 = m[2][2];
1464         d3 = m[2][3];
1465
1466         a4 = m[3][0];
1467         b4 = m[3][1];
1468         c4 = m[3][2];
1469         d4 = m[3][3];
1470
1471         ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1472                b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1473                c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1474                d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1475
1476         return ans;
1477 }
1478
1479 /****************************** Transformations ******************************/
1480
1481 void size_to_mat3(float mat[3][3], const float size[3])
1482 {
1483         mat[0][0] = size[0];
1484         mat[0][1] = 0.0f;
1485         mat[0][2] = 0.0f;
1486         mat[1][1] = size[1];
1487         mat[1][0] = 0.0f;
1488         mat[1][2] = 0.0f;
1489         mat[2][2] = size[2];
1490         mat[2][1] = 0.0f;
1491         mat[2][0] = 0.0f;
1492 }
1493
1494 void size_to_mat4(float mat[4][4], const float size[3])
1495 {
1496         mat[0][0] = size[0];
1497         mat[0][1] = 0.0f;
1498         mat[0][2] = 0.0f;
1499         mat[0][3] = 0.0f;
1500         mat[1][0] = 0.0f;
1501         mat[1][1] = size[1];
1502         mat[1][2] = 0.0f;
1503         mat[1][3] = 0.0f;
1504         mat[2][0] = 0.0f;
1505         mat[2][1] = 0.0f;
1506         mat[2][2] = size[2];
1507         mat[2][3] = 0.0f;
1508         mat[3][0] = 0.0f;
1509         mat[3][1] = 0.0f;
1510         mat[3][2] = 0.0f;
1511         mat[3][3] = 1.0f;
1512 }
1513
1514 void mat3_to_size(float size[3], const float mat[3][3])
1515 {
1516         size[0] = len_v3(mat[0]);
1517         size[1] = len_v3(mat[1]);
1518         size[2] = len_v3(mat[2]);
1519 }
1520
1521 void mat4_to_size(float size[3], const float mat[4][4])
1522 {
1523         size[0] = len_v3(mat[0]);
1524         size[1] = len_v3(mat[1]);
1525         size[2] = len_v3(mat[2]);
1526 }
1527
1528 /* this gets the average scale of a matrix, only use when your scaling
1529  * data that has no idea of scale axis, examples are bone-envelope-radius
1530  * and curve radius */
1531 float mat3_to_scale(const float mat[3][3])
1532 {
1533         /* unit length vector */
1534         float unit_vec[3];
1535         copy_v3_fl(unit_vec, (float)M_SQRT1_3);
1536         mul_m3_v3(mat, unit_vec);
1537         return len_v3(unit_vec);
1538 }
1539
1540 float mat4_to_scale(const float mat[4][4])
1541 {
1542         /* unit length vector */
1543         float unit_vec[3];
1544         copy_v3_fl(unit_vec, (float)M_SQRT1_3);
1545         mul_mat3_m4_v3(mat, unit_vec);
1546         return len_v3(unit_vec);
1547 }
1548
1549 /** Return 2D scale (in XY plane) of given mat4. */
1550 float mat4_to_xy_scale(const float M[4][4])
1551 {
1552         /* unit length vector in xy plane */
1553         float unit_vec[3] = {(float)M_SQRT1_2, (float)M_SQRT1_2, 0.0f};
1554         mul_mat3_m4_v3(M, unit_vec);
1555         return len_v3(unit_vec);
1556 }
1557
1558 void mat3_to_rot_size(float rot[3][3], float size[3], const float mat3[3][3])
1559 {
1560         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1561         size[0] = normalize_v3_v3(rot[0], mat3[0]);
1562         size[1] = normalize_v3_v3(rot[1], mat3[1]);
1563         size[2] = normalize_v3_v3(rot[2], mat3[2]);
1564         if (UNLIKELY(is_negative_m3(rot))) {
1565                 negate_m3(rot);
1566                 negate_v3(size);
1567         }
1568 }
1569
1570 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], const float wmat[4][4])
1571 {
1572         float mat3[3][3]; /* wmat -> 3x3 */
1573
1574         copy_m3_m4(mat3, wmat);
1575         mat3_to_rot_size(rot, size, mat3);
1576
1577         /* location */
1578         copy_v3_v3(loc, wmat[3]);
1579 }
1580
1581 void mat4_to_loc_quat(float loc[3], float quat[4], const float wmat[4][4])
1582 {
1583         float mat3[3][3];
1584         float mat3_n[3][3]; /* normalized mat3 */
1585
1586         copy_m3_m4(mat3, wmat);
1587         normalize_m3_m3(mat3_n, mat3);
1588
1589         /* so scale doesn't interfere with rotation [#24291] */
1590         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1591         if (is_negative_m3(mat3)) {
1592                 negate_m3(mat3_n);
1593         }
1594
1595         mat3_normalized_to_quat(quat, mat3_n);
1596         copy_v3_v3(loc, wmat[3]);
1597 }
1598
1599 void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat[4][4])
1600 {
1601         float rot[3][3];
1602         mat4_to_loc_rot_size(loc, rot, size, wmat);
1603         mat3_normalized_to_quat(quat, rot);
1604 }
1605
1606 /**
1607  * Right polar decomposition:
1608  *     M = UP
1609  *
1610  * U is the 'rotation'-like component, the closest orthogonal matrix to M.
1611  * P is the 'scaling'-like component, defined in U space.
1612  *
1613  * See https://en.wikipedia.org/wiki/Polar_decomposition for more.
1614  */
1615 #ifndef MATH_STANDALONE
1616 void mat3_polar_decompose(const float mat3[3][3], float r_U[3][3], float r_P[3][3])
1617 {
1618         /* From svd decomposition (M = WSV*), we have:
1619          *     U = WV*
1620          *     P = VSV*
1621          */
1622         float W[3][3], S[3][3], V[3][3], Vt[3][3];
1623         float sval[3];
1624
1625         BLI_svd_m3(mat3, W, sval, V);
1626
1627         size_to_mat3(S, sval);
1628
1629         transpose_m3_m3(Vt, V);
1630         mul_m3_m3m3(r_U, W, Vt);
1631         mul_m3_series(r_P, V, S, Vt);
1632 }
1633 #endif
1634
1635 void scale_m3_fl(float m[3][3], float scale)
1636 {
1637         m[0][0] = m[1][1] = m[2][2] = scale;
1638         m[0][1] = m[0][2] = 0.0;
1639         m[1][0] = m[1][2] = 0.0;
1640         m[2][0] = m[2][1] = 0.0;
1641 }
1642
1643 void scale_m4_fl(float m[4][4], float scale)
1644 {
1645         m[0][0] = m[1][1] = m[2][2] = scale;
1646         m[3][3] = 1.0;
1647         m[0][1] = m[0][2] = m[0][3] = 0.0;
1648         m[1][0] = m[1][2] = m[1][3] = 0.0;
1649         m[2][0] = m[2][1] = m[2][3] = 0.0;
1650         m[3][0] = m[3][1] = m[3][2] = 0.0;
1651 }
1652
1653 void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
1654 {
1655         mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
1656         mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
1657         mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
1658 }
1659
1660 /* TODO: enum for axis? */
1661 /**
1662  * Rotate a matrix in-place.
1663  *
1664  * \note To create a new rotation matrix see:
1665  * #axis_angle_to_mat4_single, #axis_angle_to_mat3_single, #angle_to_mat2
1666  * (axis & angle args are compatible).
1667  */
1668 void rotate_m4(float mat[4][4], const char axis, const float angle)
1669 {
1670         const float angle_cos = cosf(angle);
1671         const float angle_sin = sinf(angle);
1672
1673         assert(axis >= 'X' && axis <= 'Z');
1674
1675         switch (axis) {
1676                 case 'X':
1677                         for (int col = 0; col < 4; col++) {
1678                                 float temp  =  angle_cos * mat[1][col] + angle_sin * mat[2][col];
1679                                 mat[2][col] = -angle_sin * mat[1][col] + angle_cos * mat[2][col];
1680                                 mat[1][col] =  temp;
1681                         }
1682                         break;
1683
1684                 case 'Y':
1685                         for (int col = 0; col < 4; col++) {
1686                                 float temp  =  angle_cos * mat[0][col] - angle_sin * mat[2][col];
1687                                 mat[2][col] =  angle_sin * mat[0][col] + angle_cos * mat[2][col];
1688                                 mat[0][col] =  temp;
1689                         }
1690                         break;
1691
1692                 case 'Z':
1693                         for (int col = 0; col < 4; col++) {
1694                                 float temp  =  angle_cos * mat[0][col] + angle_sin * mat[1][col];
1695                                 mat[1][col] = -angle_sin * mat[0][col] + angle_cos * mat[1][col];
1696                                 mat[0][col] =  temp;
1697                         }
1698                         break;
1699                 default:
1700                         BLI_assert(0);
1701                         break;
1702         }
1703 }
1704
1705 /**
1706  * Scale or rotate around a pivot point,
1707  * a convenience function to avoid having to do inline.
1708  *
1709  * Since its common to make a scale/rotation matrix that pivots around an arbitrary point.
1710  *
1711  * Typical use case is to make 3x3 matrix, copy to 4x4, then pass to this function.
1712  */
1713 void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
1714 {
1715         float tmat[4][4];
1716
1717         unit_m4(tmat);
1718
1719         copy_v3_v3(tmat[3], pivot);
1720         mul_m4_m4m4(mat, tmat, mat);
1721
1722         /* invert the matrix */
1723         negate_v3(tmat[3]);
1724         mul_m4_m4m4(mat, mat, tmat);
1725 }
1726
1727 void blend_m3_m3m3(float out[3][3], const float dst[3][3], const float src[3][3], const float srcweight)
1728 {
1729         float srot[3][3], drot[3][3];
1730         float squat[4], dquat[4], fquat[4];
1731         float sscale[3], dscale[3], fsize[3];
1732         float rmat[3][3], smat[3][3];
1733
1734         mat3_to_rot_size(drot, dscale, dst);
1735         mat3_to_rot_size(srot, sscale, src);
1736
1737         mat3_normalized_to_quat(dquat, drot);
1738         mat3_normalized_to_quat(squat, srot);
1739
1740         /* do blending */
1741         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1742         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1743
1744         /* compose new matrix */
1745         quat_to_mat3(rmat, fquat);
1746         size_to_mat3(smat, fsize);
1747         mul_m3_m3m3(out, rmat, smat);
1748 }
1749
1750 void blend_m4_m4m4(float out[4][4], const float dst[4][4], const float src[4][4], const float srcweight)
1751 {
1752         float sloc[3], dloc[3], floc[3];
1753         float srot[3][3], drot[3][3];
1754         float squat[4], dquat[4], fquat[4];
1755         float sscale[3], dscale[3], fsize[3];
1756
1757         mat4_to_loc_rot_size(dloc, drot, dscale, dst);
1758         mat4_to_loc_rot_size(sloc, srot, sscale, src);
1759
1760         mat3_normalized_to_quat(dquat, drot);
1761         mat3_normalized_to_quat(squat, srot);
1762
1763         /* do blending */
1764         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1765         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1766         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1767
1768         /* compose new matrix */
1769         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1770 }
1771
1772 /* for builds without Eigen */
1773 #ifndef MATH_STANDALONE
1774 /**
1775  * A polar-decomposition-based interpolation between matrix A and matrix B.
1776  *
1777  * \note This code is about five times slower as the 'naive' interpolation done by #blend_m3_m3m3
1778  *       (it typically remains below 2 usec on an average i74700, while #blend_m3_m3m3 remains below 0.4 usec).
1779  *       However, it gives expected results even with non-uniformaly scaled matrices, see T46418 for an example.
1780  *
1781  * Based on "Matrix Animation and Polar Decomposition", by Ken Shoemake & Tom Duff
1782  *
1783  * \param R: Resulting interpolated matrix.
1784  * \param A: Input matrix which is totally effective with `t = 0.0`.
1785  * \param B: Input matrix which is totally effective with `t = 1.0`.
1786  * \param t: Interpolation factor.
1787  */
1788 void interp_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3], const float t)
1789 {
1790         /* 'Rotation' component ('U' part of polar decomposition,
1791          * the closest orthogonal matrix to M3 rot/scale
1792          * transformation matrix), spherically interpolated. */
1793         float U_A[3][3], U_B[3][3], U[3][3];
1794         float quat_A[4], quat_B[4], quat[4];
1795         /* 'Scaling' component ('P' part of polar decomposition, i.e. scaling in U-defined space),
1796          * linearly interpolated. */
1797         float P_A[3][3], P_B[3][3], P[3][3];
1798
1799         int i;
1800
1801         mat3_polar_decompose(A, U_A, P_A);
1802         mat3_polar_decompose(B, U_B, P_B);
1803
1804         mat3_to_quat(quat_A, U_A);
1805         mat3_to_quat(quat_B, U_B);
1806         interp_qt_qtqt(quat, quat_A, quat_B, t);
1807         quat_to_mat3(U, quat);
1808
1809         for (i = 0; i < 3; i++) {
1810                 interp_v3_v3v3(P[i], P_A[i], P_B[i], t);
1811         }
1812
1813         /* And we reconstruct rot/scale matrix from interpolated polar components */
1814         mul_m3_m3m3(R, U, P);
1815 }
1816
1817 /**
1818  * Complete transform matrix interpolation, based on polar-decomposition-based interpolation from #interp_m3_m3m3.
1819  *
1820  * \param R: Resulting interpolated matrix.
1821  * \param A: Input matrix which is totally effective with `t = 0.0`.
1822  * \param B: Input matrix which is totally effective with `t = 1.0`.
1823  * \param t: Interpolation factor.
1824  */
1825 void interp_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4], const float t)
1826 {
1827         float A3[3][3], B3[3][3], R3[3][3];
1828
1829         /* Location component, linearly interpolated. */
1830         float loc_A[3], loc_B[3], loc[3];
1831
1832         copy_v3_v3(loc_A, A[3]);
1833         copy_v3_v3(loc_B, B[3]);
1834         interp_v3_v3v3(loc, loc_A, loc_B, t);
1835
1836         copy_m3_m4(A3, A);
1837         copy_m3_m4(B3, B);
1838
1839         interp_m3_m3m3(R3, A3, B3, t);
1840
1841         copy_m4_m3(R, R3);
1842         copy_v3_v3(R[3], loc);
1843 }
1844 #endif  /* MATH_STANDALONE */
1845
1846 bool is_negative_m3(const float mat[3][3])
1847 {
1848         float vec[3];
1849         cross_v3_v3v3(vec, mat[0], mat[1]);
1850         return (dot_v3v3(vec, mat[2]) < 0.0f);
1851 }
1852
1853 bool is_negative_m4(const float mat[4][4])
1854 {
1855         float vec[3];
1856         cross_v3_v3v3(vec, mat[0], mat[1]);
1857         return (dot_v3v3(vec, mat[2]) < 0.0f);
1858 }
1859
1860 bool is_zero_m3(const float mat[3][3])
1861 {
1862         return (is_zero_v3(mat[0]) &&
1863                 is_zero_v3(mat[1]) &&
1864                 is_zero_v3(mat[2]));
1865 }
1866 bool is_zero_m4(const float mat[4][4])
1867 {
1868         return (is_zero_v4(mat[0]) &&
1869                 is_zero_v4(mat[1]) &&
1870                 is_zero_v4(mat[2]) &&
1871                 is_zero_v4(mat[3]));
1872 }
1873
1874 bool equals_m3m3(const float mat1[3][3], const float mat2[3][3])
1875 {
1876         return (equals_v3v3(mat1[0], mat2[0]) &&
1877                 equals_v3v3(mat1[1], mat2[1]) &&
1878                 equals_v3v3(mat1[2], mat2[2]));
1879 }
1880
1881 bool equals_m4m4(const float mat1[4][4], const float mat2[4][4])
1882 {
1883         return (equals_v4v4(mat1[0], mat2[0]) &&
1884                 equals_v4v4(mat1[1], mat2[1]) &&
1885                 equals_v4v4(mat1[2], mat2[2]) &&
1886                 equals_v4v4(mat1[3], mat2[3]));
1887 }
1888
1889 /* make a 4x4 matrix out of 3 transform components */
1890 /* matrices are made in the order: scale * rot * loc */
1891 /* TODO: need to have a version that allows for rotation order... */
1892
1893 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1894 {
1895         float rmat[3][3], smat[3][3], tmat[3][3];
1896
1897         /* initialize new matrix */
1898         unit_m4(mat);
1899
1900         /* make rotation + scaling part */
1901         eul_to_mat3(rmat, eul);
1902         size_to_mat3(smat, size);
1903         mul_m3_m3m3(tmat, rmat, smat);
1904
1905         /* copy rot/scale part to output matrix*/
1906         copy_m4_m3(mat, tmat);
1907
1908         /* copy location to matrix */
1909         mat[3][0] = loc[0];
1910         mat[3][1] = loc[1];
1911         mat[3][2] = loc[2];
1912 }
1913
1914 /* make a 4x4 matrix out of 3 transform components */
1915
1916 /* matrices are made in the order: scale * rot * loc */
1917 void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder)
1918 {
1919         float rmat[3][3], smat[3][3], tmat[3][3];
1920
1921         /* initialize new matrix */
1922         unit_m4(mat);
1923
1924         /* make rotation + scaling part */
1925         eulO_to_mat3(rmat, eul, rotOrder);
1926         size_to_mat3(smat, size);
1927         mul_m3_m3m3(tmat, rmat, smat);
1928
1929         /* copy rot/scale part to output matrix*/
1930         copy_m4_m3(mat, tmat);
1931
1932         /* copy location to matrix */
1933         mat[3][0] = loc[0];
1934         mat[3][1] = loc[1];
1935         mat[3][2] = loc[2];
1936 }
1937
1938
1939 /* make a 4x4 matrix out of 3 transform components */
1940
1941 /* matrices are made in the order: scale * rot * loc */
1942 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1943 {
1944         float rmat[3][3], smat[3][3], tmat[3][3];
1945
1946         /* initialize new matrix */
1947         unit_m4(mat);
1948
1949         /* make rotation + scaling part */
1950         quat_to_mat3(rmat, quat);
1951         size_to_mat3(smat, size);
1952         mul_m3_m3m3(tmat, rmat, smat);
1953
1954         /* copy rot/scale part to output matrix*/
1955         copy_m4_m3(mat, tmat);
1956
1957         /* copy location to matrix */
1958         mat[3][0] = loc[0];
1959         mat[3][1] = loc[1];
1960         mat[3][2] = loc[2];
1961 }
1962
1963 void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float axis[3], const float angle, const float size[3])
1964 {
1965         float q[4];
1966         axis_angle_to_quat(q, axis, angle);
1967         loc_quat_size_to_mat4(mat, loc, q, size);
1968 }
1969
1970 /*********************************** Other ***********************************/
1971
1972 void print_m3(const char *str, const float m[3][3])
1973 {
1974         printf("%s\n", str);
1975         printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
1976         printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
1977         printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
1978         printf("\n");
1979 }
1980
1981 void print_m4(const char *str, const float m[4][4])
1982 {
1983         printf("%s\n", str);
1984         printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
1985         printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
1986         printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
1987         printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
1988         printf("\n");
1989 }
1990
1991 /*********************************** SVD ************************************
1992  * from TNT matrix library
1993  *
1994  * Compute the Single Value Decomposition of an arbitrary matrix A
1995  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1996  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1997  * A = U.W.Vt. From this decomposition it is trivial to compute the
1998  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1999  */
2000
2001 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
2002 {
2003         float A[4][4];
2004         float work1[4], work2[4];
2005         int m = 4;
2006         int n = 4;
2007         int maxiter = 200;
2008         int nu = min_ii(m, n);
2009
2010         float *work = work1;
2011         float *e = work2;
2012         float eps;
2013
2014         int i = 0, j = 0, k = 0, p, pp, iter;
2015
2016         /* Reduce A to bidiagonal form, storing the diagonal elements
2017          * in s and the super-diagonal elements in e. */
2018
2019         int nct = min_ii(m - 1, n);
2020         int nrt = max_ii(0, min_ii(n - 2, m));
2021
2022         copy_m4_m4(A, A_);
2023         zero_m4(U);
2024         zero_v4(s);
2025
2026         for (k = 0; k < max_ii(nct, nrt); k++) {
2027                 if (k < nct) {
2028
2029                         /* Compute the transformation for the k-th column and
2030                          * place the k-th diagonal in s[k].
2031                          * Compute 2-norm of k-th column without under/overflow. */
2032                         s[k] = 0;
2033                         for (i = k; i < m; i++) {
2034                                 s[k] = hypotf(s[k], A[i][k]);
2035                         }
2036                         if (s[k] != 0.0f) {
2037                                 float invsk;
2038                                 if (A[k][k] < 0.0f) {
2039                                         s[k] = -s[k];
2040                                 }
2041                                 invsk = 1.0f / s[k];
2042                                 for (i = k; i < m; i++) {
2043                                         A[i][k] *= invsk;
2044                                 }
2045                                 A[k][k] += 1.0f;
2046                         }
2047                         s[k] = -s[k];
2048                 }
2049                 for (j = k + 1; j < n; j++) {
2050                         if ((k < nct) && (s[k] != 0.0f)) {
2051
2052                                 /* Apply the transformation. */
2053
2054                                 float t = 0;
2055                                 for (i = k; i < m; i++) {
2056                                         t += A[i][k] * A[i][j];
2057                                 }
2058                                 t = -t / A[k][k];
2059                                 for (i = k; i < m; i++) {
2060                                         A[i][j] += t * A[i][k];
2061                                 }
2062                         }
2063
2064                         /* Place the k-th row of A into e for the */
2065                         /* subsequent calculation of the row transformation. */
2066
2067                         e[j] = A[k][j];
2068                 }
2069                 if (k < nct) {
2070
2071                         /* Place the transformation in U for subsequent back
2072                          * multiplication. */
2073
2074                         for (i = k; i < m; i++)
2075                                 U[i][k] = A[i][k];
2076                 }
2077                 if (k < nrt) {
2078
2079                         /* Compute the k-th row transformation and place the
2080                          * k-th super-diagonal in e[k].
2081                          * Compute 2-norm without under/overflow. */
2082                         e[k] = 0;
2083                         for (i = k + 1; i < n; i++) {
2084                                 e[k] = hypotf(e[k], e[i]);
2085                         }
2086                         if (e[k] != 0.0f) {
2087                                 float invek;
2088                                 if (e[k + 1] < 0.0f) {
2089                                         e[k] = -e[k];
2090                                 }
2091                                 invek = 1.0f / e[k];
2092                                 for (i = k + 1; i < n; i++) {
2093                                         e[i] *= invek;
2094                                 }
2095                                 e[k + 1] += 1.0f;
2096                         }
2097                         e[k] = -e[k];
2098                         if ((k + 1 < m) & (e[k] != 0.0f)) {
2099                                 float invek1;
2100
2101                                 /* Apply the transformation. */
2102
2103                                 for (i = k + 1; i < m; i++) {
2104                                         work[i] = 0.0f;
2105                                 }
2106                                 for (j = k + 1; j < n; j++) {
2107                                         for (i = k + 1; i < m; i++) {
2108                                                 work[i] += e[j] * A[i][j];
2109                                         }
2110                                 }
2111                                 invek1 = 1.0f / e[k + 1];
2112                                 for (j = k + 1; j < n; j++) {
2113                                         float t = -e[j] * invek1;
2114                                         for (i = k + 1; i < m; i++) {
2115                                                 A[i][j] += t * work[i];
2116                                         }
2117                                 }
2118                         }
2119
2120                         /* Place the transformation in V for subsequent
2121                          * back multiplication. */
2122
2123                         for (i = k + 1; i < n; i++)
2124                                 V[i][k] = e[i];
2125                 }
2126         }
2127
2128         /* Set up the final bidiagonal matrix or order p. */
2129
2130         p = min_ii(n, m + 1);
2131         if (nct < n) {
2132                 s[nct] = A[nct][nct];
2133         }
2134         if (m < p) {
2135                 s[p - 1] = 0.0f;
2136         }
2137         if (nrt + 1 < p) {
2138                 e[nrt] = A[nrt][p - 1];
2139         }
2140         e[p - 1] = 0.0f;
2141
2142         /* If required, generate U. */
2143
2144         for (j = nct; j < nu; j++) {
2145                 for (i = 0; i < m; i++) {
2146                         U[i][j] = 0.0f;
2147                 }
2148                 U[j][j] = 1.0f;
2149         }
2150         for (k = nct - 1; k >= 0; k--) {
2151                 if (s[k] != 0.0f) {
2152                         for (j = k + 1; j < nu; j++) {
2153                                 float t = 0;
2154                                 for (i = k; i < m; i++) {
2155                                         t += U[i][k] * U[i][j];
2156                                 }
2157                                 t = -t / U[k][k];
2158                                 for (i = k; i < m; i++) {
2159                                         U[i][j] += t * U[i][k];
2160                                 }
2161                         }
2162                         for (i = k; i < m; i++) {
2163                                 U[i][k] = -U[i][k];
2164                         }
2165                         U[k][k] = 1.0f + U[k][k];
2166                         for (i = 0; i < k - 1; i++) {
2167                                 U[i][k] = 0.0f;
2168                         }
2169                 }
2170                 else {
2171                         for (i = 0; i < m; i++) {
2172                                 U[i][k] = 0.0f;
2173                         }
2174                         U[k][k] = 1.0f;
2175                 }
2176         }
2177
2178         /* If required, generate V. */
2179
2180         for (k = n - 1; k >= 0; k--) {
2181                 if ((k < nrt) & (e[k] != 0.0f)) {
2182                         for (j = k + 1; j < nu; j++) {
2183                                 float t = 0;
2184                                 for (i = k + 1; i < n; i++) {
2185                                         t += V[i][k] * V[i][j];
2186                                 }
2187                                 t = -t / V[k + 1][k];
2188                                 for (i = k + 1; i < n; i++) {
2189                                         V[i][j] += t * V[i][k];
2190                                 }
2191                         }
2192                 }
2193                 for (i = 0; i < n; i++) {
2194                         V[i][k] = 0.0f;
2195                 }
2196                 V[k][k] = 1.0f;
2197         }
2198
2199         /* Main iteration loop for the singular values. */
2200
2201         pp = p - 1;
2202         iter = 0;
2203         eps = powf(2.0f, -52.0f);
2204         while (p > 0) {
2205                 int kase = 0;
2206
2207                 /* Test for maximum iterations to avoid infinite loop */
2208                 if (maxiter == 0)
2209                         break;
2210                 maxiter--;
2211
2212                 /* This section of the program inspects for
2213                  * negligible elements in the s and e arrays.  On
2214                  * completion the variables kase and k are set as follows.
2215                  *
2216                  * kase = 1: if s(p) and e[k - 1] are negligible and k<p
2217                  * kase = 2: if s(k) is negligible and k<p
2218                  * kase = 3: if e[k - 1] is negligible, k<p, and
2219                  *              s(k), ..., s(p) are not negligible (qr step).
2220                  * kase = 4: if e(p - 1) is negligible (convergence). */
2221
2222                 for (k = p - 2; k >= -1; k--) {
2223                         if (k == -1) {
2224                                 break;
2225                         }
2226                         if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
2227                                 e[k] = 0.0f;
2228                                 break;
2229                         }
2230                 }
2231                 if (k == p - 2) {
2232                         kase = 4;
2233                 }
2234                 else {
2235                         int ks;
2236                         for (ks = p - 1; ks >= k; ks--) {
2237                                 float t;
2238                                 if (ks == k) {
2239                                         break;
2240                                 }
2241                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
2242                                     (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
2243                                 if (fabsf(s[ks]) <= eps * t) {
2244                                         s[ks] = 0.0f;
2245                                         break;
2246                                 }
2247                         }
2248                         if (ks == k) {
2249                                 kase = 3;
2250                         }
2251                         else if (ks == p - 1) {
2252                                 kase = 1;
2253                         }
2254                         else {
2255                                 kase = 2;
2256                                 k = ks;
2257                         }
2258                 }
2259                 k++;
2260
2261                 /* Perform the task indicated by kase. */
2262
2263                 switch (kase) {
2264
2265                         /* Deflate negligible s(p). */
2266
2267                         case 1:
2268                         {
2269                                 float f = e[p - 2];
2270                                 e[p - 2] = 0.0f;
2271                                 for (j = p - 2; j >= k; j--) {
2272                                         float t = hypotf(s[j], f);
2273                                         float invt = 1.0f / t;
2274                                         float cs = s[j] * invt;
2275                                         float sn = f * invt;
2276                                         s[j] = t;
2277                                         if (j != k) {
2278                                                 f = -sn * e[j - 1];
2279                                                 e[j - 1] = cs * e[j - 1];
2280                                         }
2281
2282                                         for (i = 0; i < n; i++) {
2283                                                 t = cs * V[i][j] + sn * V[i][p - 1];
2284                                                 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
2285                                                 V[i][j] = t;
2286                                         }
2287                                 }
2288                                 break;
2289                         }
2290
2291                         /* Split at negligible s(k). */
2292
2293                         case 2:
2294                         {
2295                                 float f = e[k - 1];
2296                                 e[k - 1] = 0.0f;
2297                                 for (j = k; j < p; j++) {
2298                                         float t = hypotf(s[j], f);
2299                                         float invt = 1.0f / t;
2300                                         float cs = s[j] * invt;
2301                                         float sn = f * invt;
2302                                         s[j] = t;
2303                                         f = -sn * e[j];
2304                                         e[j] = cs * e[j];
2305
2306                                         for (i = 0; i < m; i++) {
2307                                                 t = cs * U[i][j] + sn * U[i][k - 1];
2308                                                 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
2309                                                 U[i][j] = t;
2310                                         }
2311                                 }
2312                                 break;
2313                         }
2314
2315                         /* Perform one qr step. */
2316
2317                         case 3:
2318                         {
2319
2320                                 /* Calculate the shift. */
2321
2322                                 float scale = max_ff(max_ff(max_ff(max_ff(
2323                                                    fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
2324                                                    fabsf(s[k])), fabsf(e[k]));
2325                                 float invscale = 1.0f / scale;
2326                                 float sp = s[p - 1] * invscale;
2327                                 float spm1 = s[p - 2] * invscale;
2328                                 float epm1 = e[p - 2] * invscale;
2329                                 float sk = s[k] * invscale;
2330                                 float ek = e[k] * invscale;
2331                                 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
2332                                 float c = (sp * epm1) * (sp * epm1);
2333                                 float shift = 0.0f;
2334                                 float f, g;
2335                                 if ((b != 0.0f) || (c != 0.0f)) {
2336                                         shift = sqrtf(b * b + c);
2337                                         if (b < 0.0f) {
2338                                                 shift = -shift;
2339                                         }
2340                                         shift = c / (b + shift);
2341                                 }
2342                                 f = (sk + sp) * (sk - sp) + shift;
2343                                 g = sk * ek;
2344
2345                                 /* Chase zeros. */
2346
2347                                 for (j = k; j < p - 1; j++) {
2348                                         float t = hypotf(f, g);
2349                                         /* division by zero checks added to avoid NaN (brecht) */
2350                                         float cs = (t == 0.0f) ? 0.0f : f / t;
2351                                         float sn = (t == 0.0f) ? 0.0f : g / t;
2352                                         if (j != k) {
2353                                                 e[j - 1] = t;
2354                                         }
2355                                         f = cs * s[j] + sn * e[j];
2356                                         e[j] = cs * e[j] - sn * s[j];
2357                                         g = sn * s[j + 1];
2358                                         s[j + 1] = cs * s[j + 1];
2359
2360                                         for (i = 0; i < n; i++) {
2361                                                 t = cs * V[i][j] + sn * V[i][j + 1];
2362                                                 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
2363                                                 V[i][j] = t;
2364                                         }
2365
2366                                         t = hypotf(f, g);
2367                                         /* division by zero checks added to avoid NaN (brecht) */
2368                                         cs = (t == 0.0f) ? 0.0f : f / t;
2369                                         sn = (t == 0.0f) ? 0.0f : g / t;
2370                                         s[j] = t;
2371                                         f = cs * e[j] + sn * s[j + 1];
2372                                         s[j + 1] = -sn * e[j] + cs * s[j + 1];
2373                                         g = sn * e[j + 1];
2374                                         e[j + 1] = cs * e[j + 1];
2375                                         if (j < m - 1) {
2376                                                 for (i = 0; i < m; i++) {
2377                                                         t = cs * U[i][j] + sn * U[i][j + 1];
2378                                                         U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
2379                                                         U[i][j] = t;
2380                                                 }
2381                                         }
2382                                 }
2383                                 e[p - 2] = f;
2384                                 iter = iter + 1;
2385                                 break;
2386                         }
2387                         /* Convergence. */
2388
2389                         case 4:
2390                         {
2391
2392                                 /* Make the singular values positive. */
2393
2394                                 if (s[k] <= 0.0f) {
2395                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
2396
2397                                         for (i = 0; i <= pp; i++)
2398                                                 V[i][k] = -V[i][k];
2399                                 }
2400
2401                                 /* Order the singular values. */
2402
2403                                 while (k < pp) {
2404                                         float t;
2405                                         if (s[k] >= s[k + 1]) {
2406                                                 break;
2407                                         }
2408                                         t = s[k];
2409                                         s[k] = s[k + 1];
2410                                         s[k + 1] = t;
2411                                         if (k < n - 1) {
2412                                                 for (i = 0; i < n; i++) {
2413                                                         t = V[i][k + 1];
2414                                                         V[i][k + 1] = V[i][k];
2415                                                         V[i][k] = t;
2416                                                 }
2417                                         }
2418                                         if (k < m - 1) {
2419                                                 for (i = 0; i < m; i++) {
2420                                                         t = U[i][k + 1];
2421                                                         U[i][k + 1] = U[i][k];
2422                                                         U[i][k] = t;
2423                                                 }
2424                                         }
2425                                         k++;
2426                                 }
2427                                 iter = 0;
2428                                 p--;
2429                                 break;
2430                         }
2431                 }
2432         }
2433 }
2434
2435 void pseudoinverse_m4_m4(float Ainv[4][4], const float A_[4][4], float epsilon)
2436 {
2437         /* compute Moore-Penrose pseudo inverse of matrix, singular values
2438          * below epsilon are ignored for stability (truncated SVD) */
2439         float A[4][4], V[4][4], W[4], Wm[4][4], U[4][4];
2440         int i;
2441
2442         transpose_m4_m4(A, A_);
2443         svd_m4(V, W, U, A);
2444         transpose_m4(U);
2445         transpose_m4(V);
2446
2447         zero_m4(Wm);
2448         for (i = 0; i < 4; i++)
2449                 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
2450
2451         transpose_m4(V);
2452
2453         mul_m4_series(Ainv, U, Wm, V);
2454 }
2455
2456 void pseudoinverse_m3_m3(float Ainv[3][3], const float A[3][3], float epsilon)
2457 {
2458         /* try regular inverse when possible, otherwise fall back to slow svd */
2459         if (!invert_m3_m3(Ainv, A)) {
2460                 float tmp[4][4], tmpinv[4][4];
2461
2462                 copy_m4_m3(tmp, A);
2463                 pseudoinverse_m4_m4(tmpinv, tmp, epsilon);
2464                 copy_m3_m4(Ainv, tmpinv);
2465         }
2466 }
2467
2468 bool has_zero_axis_m4(const float matrix[4][4])
2469 {
2470         return len_squared_v3(matrix[0]) < FLT_EPSILON ||
2471                len_squared_v3(matrix[1]) < FLT_EPSILON ||
2472                len_squared_v3(matrix[2]) < FLT_EPSILON;
2473 }
2474
2475 void invert_m4_m4_safe(float Ainv[4][4], const float A[4][4])
2476 {
2477         if (!invert_m4_m4(Ainv, A)) {
2478                 float Atemp[4][4];
2479
2480                 copy_m4_m4(Atemp, A);
2481
2482                 /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should
2483                  * never be in this situation, but try to invert it anyway with tweak.
2484                  */
2485                 Atemp[0][0] += 1e-8f;
2486                 Atemp[1][1] += 1e-8f;
2487                 Atemp[2][2] += 1e-8f;
2488
2489                 if (!invert_m4_m4(Ainv, Atemp)) {
2490                         unit_m4(Ainv);
2491                 }
2492         }
2493 }
2494
2495 /**
2496  * SpaceTransform struct encapsulates all needed data to convert between two coordinate spaces
2497  * (where conversion can be represented by a matrix multiplication).
2498  *
2499  * A SpaceTransform is initialized using:
2500  *   BLI_SPACE_TRANSFORM_SETUP(&data,  ob1, ob2)
2501  *
2502  * After that the following calls can be used:
2503  *   BLI_space_transform_apply(&data, co);  // converts a coordinate in ob1 space to the corresponding ob2 space
2504  *   BLI_space_transform_invert(&data, co);  // converts a coordinate in ob2 space to the corresponding ob1 space
2505  *
2506  * Same concept as BLI_space_transform_apply and BLI_space_transform_invert, but no is normalized after conversion
2507  * (and not translated at all!):
2508  *   BLI_space_transform_apply_normal(&data, no);
2509  *   BLI_space_transform_invert_normal(&data, no);
2510  */
2511
2512 /**
2513  * Global-invariant transform.
2514  *
2515  * This defines a matrix transforming a point in local space to a point in target space such that its global
2516  * coordinates remain unchanged.
2517  *
2518  * In other words, if we have a global point P with local coordinates (x, y, z) and global coordinates (X, Y, Z),
2519  * this defines a transform matrix TM such that (x', y', z') = TM * (x, y, z)
2520  * where (x', y', z') are the coordinates of P' in target space such that it keeps (X, Y, Z) coordinates in global space.
2521  */
2522 void BLI_space_transform_from_matrices(SpaceTransform *data, const float local[4][4], const float target[4][4])
2523 {
2524         float itarget[4][4];
2525         invert_m4_m4(itarget, target);
2526         mul_m4_m4m4(data->local2target, itarget, local);
2527         invert_m4_m4(data->target2local, data->local2target);
2528 }
2529
2530 /**
2531  * Local-invariant transform.
2532  *
2533  * This defines a matrix transforming a point in global space such that its local coordinates
2534  * (from local space to target space) remain unchanged.
2535  *
2536  * In other words, if we have a local point p with local coordinates (x, y, z) and global coordinates (X, Y, Z),
2537  * this defines a transform matrix TM such that (X', Y', Z') = TM * (X, Y, Z)
2538  * where (X', Y', Z') are the coordinates of p' in global space such that it keeps (x, y, z) coordinates in target space.
2539  */
2540 void BLI_space_transform_global_from_matrices(SpaceTransform *data, const float local[4][4], const float target[4][4])
2541 {
2542         float ilocal[4][4];
2543         invert_m4_m4(ilocal, local);
2544         mul_m4_m4m4(data->local2target, target, ilocal);
2545         invert_m4_m4(data->target2local, data->local2target);
2546 }
2547
2548 void BLI_space_transform_apply(const SpaceTransform *data, float co[3])
2549 {
2550         mul_v3_m4v3(co, ((SpaceTransform *)data)->local2target, co);
2551 }
2552
2553 void BLI_space_transform_invert(const SpaceTransform *data, float co[3])
2554 {
2555         mul_v3_m4v3(co, ((SpaceTransform *)data)->target2local, co);
2556 }
2557
2558 void BLI_space_transform_apply_normal(const SpaceTransform *data, float no[3])
2559 {
2560         mul_mat3_m4_v3(((SpaceTransform *)data)->local2target, no);
2561         normalize_v3(no);
2562 }
2563
2564 void BLI_space_transform_invert_normal(const SpaceTransform *data, float no[3])
2565 {
2566         mul_mat3_m4_v3(((SpaceTransform *)data)->target2local, no);
2567         normalize_v3(no);
2568 }