Code Cleanup: use strict flags for math lib, add inline declarations
[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 /* same as mul_m4_v3() but doesnt apply translation component */
402 void mul_mat3_m4_v3(float mat[4][4], float vec[3])
403 {
404         float x, y;
405
406         x = vec[0];
407         y = vec[1];
408         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
409         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
410         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
411 }
412
413 void mul_project_m4_v3(float mat[4][4], float vec[3])
414 {
415         const float w = mul_project_m4_v3_zfac(mat, vec);
416         mul_m4_v3(mat, vec);
417
418         vec[0] /= w;
419         vec[1] /= w;
420         vec[2] /= w;
421 }
422
423 void mul_v2_project_m4_v3(float r[2], float mat[4][4], const float vec[3])
424 {
425         const float w = mul_project_m4_v3_zfac(mat, vec);
426         mul_v2_m4v3(r, mat, vec);
427
428         r[0] /= w;
429         r[1] /= w;
430 }
431
432 void mul_v4_m4v4(float r[4], float mat[4][4], const float v[4])
433 {
434         float x, y, z;
435
436         x = v[0];
437         y = v[1];
438         z = v[2];
439
440         r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
441         r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
442         r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3];
443         r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3];
444 }
445
446 void mul_m4_v4(float mat[4][4], float r[4])
447 {
448         mul_v4_m4v4(r, mat, r);
449 }
450
451 void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4])
452 {
453         double x, y, z;
454
455         x = v[0];
456         y = v[1];
457         z = v[2];
458
459         r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
460         r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
461         r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3];
462         r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3];
463 }
464
465 void mul_m4_v4d(float mat[4][4], double r[4])
466 {
467         mul_v4d_m4v4d(r, mat, r);
468 }
469
470 void mul_v3_m3v3(float r[3], float M[3][3], const float a[3])
471 {
472         BLI_assert(r != a);
473
474         r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
475         r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
476         r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
477 }
478
479 void mul_v2_m3v3(float r[2], float M[3][3], const float a[3])
480 {
481         BLI_assert(r != a);
482
483         r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
484         r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
485 }
486
487 void mul_m3_v3(float M[3][3], float r[3])
488 {
489         float tmp[3];
490
491         mul_v3_m3v3(tmp, M, r);
492         copy_v3_v3(r, tmp);
493 }
494
495 void mul_transposed_m3_v3(float mat[3][3], float vec[3])
496 {
497         float x, y;
498
499         x = vec[0];
500         y = vec[1];
501         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
502         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
503         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
504 }
505
506 void mul_transposed_mat3_m4_v3(float mat[4][4], float vec[3])
507 {
508         float x, y;
509
510         x = vec[0];
511         y = vec[1];
512         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
513         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
514         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
515 }
516
517
518 void mul_m3_fl(float m[3][3], float f)
519 {
520         int i, j;
521
522         for (i = 0; i < 3; i++)
523                 for (j = 0; j < 3; j++)
524                         m[i][j] *= f;
525 }
526
527 void mul_m4_fl(float m[4][4], float f)
528 {
529         int i, j;
530
531         for (i = 0; i < 4; i++)
532                 for (j = 0; j < 4; j++)
533                         m[i][j] *= f;
534 }
535
536 void mul_mat3_m4_fl(float m[4][4], float f)
537 {
538         int i, j;
539
540         for (i = 0; i < 3; i++)
541                 for (j = 0; j < 3; j++)
542                         m[i][j] *= f;
543 }
544
545 void mul_m3_v3_double(float mat[3][3], double vec[3])
546 {
547         double x, y;
548
549         x = vec[0];
550         y = vec[1];
551         vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
552         vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
553         vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
554 }
555
556 void add_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
557 {
558         int i, j;
559
560         for (i = 0; i < 3; i++)
561                 for (j = 0; j < 3; j++)
562                         m1[i][j] = m2[i][j] + m3[i][j];
563 }
564
565 void add_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
566 {
567         int i, j;
568
569         for (i = 0; i < 4; i++)
570                 for (j = 0; j < 4; j++)
571                         m1[i][j] = m2[i][j] + m3[i][j];
572 }
573
574 void sub_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
575 {
576         int i, j;
577
578         for (i = 0; i < 3; i++)
579                 for (j = 0; j < 3; j++)
580                         m1[i][j] = m2[i][j] - m3[i][j];
581 }
582
583 void sub_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
584 {
585         int i, j;
586
587         for (i = 0; i < 4; i++)
588                 for (j = 0; j < 4; j++)
589                         m1[i][j] = m2[i][j] - m3[i][j];
590 }
591
592 float determinant_m3_array(float m[3][3])
593 {
594         return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
595                 m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
596                 m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
597 }
598
599 int invert_m3_ex(float m[3][3], const float epsilon)
600 {
601         float tmp[3][3];
602         int success;
603
604         success = invert_m3_m3_ex(tmp, m, epsilon);
605         copy_m3_m3(m, tmp);
606
607         return success;
608 }
609
610 int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon)
611 {
612         float det;
613         int a, b, success;
614
615         BLI_assert(epsilon >= 0.0f);
616
617         /* calc adjoint */
618         adjoint_m3_m3(m1, m2);
619
620         /* then determinant old matrix! */
621         det = determinant_m3_array(m2);
622
623         success = (fabsf(det) > epsilon);
624
625         if (LIKELY(det != 0.0f)) {
626                 det = 1.0f / det;
627                 for (a = 0; a < 3; a++) {
628                         for (b = 0; b < 3; b++) {
629                                 m1[a][b] *= det;
630                         }
631                 }
632         }
633         return success;
634 }
635
636 int invert_m3(float m[3][3])
637 {
638         float tmp[3][3];
639         int success;
640
641         success = invert_m3_m3(tmp, m);
642         copy_m3_m3(m, tmp);
643
644         return success;
645 }
646
647 int invert_m3_m3(float m1[3][3], float m2[3][3])
648 {
649         float det;
650         int a, b, success;
651
652         /* calc adjoint */
653         adjoint_m3_m3(m1, m2);
654
655         /* then determinant old matrix! */
656         det = determinant_m3_array(m2);
657
658         success = (det != 0.0f);
659
660         if (LIKELY(det != 0.0f)) {
661                 det = 1.0f / det;
662                 for (a = 0; a < 3; a++) {
663                         for (b = 0; b < 3; b++) {
664                                 m1[a][b] *= det;
665                         }
666                 }
667         }
668
669         return success;
670 }
671
672 int invert_m4(float m[4][4])
673 {
674         float tmp[4][4];
675         int success;
676
677         success = invert_m4_m4(tmp, m);
678         copy_m4_m4(m, tmp);
679
680         return success;
681 }
682
683 /*
684  * invertmat -
685  *      computes the inverse of mat and puts it in inverse.  Returns
686  *  TRUE on success (i.e. can always find a pivot) and FALSE on failure.
687  *  Uses Gaussian Elimination with partial (maximal column) pivoting.
688  *
689  *                                      Mark Segal - 1992
690  */
691
692 int invert_m4_m4(float inverse[4][4], float mat[4][4])
693 {
694         int i, j, k;
695         double temp;
696         float tempmat[4][4];
697         float max;
698         int maxj;
699
700         BLI_assert(inverse != mat);
701
702         /* Set inverse to identity */
703         for (i = 0; i < 4; i++)
704                 for (j = 0; j < 4; j++)
705                         inverse[i][j] = 0;
706         for (i = 0; i < 4; i++)
707                 inverse[i][i] = 1;
708
709         /* Copy original matrix so we don't mess it up */
710         for (i = 0; i < 4; i++)
711                 for (j = 0; j < 4; j++)
712                         tempmat[i][j] = mat[i][j];
713
714         for (i = 0; i < 4; i++) {
715                 /* Look for row with max pivot */
716                 max = fabsf(tempmat[i][i]);
717                 maxj = i;
718                 for (j = i + 1; j < 4; j++) {
719                         if (fabsf(tempmat[j][i]) > max) {
720                                 max = fabsf(tempmat[j][i]);
721                                 maxj = j;
722                         }
723                 }
724                 /* Swap rows if necessary */
725                 if (maxj != i) {
726                         for (k = 0; k < 4; k++) {
727                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
728                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
729                         }
730                 }
731
732                 temp = tempmat[i][i];
733                 if (temp == 0)
734                         return 0;  /* No non-zero pivot */
735                 for (k = 0; k < 4; k++) {
736                         tempmat[i][k] = (float)((double)tempmat[i][k] / temp);
737                         inverse[i][k] = (float)((double)inverse[i][k] / temp);
738                 }
739                 for (j = 0; j < 4; j++) {
740                         if (j != i) {
741                                 temp = tempmat[j][i];
742                                 for (k = 0; k < 4; k++) {
743                                         tempmat[j][k] -= (float)((double)tempmat[i][k] * temp);
744                                         inverse[j][k] -= (float)((double)inverse[i][k] * temp);
745                                 }
746                         }
747                 }
748         }
749         return 1;
750 }
751
752 /****************************** Linear Algebra *******************************/
753
754 void transpose_m3(float mat[3][3])
755 {
756         float t;
757
758         t = mat[0][1];
759         mat[0][1] = mat[1][0];
760         mat[1][0] = t;
761         t = mat[0][2];
762         mat[0][2] = mat[2][0];
763         mat[2][0] = t;
764         t = mat[1][2];
765         mat[1][2] = mat[2][1];
766         mat[2][1] = t;
767 }
768
769 void transpose_m4(float mat[4][4])
770 {
771         float t;
772
773         t = mat[0][1];
774         mat[0][1] = mat[1][0];
775         mat[1][0] = t;
776         t = mat[0][2];
777         mat[0][2] = mat[2][0];
778         mat[2][0] = t;
779         t = mat[0][3];
780         mat[0][3] = mat[3][0];
781         mat[3][0] = t;
782
783         t = mat[1][2];
784         mat[1][2] = mat[2][1];
785         mat[2][1] = t;
786         t = mat[1][3];
787         mat[1][3] = mat[3][1];
788         mat[3][1] = t;
789
790         t = mat[2][3];
791         mat[2][3] = mat[3][2];
792         mat[3][2] = t;
793 }
794
795 int compare_m4m4(float mat1[4][4], float mat2[4][4], float limit)
796 {
797         if (compare_v4v4(mat1[0], mat2[0], limit))
798                 if (compare_v4v4(mat1[1], mat2[1], limit))
799                         if (compare_v4v4(mat1[2], mat2[2], limit))
800                                 if (compare_v4v4(mat1[3], mat2[3], limit))
801                                         return 1;
802         return 0;
803 }
804
805 void orthogonalize_m3(float mat[3][3], int axis)
806 {
807         float size[3];
808         mat3_to_size(size, mat);
809         normalize_v3(mat[axis]);
810         switch (axis) {
811                 case 0:
812                         if (dot_v3v3(mat[0], mat[1]) < 1) {
813                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
814                                 normalize_v3(mat[2]);
815                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
816                         }
817                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
818                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
819                                 normalize_v3(mat[1]);
820                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
821                         }
822                         else {
823                                 float vec[3];
824
825                                 vec[0] = mat[0][1];
826                                 vec[1] = mat[0][2];
827                                 vec[2] = mat[0][0];
828
829                                 cross_v3_v3v3(mat[2], mat[0], vec);
830                                 normalize_v3(mat[2]);
831                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
832                         }
833                         break;
834                 case 1:
835                         if (dot_v3v3(mat[1], mat[0]) < 1) {
836                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
837                                 normalize_v3(mat[2]);
838                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
839                         }
840                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
841                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
842                                 normalize_v3(mat[0]);
843                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
844                         }
845                         else {
846                                 float vec[3];
847
848                                 vec[0] = mat[1][1];
849                                 vec[1] = mat[1][2];
850                                 vec[2] = mat[1][0];
851
852                                 cross_v3_v3v3(mat[0], mat[1], vec);
853                                 normalize_v3(mat[0]);
854                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
855                         }
856                         break;
857                 case 2:
858                         if (dot_v3v3(mat[2], mat[0]) < 1) {
859                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
860                                 normalize_v3(mat[1]);
861                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
862                         }
863                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
864                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
865                                 normalize_v3(mat[0]);
866                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
867                         }
868                         else {
869                                 float vec[3];
870
871                                 vec[0] = mat[2][1];
872                                 vec[1] = mat[2][2];
873                                 vec[2] = mat[2][0];
874
875                                 cross_v3_v3v3(mat[0], vec, mat[2]);
876                                 normalize_v3(mat[0]);
877                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
878                         }
879                         break;
880                 default:
881                         BLI_assert(0);
882                         break;
883         }
884         mul_v3_fl(mat[0], size[0]);
885         mul_v3_fl(mat[1], size[1]);
886         mul_v3_fl(mat[2], size[2]);
887 }
888
889 void orthogonalize_m4(float mat[4][4], int axis)
890 {
891         float size[3];
892         mat4_to_size(size, mat);
893         normalize_v3(mat[axis]);
894         switch (axis) {
895                 case 0:
896                         if (dot_v3v3(mat[0], mat[1]) < 1) {
897                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
898                                 normalize_v3(mat[2]);
899                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
900                         }
901                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
902                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
903                                 normalize_v3(mat[1]);
904                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
905                         }
906                         else {
907                                 float vec[3];
908
909                                 vec[0] = mat[0][1];
910                                 vec[1] = mat[0][2];
911                                 vec[2] = mat[0][0];
912
913                                 cross_v3_v3v3(mat[2], mat[0], vec);
914                                 normalize_v3(mat[2]);
915                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
916                         }
917                         break;
918                 case 1:
919                         if (dot_v3v3(mat[1], mat[0]) < 1) {
920                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
921                                 normalize_v3(mat[2]);
922                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
923                         }
924                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
925                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
926                                 normalize_v3(mat[0]);
927                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
928                         }
929                         else {
930                                 float vec[3];
931
932                                 vec[0] = mat[1][1];
933                                 vec[1] = mat[1][2];
934                                 vec[2] = mat[1][0];
935
936                                 cross_v3_v3v3(mat[0], mat[1], vec);
937                                 normalize_v3(mat[0]);
938                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
939                         }
940                         break;
941                 case 2:
942                         if (dot_v3v3(mat[2], mat[0]) < 1) {
943                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
944                                 normalize_v3(mat[1]);
945                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
946                         }
947                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
948                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
949                                 normalize_v3(mat[0]);
950                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
951                         }
952                         else {
953                                 float vec[3];
954
955                                 vec[0] = mat[2][1];
956                                 vec[1] = mat[2][2];
957                                 vec[2] = mat[2][0];
958
959                                 cross_v3_v3v3(mat[0], vec, mat[2]);
960                                 normalize_v3(mat[0]);
961                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
962                         }
963                         break;
964                 default:
965                         BLI_assert(0);
966                         break;
967         }
968         mul_v3_fl(mat[0], size[0]);
969         mul_v3_fl(mat[1], size[1]);
970         mul_v3_fl(mat[2], size[2]);
971 }
972
973 bool is_orthogonal_m3(float m[3][3])
974 {
975         int i, j;
976
977         for (i = 0; i < 3; i++) {
978                 for (j = 0; j < i; j++) {
979                         if (fabsf(dot_v3v3(m[i], m[j])) > 1.5f * FLT_EPSILON)
980                                 return 0;
981                 }
982         }
983
984         return 1;
985 }
986
987 bool is_orthogonal_m4(float m[4][4])
988 {
989         int i, j;
990
991         for (i = 0; i < 4; i++) {
992                 for (j = 0; j < i; j++) {
993                         if (fabsf(dot_v4v4(m[i], m[j])) > 1.5f * FLT_EPSILON)
994                                 return 0;
995                 }
996
997         }
998
999         return 1;
1000 }
1001
1002 bool is_orthonormal_m3(float m[3][3])
1003 {
1004         if (is_orthogonal_m3(m)) {
1005                 int i;
1006
1007                 for (i = 0; i < 3; i++)
1008                         if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
1009                                 return 0;
1010
1011                 return 1;
1012         }
1013
1014         return 0;
1015 }
1016
1017 bool is_orthonormal_m4(float m[4][4])
1018 {
1019         if (is_orthogonal_m4(m)) {
1020                 int i;
1021
1022                 for (i = 0; i < 4; i++)
1023                         if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
1024                                 return 0;
1025
1026                 return 1;
1027         }
1028
1029         return 0;
1030 }
1031
1032 bool is_uniform_scaled_m3(float m[3][3])
1033 {
1034         const float eps = 1e-7f;
1035         float t[3][3];
1036         float l1, l2, l3, l4, l5, l6;
1037
1038         copy_m3_m3(t, m);
1039         transpose_m3(t);
1040
1041         l1 = len_squared_v3(m[0]);
1042         l2 = len_squared_v3(m[1]);
1043         l3 = len_squared_v3(m[2]);
1044
1045         l4 = len_squared_v3(t[0]);
1046         l5 = len_squared_v3(t[1]);
1047         l6 = len_squared_v3(t[2]);
1048
1049         if (fabsf(l2 - l1) <= eps &&
1050             fabsf(l3 - l1) <= eps &&
1051             fabsf(l4 - l1) <= eps &&
1052             fabsf(l5 - l1) <= eps &&
1053             fabsf(l6 - l1) <= eps)
1054         {
1055                 return 1;
1056         }
1057
1058         return 0;
1059 }
1060
1061 void normalize_m3(float mat[3][3])
1062 {
1063         normalize_v3(mat[0]);
1064         normalize_v3(mat[1]);
1065         normalize_v3(mat[2]);
1066 }
1067
1068 void normalize_m3_m3(float rmat[3][3], float mat[3][3])
1069 {
1070         normalize_v3_v3(rmat[0], mat[0]);
1071         normalize_v3_v3(rmat[1], mat[1]);
1072         normalize_v3_v3(rmat[2], mat[2]);
1073 }
1074
1075 void normalize_m4(float mat[4][4])
1076 {
1077         float len;
1078
1079         len = normalize_v3(mat[0]);
1080         if (len != 0.0f) mat[0][3] /= len;
1081         len = normalize_v3(mat[1]);
1082         if (len != 0.0f) mat[1][3] /= len;
1083         len = normalize_v3(mat[2]);
1084         if (len != 0.0f) mat[2][3] /= len;
1085 }
1086
1087 void normalize_m4_m4(float rmat[4][4], float mat[4][4])
1088 {
1089         copy_m4_m4(rmat, mat);
1090         normalize_m4(rmat);
1091 }
1092
1093 void adjoint_m2_m2(float m1[2][2], float m[2][2])
1094 {
1095         BLI_assert(m1 != m);
1096         m1[0][0] =  m[1][1];
1097         m1[0][1] = -m[0][1];
1098         m1[1][0] = -m[1][0];
1099         m1[1][1] =  m[0][0];
1100 }
1101
1102 void adjoint_m3_m3(float m1[3][3], float m[3][3])
1103 {
1104         BLI_assert(m1 != m);
1105         m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
1106         m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
1107         m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
1108
1109         m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0];
1110         m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
1111         m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0];
1112
1113         m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
1114         m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0];
1115         m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
1116 }
1117
1118 void adjoint_m4_m4(float out[4][4], float in[4][4]) /* out = ADJ(in) */
1119 {
1120         float a1, a2, a3, a4, b1, b2, b3, b4;
1121         float c1, c2, c3, c4, d1, d2, d3, d4;
1122
1123         a1 = in[0][0];
1124         b1 = in[0][1];
1125         c1 = in[0][2];
1126         d1 = in[0][3];
1127
1128         a2 = in[1][0];
1129         b2 = in[1][1];
1130         c2 = in[1][2];
1131         d2 = in[1][3];
1132
1133         a3 = in[2][0];
1134         b3 = in[2][1];
1135         c3 = in[2][2];
1136         d3 = in[2][3];
1137
1138         a4 = in[3][0];
1139         b4 = in[3][1];
1140         c4 = in[3][2];
1141         d4 = in[3][3];
1142
1143
1144         out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1145         out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1146         out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1147         out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1148
1149         out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1150         out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1151         out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1152         out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1153
1154         out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1155         out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1156         out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1157         out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1158
1159         out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1160         out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1161         out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1162         out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1163 }
1164
1165 float determinant_m2(float a, float b, float c, float d)
1166 {
1167
1168         return a * d - b * c;
1169 }
1170
1171 float determinant_m3(float a1, float a2, float a3,
1172                      float b1, float b2, float b3,
1173                      float c1, float c2, float c3)
1174 {
1175         float ans;
1176
1177         ans = (a1 * determinant_m2(b2, b3, c2, c3) -
1178                b1 * determinant_m2(a2, a3, c2, c3) +
1179                c1 * determinant_m2(a2, a3, b2, b3));
1180
1181         return ans;
1182 }
1183
1184 float determinant_m4(float m[4][4])
1185 {
1186         float ans;
1187         float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1188
1189         a1 = m[0][0];
1190         b1 = m[0][1];
1191         c1 = m[0][2];
1192         d1 = m[0][3];
1193
1194         a2 = m[1][0];
1195         b2 = m[1][1];
1196         c2 = m[1][2];
1197         d2 = m[1][3];
1198
1199         a3 = m[2][0];
1200         b3 = m[2][1];
1201         c3 = m[2][2];
1202         d3 = m[2][3];
1203
1204         a4 = m[3][0];
1205         b4 = m[3][1];
1206         c4 = m[3][2];
1207         d4 = m[3][3];
1208
1209         ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1210                b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1211                c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1212                d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1213
1214         return ans;
1215 }
1216
1217 /****************************** Transformations ******************************/
1218
1219 void size_to_mat3(float mat[3][3], const float size[3])
1220 {
1221         mat[0][0] = size[0];
1222         mat[0][1] = 0.0f;
1223         mat[0][2] = 0.0f;
1224         mat[1][1] = size[1];
1225         mat[1][0] = 0.0f;
1226         mat[1][2] = 0.0f;
1227         mat[2][2] = size[2];
1228         mat[2][1] = 0.0f;
1229         mat[2][0] = 0.0f;
1230 }
1231
1232 void size_to_mat4(float mat[4][4], const float size[3])
1233 {
1234         float tmat[3][3];
1235
1236         size_to_mat3(tmat, size);
1237         unit_m4(mat);
1238         copy_m4_m3(mat, tmat);
1239 }
1240
1241 void mat3_to_size(float size[3], float mat[3][3])
1242 {
1243         size[0] = len_v3(mat[0]);
1244         size[1] = len_v3(mat[1]);
1245         size[2] = len_v3(mat[2]);
1246 }
1247
1248 void mat4_to_size(float size[3], float mat[4][4])
1249 {
1250         size[0] = len_v3(mat[0]);
1251         size[1] = len_v3(mat[1]);
1252         size[2] = len_v3(mat[2]);
1253 }
1254
1255 /* this gets the average scale of a matrix, only use when your scaling
1256  * data that has no idea of scale axis, examples are bone-envelope-radius
1257  * and curve radius */
1258 float mat3_to_scale(float mat[3][3])
1259 {
1260         /* unit length vector */
1261         float unit_vec[3];
1262         copy_v3_fl(unit_vec, 0.577350269189626f);
1263         mul_m3_v3(mat, unit_vec);
1264         return len_v3(unit_vec);
1265 }
1266
1267 float mat4_to_scale(float mat[4][4])
1268 {
1269         /* unit length vector */
1270         float unit_vec[3];
1271         copy_v3_fl(unit_vec, 0.577350269189626f);
1272         mul_mat3_m4_v3(mat, unit_vec);
1273         return len_v3(unit_vec);
1274 }
1275
1276 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
1277 {
1278         float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */
1279         float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */
1280
1281         /* rotation & scale are linked, we need to create the mat's
1282          * for these together since they are related. */
1283
1284         /* so scale doesn't interfere with rotation [#24291] */
1285         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1286         normalize_m3_m3(mat3_n, mat3);
1287         if (is_negative_m3(mat3)) {
1288                 negate_v3(mat3_n[0]);
1289                 negate_v3(mat3_n[1]);
1290                 negate_v3(mat3_n[2]);
1291         }
1292
1293         /* rotation */
1294         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1295         copy_m3_m3(rot, mat3_n);
1296
1297         /* scale */
1298         /* note: mat4_to_size(ob->size, mat) fails for negative scale */
1299         invert_m3_m3(imat3_n, mat3_n);
1300         mul_m3_m3m3(mat3, imat3_n, mat3);
1301
1302         size[0] = mat3[0][0];
1303         size[1] = mat3[1][1];
1304         size[2] = mat3[2][2];
1305 }
1306
1307 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[4][4])
1308 {
1309         float mat3[3][3]; /* wmat -> 3x3 */
1310
1311         copy_m3_m4(mat3, wmat);
1312         mat3_to_rot_size(rot, size, mat3);
1313
1314         /* location */
1315         copy_v3_v3(loc, wmat[3]);
1316 }
1317
1318 void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])
1319 {
1320         float mat3[3][3];
1321         float mat3_n[3][3]; /* normalized mat3 */
1322
1323         copy_m3_m4(mat3, wmat);
1324         normalize_m3_m3(mat3_n, mat3);
1325
1326         /* so scale doesn't interfere with rotation [#24291] */
1327         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1328         if (is_negative_m3(mat3)) {
1329                 negate_v3(mat3_n[0]);
1330                 negate_v3(mat3_n[1]);
1331                 negate_v3(mat3_n[2]);
1332         }
1333
1334         mat3_to_quat(quat, mat3_n);
1335         copy_v3_v3(loc, wmat[3]);
1336 }
1337
1338 void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4])
1339 {
1340         float rot[3][3];
1341         mat4_to_loc_rot_size(loc, rot, size, wmat);
1342         mat3_to_quat(quat, rot);
1343 }
1344
1345 void scale_m3_fl(float m[3][3], float scale)
1346 {
1347         m[0][0] = m[1][1] = m[2][2] = scale;
1348         m[0][1] = m[0][2] = 0.0;
1349         m[1][0] = m[1][2] = 0.0;
1350         m[2][0] = m[2][1] = 0.0;
1351 }
1352
1353 void scale_m4_fl(float m[4][4], float scale)
1354 {
1355         m[0][0] = m[1][1] = m[2][2] = scale;
1356         m[3][3] = 1.0;
1357         m[0][1] = m[0][2] = m[0][3] = 0.0;
1358         m[1][0] = m[1][2] = m[1][3] = 0.0;
1359         m[2][0] = m[2][1] = m[2][3] = 0.0;
1360         m[3][0] = m[3][1] = m[3][2] = 0.0;
1361 }
1362
1363 void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
1364 {
1365         mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
1366         mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
1367         mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
1368 }
1369
1370 void rotate_m4(float mat[4][4], const char axis, const float angle)
1371 {
1372         int col;
1373         float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
1374         float cosine, sine;
1375
1376         assert(axis >= 'X' && axis <= 'Z');
1377
1378         cosine = cosf(angle);
1379         sine   = sinf(angle);
1380         switch (axis) {
1381                 case 'X':
1382                         for (col = 0; col < 4; col++)
1383                                 temp[col] = cosine * mat[1][col] + sine * mat[2][col];
1384                         for (col = 0; col < 4; col++) {
1385                                 mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col];
1386                                 mat[1][col] = temp[col];
1387                         }
1388                         break;
1389
1390                 case 'Y':
1391                         for (col = 0; col < 4; col++)
1392                                 temp[col] = cosine * mat[0][col] - sine * mat[2][col];
1393                         for (col = 0; col < 4; col++) {
1394                                 mat[2][col] = sine * mat[0][col] + cosine * mat[2][col];
1395                                 mat[0][col] = temp[col];
1396                         }
1397                         break;
1398
1399                 case 'Z':
1400                         for (col = 0; col < 4; col++)
1401                                 temp[col] = cosine * mat[0][col] + sine * mat[1][col];
1402                         for (col = 0; col < 4; col++) {
1403                                 mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col];
1404                                 mat[0][col] = temp[col];
1405                         }
1406                         break;
1407         }
1408 }
1409
1410 void rotate_m2(float mat[2][2], const float angle)
1411 {
1412         mat[0][0] = mat[1][1] = cosf(angle);
1413         mat[0][1] = sinf(angle);
1414         mat[1][0] = -mat[0][1];
1415 }
1416
1417 /**
1418  * Scale or rotate around a pivot point,
1419  * a convenience function to avoid having to do inline.
1420  *
1421  * Since its common to make a scale/rotation matrix that pivots around an arbitrary point.
1422  *
1423  * Typical use case is to make 3x3 matrix, copy to 4x4, then pass to this function.
1424  */
1425 void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
1426 {
1427         float tmat[4][4];
1428
1429         unit_m4(tmat);
1430
1431         copy_v3_v3(tmat[3], pivot);
1432         mul_m4_m4m4(mat, tmat, mat);
1433
1434         /* invert the matrix */
1435         negate_v3(tmat[3]);
1436         mul_m4_m4m4(mat, mat, tmat);
1437 }
1438
1439 void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const float srcweight)
1440 {
1441         float srot[3][3], drot[3][3];
1442         float squat[4], dquat[4], fquat[4];
1443         float sscale[3], dscale[3], fsize[3];
1444         float rmat[3][3], smat[3][3];
1445
1446         mat3_to_rot_size(drot, dscale, dst);
1447         mat3_to_rot_size(srot, sscale, src);
1448
1449         mat3_to_quat(dquat, drot);
1450         mat3_to_quat(squat, srot);
1451
1452         /* do blending */
1453         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1454         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1455
1456         /* compose new matrix */
1457         quat_to_mat3(rmat, fquat);
1458         size_to_mat3(smat, fsize);
1459         mul_m3_m3m3(out, rmat, smat);
1460 }
1461
1462 void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const float srcweight)
1463 {
1464         float sloc[3], dloc[3], floc[3];
1465         float srot[3][3], drot[3][3];
1466         float squat[4], dquat[4], fquat[4];
1467         float sscale[3], dscale[3], fsize[3];
1468
1469         mat4_to_loc_rot_size(dloc, drot, dscale, dst);
1470         mat4_to_loc_rot_size(sloc, srot, sscale, src);
1471
1472         mat3_to_quat(dquat, drot);
1473         mat3_to_quat(squat, srot);
1474
1475         /* do blending */
1476         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1477         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1478         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1479
1480         /* compose new matrix */
1481         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1482 }
1483
1484 bool is_negative_m3(float mat[3][3])
1485 {
1486         float vec[3];
1487         cross_v3_v3v3(vec, mat[0], mat[1]);
1488         return (dot_v3v3(vec, mat[2]) < 0.0f);
1489 }
1490
1491 bool is_negative_m4(float mat[4][4])
1492 {
1493         float vec[3];
1494         cross_v3_v3v3(vec, mat[0], mat[1]);
1495         return (dot_v3v3(vec, mat[2]) < 0.0f);
1496 }
1497
1498 bool is_zero_m3(float mat[3][3])
1499 {
1500         return (is_zero_v3(mat[0]) &&
1501                 is_zero_v3(mat[1]) &&
1502                 is_zero_v3(mat[2]));
1503 }
1504 bool is_zero_m4(float mat[4][4])
1505 {
1506         return (is_zero_v4(mat[0]) &&
1507                 is_zero_v4(mat[1]) &&
1508                 is_zero_v4(mat[2]) &&
1509                 is_zero_v4(mat[3]));
1510 }
1511
1512 /* make a 4x4 matrix out of 3 transform components */
1513 /* matrices are made in the order: scale * rot * loc */
1514 /* TODO: need to have a version that allows for rotation order... */
1515
1516 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1517 {
1518         float rmat[3][3], smat[3][3], tmat[3][3];
1519
1520         /* initialize new matrix */
1521         unit_m4(mat);
1522
1523         /* make rotation + scaling part */
1524         eul_to_mat3(rmat, eul);
1525         size_to_mat3(smat, size);
1526         mul_m3_m3m3(tmat, rmat, smat);
1527
1528         /* copy rot/scale part to output matrix*/
1529         copy_m4_m3(mat, tmat);
1530
1531         /* copy location to matrix */
1532         mat[3][0] = loc[0];
1533         mat[3][1] = loc[1];
1534         mat[3][2] = loc[2];
1535 }
1536
1537 /* make a 4x4 matrix out of 3 transform components */
1538
1539 /* matrices are made in the order: scale * rot * loc */
1540 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)
1541 {
1542         float rmat[3][3], smat[3][3], tmat[3][3];
1543
1544         /* initialize new matrix */
1545         unit_m4(mat);
1546
1547         /* make rotation + scaling part */
1548         eulO_to_mat3(rmat, eul, rotOrder);
1549         size_to_mat3(smat, size);
1550         mul_m3_m3m3(tmat, rmat, smat);
1551
1552         /* copy rot/scale part to output matrix*/
1553         copy_m4_m3(mat, tmat);
1554
1555         /* copy location to matrix */
1556         mat[3][0] = loc[0];
1557         mat[3][1] = loc[1];
1558         mat[3][2] = loc[2];
1559 }
1560
1561
1562 /* make a 4x4 matrix out of 3 transform components */
1563
1564 /* matrices are made in the order: scale * rot * loc */
1565 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1566 {
1567         float rmat[3][3], smat[3][3], tmat[3][3];
1568
1569         /* initialize new matrix */
1570         unit_m4(mat);
1571
1572         /* make rotation + scaling part */
1573         quat_to_mat3(rmat, quat);
1574         size_to_mat3(smat, size);
1575         mul_m3_m3m3(tmat, rmat, smat);
1576
1577         /* copy rot/scale part to output matrix*/
1578         copy_m4_m3(mat, tmat);
1579
1580         /* copy location to matrix */
1581         mat[3][0] = loc[0];
1582         mat[3][1] = loc[1];
1583         mat[3][2] = loc[2];
1584 }
1585
1586 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])
1587 {
1588         float q[4];
1589         axis_angle_to_quat(q, axis, angle);
1590         loc_quat_size_to_mat4(mat, loc, q, size);
1591 }
1592
1593 /*********************************** Other ***********************************/
1594
1595 void print_m3(const char *str, float m[3][3])
1596 {
1597         printf("%s\n", str);
1598         printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
1599         printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
1600         printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
1601         printf("\n");
1602 }
1603
1604 void print_m4(const char *str, float m[4][4])
1605 {
1606         printf("%s\n", str);
1607         printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
1608         printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
1609         printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
1610         printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
1611         printf("\n");
1612 }
1613
1614 /*********************************** SVD ************************************
1615  * from TNT matrix library
1616  *
1617  * Compute the Single Value Decomposition of an arbitrary matrix A
1618  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1619  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1620  * A = U.W.Vt. From this decomposition it is trivial to compute the
1621  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1622  */
1623
1624 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
1625 {
1626         float A[4][4];
1627         float work1[4], work2[4];
1628         int m = 4;
1629         int n = 4;
1630         int maxiter = 200;
1631         int nu = min_ii(m, n);
1632
1633         float *work = work1;
1634         float *e = work2;
1635         float eps;
1636
1637         int i = 0, j = 0, k = 0, p, pp, iter;
1638
1639         /* Reduce A to bidiagonal form, storing the diagonal elements
1640          * in s and the super-diagonal elements in e. */
1641
1642         int nct = min_ii(m - 1, n);
1643         int nrt = max_ii(0, min_ii(n - 2, m));
1644
1645         copy_m4_m4(A, A_);
1646         zero_m4(U);
1647         zero_v4(s);
1648
1649         for (k = 0; k < max_ii(nct, nrt); k++) {
1650                 if (k < nct) {
1651
1652                         /* Compute the transformation for the k-th column and
1653                          * place the k-th diagonal in s[k].
1654                          * Compute 2-norm of k-th column without under/overflow. */
1655                         s[k] = 0;
1656                         for (i = k; i < m; i++) {
1657                                 s[k] = hypotf(s[k], A[i][k]);
1658                         }
1659                         if (s[k] != 0.0f) {
1660                                 float invsk;
1661                                 if (A[k][k] < 0.0f) {
1662                                         s[k] = -s[k];
1663                                 }
1664                                 invsk = 1.0f / s[k];
1665                                 for (i = k; i < m; i++) {
1666                                         A[i][k] *= invsk;
1667                                 }
1668                                 A[k][k] += 1.0f;
1669                         }
1670                         s[k] = -s[k];
1671                 }
1672                 for (j = k + 1; j < n; j++) {
1673                         if ((k < nct) && (s[k] != 0.0f)) {
1674
1675                                 /* Apply the transformation. */
1676
1677                                 float t = 0;
1678                                 for (i = k; i < m; i++) {
1679                                         t += A[i][k] * A[i][j];
1680                                 }
1681                                 t = -t / A[k][k];
1682                                 for (i = k; i < m; i++) {
1683                                         A[i][j] += t * A[i][k];
1684                                 }
1685                         }
1686
1687                         /* Place the k-th row of A into e for the */
1688                         /* subsequent calculation of the row transformation. */
1689
1690                         e[j] = A[k][j];
1691                 }
1692                 if (k < nct) {
1693
1694                         /* Place the transformation in U for subsequent back
1695                          * multiplication. */
1696
1697                         for (i = k; i < m; i++)
1698                                 U[i][k] = A[i][k];
1699                 }
1700                 if (k < nrt) {
1701
1702                         /* Compute the k-th row transformation and place the
1703                          * k-th super-diagonal in e[k].
1704                          * Compute 2-norm without under/overflow. */
1705                         e[k] = 0;
1706                         for (i = k + 1; i < n; i++) {
1707                                 e[k] = hypotf(e[k], e[i]);
1708                         }
1709                         if (e[k] != 0.0f) {
1710                                 float invek;
1711                                 if (e[k + 1] < 0.0f) {
1712                                         e[k] = -e[k];
1713                                 }
1714                                 invek = 1.0f / e[k];
1715                                 for (i = k + 1; i < n; i++) {
1716                                         e[i] *= invek;
1717                                 }
1718                                 e[k + 1] += 1.0f;
1719                         }
1720                         e[k] = -e[k];
1721                         if ((k + 1 < m) & (e[k] != 0.0f)) {
1722                                 float invek1;
1723
1724                                 /* Apply the transformation. */
1725
1726                                 for (i = k + 1; i < m; i++) {
1727                                         work[i] = 0.0f;
1728                                 }
1729                                 for (j = k + 1; j < n; j++) {
1730                                         for (i = k + 1; i < m; i++) {
1731                                                 work[i] += e[j] * A[i][j];
1732                                         }
1733                                 }
1734                                 invek1 = 1.0f / e[k + 1];
1735                                 for (j = k + 1; j < n; j++) {
1736                                         float t = -e[j] * invek1;
1737                                         for (i = k + 1; i < m; i++) {
1738                                                 A[i][j] += t * work[i];
1739                                         }
1740                                 }
1741                         }
1742
1743                         /* Place the transformation in V for subsequent
1744                          * back multiplication. */
1745
1746                         for (i = k + 1; i < n; i++)
1747                                 V[i][k] = e[i];
1748                 }
1749         }
1750
1751         /* Set up the final bidiagonal matrix or order p. */
1752
1753         p = min_ii(n, m + 1);
1754         if (nct < n) {
1755                 s[nct] = A[nct][nct];
1756         }
1757         if (m < p) {
1758                 s[p - 1] = 0.0f;
1759         }
1760         if (nrt + 1 < p) {
1761                 e[nrt] = A[nrt][p - 1];
1762         }
1763         e[p - 1] = 0.0f;
1764
1765         /* If required, generate U. */
1766
1767         for (j = nct; j < nu; j++) {
1768                 for (i = 0; i < m; i++) {
1769                         U[i][j] = 0.0f;
1770                 }
1771                 U[j][j] = 1.0f;
1772         }
1773         for (k = nct - 1; k >= 0; k--) {
1774                 if (s[k] != 0.0f) {
1775                         for (j = k + 1; j < nu; j++) {
1776                                 float t = 0;
1777                                 for (i = k; i < m; i++) {
1778                                         t += U[i][k] * U[i][j];
1779                                 }
1780                                 t = -t / U[k][k];
1781                                 for (i = k; i < m; i++) {
1782                                         U[i][j] += t * U[i][k];
1783                                 }
1784                         }
1785                         for (i = k; i < m; i++) {
1786                                 U[i][k] = -U[i][k];
1787                         }
1788                         U[k][k] = 1.0f + U[k][k];
1789                         for (i = 0; i < k - 1; i++) {
1790                                 U[i][k] = 0.0f;
1791                         }
1792                 }
1793                 else {
1794                         for (i = 0; i < m; i++) {
1795                                 U[i][k] = 0.0f;
1796                         }
1797                         U[k][k] = 1.0f;
1798                 }
1799         }
1800
1801         /* If required, generate V. */
1802
1803         for (k = n - 1; k >= 0; k--) {
1804                 if ((k < nrt) & (e[k] != 0.0f)) {
1805                         for (j = k + 1; j < nu; j++) {
1806                                 float t = 0;
1807                                 for (i = k + 1; i < n; i++) {
1808                                         t += V[i][k] * V[i][j];
1809                                 }
1810                                 t = -t / V[k + 1][k];
1811                                 for (i = k + 1; i < n; i++) {
1812                                         V[i][j] += t * V[i][k];
1813                                 }
1814                         }
1815                 }
1816                 for (i = 0; i < n; i++) {
1817                         V[i][k] = 0.0f;
1818                 }
1819                 V[k][k] = 1.0f;
1820         }
1821
1822         /* Main iteration loop for the singular values. */
1823
1824         pp = p - 1;
1825         iter = 0;
1826         eps = powf(2.0f, -52.0f);
1827         while (p > 0) {
1828                 int kase = 0;
1829
1830                 /* Test for maximum iterations to avoid infinite loop */
1831                 if (maxiter == 0)
1832                         break;
1833                 maxiter--;
1834
1835                 /* This section of the program inspects for
1836                  * negligible elements in the s and e arrays.  On
1837                  * completion the variables kase and k are set as follows.
1838                  *
1839                  * kase = 1       if s(p) and e[k - 1] are negligible and k<p
1840                  * kase = 2       if s(k) is negligible and k<p
1841                  * kase = 3       if e[k - 1] is negligible, k<p, and
1842                  *               s(k), ..., s(p) are not negligible (qr step).
1843                  * kase = 4       if e(p - 1) is negligible (convergence). */
1844
1845                 for (k = p - 2; k >= -1; k--) {
1846                         if (k == -1) {
1847                                 break;
1848                         }
1849                         if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
1850                                 e[k] = 0.0f;
1851                                 break;
1852                         }
1853                 }
1854                 if (k == p - 2) {
1855                         kase = 4;
1856                 }
1857                 else {
1858                         int ks;
1859                         for (ks = p - 1; ks >= k; ks--) {
1860                                 float t;
1861                                 if (ks == k) {
1862                                         break;
1863                                 }
1864                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
1865                                     (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
1866                                 if (fabsf(s[ks]) <= eps * t) {
1867                                         s[ks] = 0.0f;
1868                                         break;
1869                                 }
1870                         }
1871                         if (ks == k) {
1872                                 kase = 3;
1873                         }
1874                         else if (ks == p - 1) {
1875                                 kase = 1;
1876                         }
1877                         else {
1878                                 kase = 2;
1879                                 k = ks;
1880                         }
1881                 }
1882                 k++;
1883
1884                 /* Perform the task indicated by kase. */
1885
1886                 switch (kase) {
1887
1888                         /* Deflate negligible s(p). */
1889
1890                         case 1:
1891                         {
1892                                 float f = e[p - 2];
1893                                 e[p - 2] = 0.0f;
1894                                 for (j = p - 2; j >= k; j--) {
1895                                         float t = hypotf(s[j], f);
1896                                         float invt = 1.0f / t;
1897                                         float cs = s[j] * invt;
1898                                         float sn = f * invt;
1899                                         s[j] = t;
1900                                         if (j != k) {
1901                                                 f = -sn * e[j - 1];
1902                                                 e[j - 1] = cs * e[j - 1];
1903                                         }
1904
1905                                         for (i = 0; i < n; i++) {
1906                                                 t = cs * V[i][j] + sn * V[i][p - 1];
1907                                                 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
1908                                                 V[i][j] = t;
1909                                         }
1910                                 }
1911                                 break;
1912                         }
1913
1914                         /* Split at negligible s(k). */
1915
1916                         case 2:
1917                         {
1918                                 float f = e[k - 1];
1919                                 e[k - 1] = 0.0f;
1920                                 for (j = k; j < p; j++) {
1921                                         float t = hypotf(s[j], f);
1922                                         float invt = 1.0f / t;
1923                                         float cs = s[j] * invt;
1924                                         float sn = f * invt;
1925                                         s[j] = t;
1926                                         f = -sn * e[j];
1927                                         e[j] = cs * e[j];
1928
1929                                         for (i = 0; i < m; i++) {
1930                                                 t = cs * U[i][j] + sn * U[i][k - 1];
1931                                                 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
1932                                                 U[i][j] = t;
1933                                         }
1934                                 }
1935                                 break;
1936                         }
1937
1938                         /* Perform one qr step. */
1939
1940                         case 3:
1941                         {
1942
1943                                 /* Calculate the shift. */
1944
1945                                 float scale = max_ff(max_ff(max_ff(max_ff(
1946                                                    fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
1947                                                    fabsf(s[k])), fabsf(e[k]));
1948                                 float invscale = 1.0f / scale;
1949                                 float sp = s[p - 1] * invscale;
1950                                 float spm1 = s[p - 2] * invscale;
1951                                 float epm1 = e[p - 2] * invscale;
1952                                 float sk = s[k] * invscale;
1953                                 float ek = e[k] * invscale;
1954                                 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
1955                                 float c = (sp * epm1) * (sp * epm1);
1956                                 float shift = 0.0f;
1957                                 float f, g;
1958                                 if ((b != 0.0f) || (c != 0.0f)) {
1959                                         shift = sqrtf(b * b + c);
1960                                         if (b < 0.0f) {
1961                                                 shift = -shift;
1962                                         }
1963                                         shift = c / (b + shift);
1964                                 }
1965                                 f = (sk + sp) * (sk - sp) + shift;
1966                                 g = sk * ek;
1967
1968                                 /* Chase zeros. */
1969
1970                                 for (j = k; j < p - 1; j++) {
1971                                         float t = hypotf(f, g);
1972                                         /* division by zero checks added to avoid NaN (brecht) */
1973                                         float cs = (t == 0.0f) ? 0.0f : f / t;
1974                                         float sn = (t == 0.0f) ? 0.0f : g / t;
1975                                         if (j != k) {
1976                                                 e[j - 1] = t;
1977                                         }
1978                                         f = cs * s[j] + sn * e[j];
1979                                         e[j] = cs * e[j] - sn * s[j];
1980                                         g = sn * s[j + 1];
1981                                         s[j + 1] = cs * s[j + 1];
1982
1983                                         for (i = 0; i < n; i++) {
1984                                                 t = cs * V[i][j] + sn * V[i][j + 1];
1985                                                 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
1986                                                 V[i][j] = t;
1987                                         }
1988
1989                                         t = hypotf(f, g);
1990                                         /* division by zero checks added to avoid NaN (brecht) */
1991                                         cs = (t == 0.0f) ? 0.0f : f / t;
1992                                         sn = (t == 0.0f) ? 0.0f : g / t;
1993                                         s[j] = t;
1994                                         f = cs * e[j] + sn * s[j + 1];
1995                                         s[j + 1] = -sn * e[j] + cs * s[j + 1];
1996                                         g = sn * e[j + 1];
1997                                         e[j + 1] = cs * e[j + 1];
1998                                         if (j < m - 1) {
1999                                                 for (i = 0; i < m; i++) {
2000                                                         t = cs * U[i][j] + sn * U[i][j + 1];
2001                                                         U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
2002                                                         U[i][j] = t;
2003                                                 }
2004                                         }
2005                                 }
2006                                 e[p - 2] = f;
2007                                 iter = iter + 1;
2008                                 break;
2009                         }
2010                         /* Convergence. */
2011
2012                         case 4:
2013                         {
2014
2015                                 /* Make the singular values positive. */
2016
2017                                 if (s[k] <= 0.0f) {
2018                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
2019
2020                                         for (i = 0; i <= pp; i++)
2021                                                 V[i][k] = -V[i][k];
2022                                 }
2023
2024                                 /* Order the singular values. */
2025
2026                                 while (k < pp) {
2027                                         float t;
2028                                         if (s[k] >= s[k + 1]) {
2029                                                 break;
2030                                         }
2031                                         t = s[k];
2032                                         s[k] = s[k + 1];
2033                                         s[k + 1] = t;
2034                                         if (k < n - 1) {
2035                                                 for (i = 0; i < n; i++) {
2036                                                         t = V[i][k + 1];
2037                                                         V[i][k + 1] = V[i][k];
2038                                                         V[i][k] = t;
2039                                                 }
2040                                         }
2041                                         if (k < m - 1) {
2042                                                 for (i = 0; i < m; i++) {
2043                                                         t = U[i][k + 1];
2044                                                         U[i][k + 1] = U[i][k];
2045                                                         U[i][k] = t;
2046                                                 }
2047                                         }
2048                                         k++;
2049                                 }
2050                                 iter = 0;
2051                                 p--;
2052                                 break;
2053                         }
2054                 }
2055         }
2056 }
2057
2058 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
2059 {
2060         /* compute moon-penrose pseudo inverse of matrix, singular values
2061          * below epsilon are ignored for stability (truncated SVD) */
2062         float V[4][4], W[4], Wm[4][4], U[4][4];
2063         int i;
2064
2065         transpose_m4(A);
2066         svd_m4(V, W, U, A);
2067         transpose_m4(U);
2068         transpose_m4(V);
2069
2070         zero_m4(Wm);
2071         for (i = 0; i < 4; i++)
2072                 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
2073
2074         transpose_m4(V);
2075
2076         mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);
2077 }
2078
2079 void pseudoinverse_m3_m3(float Ainv[3][3], float A[3][3], float epsilon)
2080 {
2081         /* try regular inverse when possible, otherwise fall back to slow svd */
2082         if (!invert_m3_m3(Ainv, A)) {
2083                 float tmp[4][4], tmpinv[4][4];
2084
2085                 copy_m4_m3(tmp, A);
2086                 pseudoinverse_m4_m4(tmpinv, tmp, epsilon);
2087                 copy_m3_m4(Ainv, tmpinv);
2088         }
2089 }
2090
2091 bool has_zero_axis_m4(float matrix[4][4])
2092 {
2093         return len_squared_v3(matrix[0]) < FLT_EPSILON ||
2094                len_squared_v3(matrix[1]) < FLT_EPSILON ||
2095                len_squared_v3(matrix[2]) < FLT_EPSILON;
2096 }