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