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