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