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