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