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