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