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