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