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