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