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