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