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