Sculpt:
[blender-staging.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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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 <float.h>
29 #include <stdio.h>
30 #include <stdlib.h>
31 #include <string.h>
32
33 #include "BLI_math.h"
34
35 /********************************* Init **************************************/
36
37 void zero_m3(float m[3][3])
38 {
39         memset(m, 0, 3*3*sizeof(float));
40 }
41
42 void zero_m4(float m[4][4])
43 {
44         memset(m, 0, 4*4*sizeof(float));
45 }
46
47 void unit_m3(float m[][3])
48 {
49         m[0][0]= m[1][1]= m[2][2]= 1.0;
50         m[0][1]= m[0][2]= 0.0;
51         m[1][0]= m[1][2]= 0.0;
52         m[2][0]= m[2][1]= 0.0;
53 }
54
55 void unit_m4(float m[][4])
56 {
57         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
58         m[0][1]= m[0][2]= m[0][3]= 0.0;
59         m[1][0]= m[1][2]= m[1][3]= 0.0;
60         m[2][0]= m[2][1]= m[2][3]= 0.0;
61         m[3][0]= m[3][1]= m[3][2]= 0.0;
62 }
63
64 void copy_m3_m3(float m1[][3], float m2[][3]) 
65 {       
66         /* destination comes first: */
67         memcpy(&m1[0], &m2[0], 9*sizeof(float));
68 }
69
70 void copy_m4_m4(float m1[][4], float m2[][4]) 
71 {
72         memcpy(m1, m2, 4*4*sizeof(float));
73 }
74
75 void copy_m3_m4(float m1[][3], float m2[][4])
76 {
77         m1[0][0]= m2[0][0];
78         m1[0][1]= m2[0][1];
79         m1[0][2]= m2[0][2];
80
81         m1[1][0]= m2[1][0];
82         m1[1][1]= m2[1][1];
83         m1[1][2]= m2[1][2];
84
85         m1[2][0]= m2[2][0];
86         m1[2][1]= m2[2][1];
87         m1[2][2]= m2[2][2];
88 }
89
90 void copy_m4_m3(float m1[][4], float m2[][3])   /* no clear */
91 {
92         m1[0][0]= m2[0][0];
93         m1[0][1]= m2[0][1];
94         m1[0][2]= m2[0][2];
95
96         m1[1][0]= m2[1][0];
97         m1[1][1]= m2[1][1];
98         m1[1][2]= m2[1][2];
99
100         m1[2][0]= m2[2][0];
101         m1[2][1]= m2[2][1];
102         m1[2][2]= m2[2][2];
103
104         /*      Reevan's Bugfix */
105         m1[0][3]=0.0F;
106         m1[1][3]=0.0F;
107         m1[2][3]=0.0F;
108
109         m1[3][0]=0.0F;  
110         m1[3][1]=0.0F;  
111         m1[3][2]=0.0F;  
112         m1[3][3]=1.0F;
113
114 }
115
116 void swap_m4m4(float m1[][4], float m2[][4])
117 {
118         float t;
119         int i, j;
120
121         for(i = 0; i < 4; i++) {
122                 for (j = 0; j < 4; j++) {
123                         t        = m1[i][j];
124                         m1[i][j] = m2[i][j];
125                         m2[i][j] = t;
126                 }
127         }
128 }
129
130 /******************************** Arithmetic *********************************/
131
132 void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
133 {
134   /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
135
136         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];
137         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];
138         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];
139         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];
140
141         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];
142         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];
143         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];
144         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];
145
146         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];
147         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];
148         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];
149         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];
150
151         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];
152         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];
153         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];
154         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];
155
156 }
157
158 void mul_m3_m3m3(float m1[][3], float m3[][3], float m2[][3])
159 {
160    /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
161         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 
162         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 
163         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 
164
165         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 
166         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 
167         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 
168
169         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 
170         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 
171         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
172 }
173
174 void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3])
175 {
176         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
177         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
178         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
179         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
180         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
181         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
182         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
183         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
184         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
185 }
186
187 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
188 void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4])
189 {
190     /* m1[i][j] = m2[i][k] * m3[k][j] */
191     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
192     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
193     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
194
195     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
196     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
197     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
198
199     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
200     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
201     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
202 }
203
204 void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
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 void mul_serie_m3(float answ[][3],
218                                    float m1[][3], float m2[][3], float m3[][3],
219                                    float m4[][3], float m5[][3], float m6[][3],
220                                    float m7[][3], float m8[][3])
221 {
222         float temp[3][3];
223         
224         if(m1==0 || m2==0) return;
225         
226         mul_m3_m3m3(answ, m2, m1);
227         if(m3) {
228                 mul_m3_m3m3(temp, m3, answ);
229                 if(m4) {
230                         mul_m3_m3m3(answ, m4, temp);
231                         if(m5) {
232                                 mul_m3_m3m3(temp, m5, answ);
233                                 if(m6) {
234                                         mul_m3_m3m3(answ, m6, temp);
235                                         if(m7) {
236                                                 mul_m3_m3m3(temp, m7, answ);
237                                                 if(m8) {
238                                                         mul_m3_m3m3(answ, m8, temp);
239                                                 }
240                                                 else copy_m3_m3(answ, temp);
241                                         }
242                                 }
243                                 else copy_m3_m3(answ, temp);
244                         }
245                 }
246                 else copy_m3_m3(answ, temp);
247         }
248 }
249
250 void mul_serie_m4(float answ[][4], float m1[][4],
251                                 float m2[][4], float m3[][4], float m4[][4],
252                                 float m5[][4], float m6[][4], float m7[][4],
253                                 float m8[][4])
254 {
255         float temp[4][4];
256         
257         if(m1==0 || m2==0) return;
258         
259         mul_m4_m4m4(answ, m2, m1);
260         if(m3) {
261                 mul_m4_m4m4(temp, m3, answ);
262                 if(m4) {
263                         mul_m4_m4m4(answ, m4, temp);
264                         if(m5) {
265                                 mul_m4_m4m4(temp, m5, answ);
266                                 if(m6) {
267                                         mul_m4_m4m4(answ, m6, temp);
268                                         if(m7) {
269                                                 mul_m4_m4m4(temp, m7, answ);
270                                                 if(m8) {
271                                                         mul_m4_m4m4(answ, m8, temp);
272                                                 }
273                                                 else copy_m4_m4(answ, temp);
274                                         }
275                                 }
276                                 else copy_m4_m4(answ, temp);
277                         }
278                 }
279                 else copy_m4_m4(answ, temp);
280         }
281 }
282
283 void mul_m4_v3(float mat[][4], float *vec)
284 {
285         float x,y;
286
287         x=vec[0]; 
288         y=vec[1];
289         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
290         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
291         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
292 }
293
294 void mul_v3_m4v3(float *in, float mat[][4], float *vec)
295 {
296         float x,y;
297
298         x=vec[0]; 
299         y=vec[1];
300         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
301         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
302         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
303 }
304
305 void mul_mat3_m4_v3(float mat[][4], float *vec)
306 {
307         float x,y;
308
309         x= vec[0]; 
310         y= vec[1];
311         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
312         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
313         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
314 }
315
316 void mul_project_m4_v4(float mat[][4], float *vec)
317 {
318         float w;
319
320         w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
321         mul_m4_v3(mat, vec);
322
323         vec[0] /= w;
324         vec[1] /= w;
325         vec[2] /= w;
326 }
327
328 void mul_m4_v4(float mat[][4], float *vec)
329 {
330         float x,y,z;
331
332         x=vec[0]; 
333         y=vec[1]; 
334         z= vec[2];
335         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
336         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
337         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
338         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
339 }
340
341 void mul_v3_m3v3(float r[3], float M[3][3], float a[3])
342 {
343         r[0]= M[0][0]*a[0] + M[1][0]*a[1] + M[2][0]*a[2];
344         r[1]= M[0][1]*a[0] + M[1][1]*a[1] + M[2][1]*a[2];
345         r[2]= M[0][2]*a[0] + M[1][2]*a[1] + M[2][2]*a[2];
346 }
347
348 void mul_m3_v3(float M[3][3], float r[3])
349 {
350         float tmp[3];
351
352         mul_v3_m3v3(tmp, M, r);
353         copy_v3_v3(r, tmp);
354 }
355
356 void mul_transposed_m3_v3(float mat[][3], float *vec)
357 {
358         float x,y;
359
360         x=vec[0]; 
361         y=vec[1];
362         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
363         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
364         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
365 }
366
367 void mul_m3_fl(float m[3][3], float f)
368 {
369         int i, j;
370
371         for(i=0;i<3;i++)
372                 for(j=0;j<3;j++)
373                         m[i][j] *= f;
374 }
375
376 void mul_m4_fl(float m[4][4], float f)
377 {
378         int i, j;
379
380         for(i=0;i<4;i++)
381                 for(j=0;j<4;j++)
382                         m[i][j] *= f;
383 }
384
385 void mul_mat3_m4_fl(float m[4][4], float f)
386 {
387         int i, j;
388
389         for(i=0; i<3; i++)
390                 for(j=0; j<3; j++)
391                         m[i][j] *= f;
392 }
393
394 void mul_m3_v3_double(float mat[][3], double *vec)
395 {
396         double x,y;
397
398         x=vec[0]; 
399         y=vec[1];
400         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
401         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
402         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
403 }
404
405 void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
406 {
407         int i, j;
408
409         for(i=0;i<3;i++)
410                 for(j=0;j<3;j++)
411                         m1[i][j]= m2[i][j] + m3[i][j];
412 }
413
414 void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
415 {
416         int i, j;
417
418         for(i=0;i<4;i++)
419                 for(j=0;j<4;j++)
420                         m1[i][j]= m2[i][j] + m3[i][j];
421 }
422
423 int invert_m3(float m[3][3])
424 {
425         float tmp[3][3];
426         int success;
427
428         success= invert_m3_m3(tmp, m);
429         copy_m3_m3(m, tmp);
430
431         return success;
432 }
433
434 int invert_m3_m3(float m1[3][3], float m2[3][3])
435 {
436         float det;
437         int a, b, success;
438
439         /* calc adjoint */
440         adjoint_m3_m3(m1,m2);
441
442         /* then determinant old matrix! */
443         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
444             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
445             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
446
447         success= (det != 0);
448
449         if(det==0) det=1;
450         det= 1/det;
451         for(a=0;a<3;a++) {
452                 for(b=0;b<3;b++) {
453                         m1[a][b]*=det;
454                 }
455         }
456
457         return success;
458 }
459
460 int invert_m4(float m[4][4])
461 {
462         float tmp[4][4];
463         int success;
464
465         success= invert_m4_m4(tmp, m);
466         copy_m4_m4(m, tmp);
467
468         return success;
469 }
470
471 /*
472  * invertmat - 
473  *              computes the inverse of mat and puts it in inverse.  Returns 
474  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
475  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
476  *
477  *                                      Mark Segal - 1992
478  */
479
480 int invert_m4_m4(float inverse[4][4], float mat[4][4])
481 {
482         int i, j, k;
483         double temp;
484         float tempmat[4][4];
485         float max;
486         int maxj;
487
488         /* Set inverse to identity */
489         for (i=0; i<4; i++)
490                 for (j=0; j<4; j++)
491                         inverse[i][j] = 0;
492         for (i=0; i<4; i++)
493                 inverse[i][i] = 1;
494
495         /* Copy original matrix so we don't mess it up */
496         for(i = 0; i < 4; i++)
497                 for(j = 0; j <4; j++)
498                         tempmat[i][j] = mat[i][j];
499
500         for(i = 0; i < 4; i++) {
501                 /* Look for row with max pivot */
502                 max = fabs(tempmat[i][i]);
503                 maxj = i;
504                 for(j = i + 1; j < 4; j++) {
505                         if(fabs(tempmat[j][i]) > max) {
506                                 max = fabs(tempmat[j][i]);
507                                 maxj = j;
508                         }
509                 }
510                 /* Swap rows if necessary */
511                 if (maxj != i) {
512                         for(k = 0; k < 4; k++) {
513                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
514                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
515                         }
516                 }
517
518                 temp = tempmat[i][i];
519                 if (temp == 0)
520                         return 0;  /* No non-zero pivot */
521                 for(k = 0; k < 4; k++) {
522                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
523                         inverse[i][k] = (float)(inverse[i][k]/temp);
524                 }
525                 for(j = 0; j < 4; j++) {
526                         if(j != i) {
527                                 temp = tempmat[j][i];
528                                 for(k = 0; k < 4; k++) {
529                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
530                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
531                                 }
532                         }
533                 }
534         }
535         return 1;
536 }
537
538 /****************************** Linear Algebra *******************************/
539
540 void transpose_m3(float mat[][3])
541 {
542         float t;
543
544         t = mat[0][1] ; 
545         mat[0][1] = mat[1][0] ; 
546         mat[1][0] = t;
547         t = mat[0][2] ; 
548         mat[0][2] = mat[2][0] ; 
549         mat[2][0] = t;
550         t = mat[1][2] ; 
551         mat[1][2] = mat[2][1] ; 
552         mat[2][1] = t;
553 }
554
555 void transpose_m4(float mat[][4])
556 {
557         float t;
558
559         t = mat[0][1] ; 
560         mat[0][1] = mat[1][0] ; 
561         mat[1][0] = t;
562         t = mat[0][2] ; 
563         mat[0][2] = mat[2][0] ; 
564         mat[2][0] = t;
565         t = mat[0][3] ; 
566         mat[0][3] = mat[3][0] ; 
567         mat[3][0] = t;
568
569         t = mat[1][2] ; 
570         mat[1][2] = mat[2][1] ; 
571         mat[2][1] = t;
572         t = mat[1][3] ; 
573         mat[1][3] = mat[3][1] ; 
574         mat[3][1] = t;
575
576         t = mat[2][3] ; 
577         mat[2][3] = mat[3][2] ; 
578         mat[3][2] = t;
579 }
580
581 void orthogonalize_m3(float mat[][3], int axis)
582 {
583         float size[3];
584         size[0] = len_v3(mat[0]);
585         size[1] = len_v3(mat[1]);
586         size[2] = len_v3(mat[2]);
587         normalize_v3(mat[axis]);
588         switch(axis)
589         {
590                 case 0:
591                         if (dot_v3v3(mat[0], mat[1]) < 1) {
592                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
593                                 normalize_v3(mat[2]);
594                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
595                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
596                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
597                                 normalize_v3(mat[1]);
598                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
599                         } else {
600                                 float vec[3] = {mat[0][1], mat[0][2], mat[0][0]};
601
602                                 cross_v3_v3v3(mat[2], mat[0], vec);
603                                 normalize_v3(mat[2]);
604                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
605                         }
606                 case 1:
607                         if (dot_v3v3(mat[1], mat[0]) < 1) {
608                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
609                                 normalize_v3(mat[2]);
610                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
611                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
612                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
613                                 normalize_v3(mat[0]);
614                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
615                         } else {
616                                 float vec[3] = {mat[1][1], mat[1][2], mat[1][0]};
617
618                                 cross_v3_v3v3(mat[0], mat[1], vec);
619                                 normalize_v3(mat[0]);
620                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
621                         }
622                 case 2:
623                         if (dot_v3v3(mat[2], mat[0]) < 1) {
624                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
625                                 normalize_v3(mat[1]);
626                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
627                         } else if (dot_v3v3(mat[2], mat[1]) < 1) {
628                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
629                                 normalize_v3(mat[0]);
630                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
631                         } else {
632                                 float vec[3] = {mat[2][1], mat[2][2], mat[2][0]};
633
634                                 cross_v3_v3v3(mat[0], vec, mat[2]);
635                                 normalize_v3(mat[0]);
636                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
637                         }
638         }
639         mul_v3_fl(mat[0], size[0]);
640         mul_v3_fl(mat[1], size[1]);
641         mul_v3_fl(mat[2], size[2]);
642 }
643
644 void orthogonalize_m4(float mat[][4], int axis)
645 {
646         float size[3];
647         size[0] = len_v3(mat[0]);
648         size[1] = len_v3(mat[1]);
649         size[2] = len_v3(mat[2]);
650         normalize_v3(mat[axis]);
651         switch(axis)
652         {
653                 case 0:
654                         if (dot_v3v3(mat[0], mat[1]) < 1) {
655                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
656                                 normalize_v3(mat[2]);
657                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
658                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
659                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
660                                 normalize_v3(mat[1]);
661                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
662                         } else {
663                                 float vec[3] = {mat[0][1], mat[0][2], mat[0][0]};
664
665                                 cross_v3_v3v3(mat[2], mat[0], vec);
666                                 normalize_v3(mat[2]);
667                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
668                         }
669                 case 1:
670                         normalize_v3(mat[0]);
671                         if (dot_v3v3(mat[1], mat[0]) < 1) {
672                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
673                                 normalize_v3(mat[2]);
674                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
675                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
676                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
677                                 normalize_v3(mat[0]);
678                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
679                         } else {
680                                 float vec[3] = {mat[1][1], mat[1][2], mat[1][0]};
681
682                                 cross_v3_v3v3(mat[0], mat[1], vec);
683                                 normalize_v3(mat[0]);
684                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
685                         }
686                 case 2:
687                         if (dot_v3v3(mat[2], mat[0]) < 1) {
688                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
689                                 normalize_v3(mat[1]);
690                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
691                         } else if (dot_v3v3(mat[2], mat[1]) < 1) {
692                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
693                                 normalize_v3(mat[0]);
694                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
695                         } else {
696                                 float vec[3] = {mat[2][1], mat[2][2], mat[2][0]};
697
698                                 cross_v3_v3v3(mat[0], vec, mat[2]);
699                                 normalize_v3(mat[0]);
700                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
701                         }
702         }
703         mul_v3_fl(mat[0], size[0]);
704         mul_v3_fl(mat[1], size[1]);
705         mul_v3_fl(mat[2], size[2]);
706 }
707
708 int is_orthogonal_m3(float mat[][3])
709 {
710         if (fabs(dot_v3v3(mat[0], mat[1])) > 1.5 * FLT_EPSILON)
711                 return 0;
712
713         if (fabs(dot_v3v3(mat[1], mat[2])) > 1.5 * FLT_EPSILON)
714                 return 0;
715
716         if (fabs(dot_v3v3(mat[0], mat[2])) > 1.5 * FLT_EPSILON)
717                 return 0;
718         
719         return 1;
720 }
721
722 int is_orthogonal_m4(float mat[][4])
723 {
724         if (fabs(dot_v3v3(mat[0], mat[1])) > 1.5 * FLT_EPSILON)
725                 return 0;
726
727         if (fabs(dot_v3v3(mat[1], mat[2])) > 1.5 * FLT_EPSILON)
728                 return 0;
729
730         if (fabs(dot_v3v3(mat[0], mat[2])) > 1.5 * FLT_EPSILON)
731                 return 0;
732         
733         return 1;
734 }
735
736 void normalize_m3(float mat[][3])
737 {       
738         normalize_v3(mat[0]);
739         normalize_v3(mat[1]);
740         normalize_v3(mat[2]);
741 }
742
743 void normalize_m4(float mat[][4])
744 {
745         float len;
746         
747         len= normalize_v3(mat[0]);
748         if(len!=0.0) mat[0][3]/= len;
749         len= normalize_v3(mat[1]);
750         if(len!=0.0) mat[1][3]/= len;
751         len= normalize_v3(mat[2]);
752         if(len!=0.0) mat[2][3]/= len;
753 }
754
755 void adjoint_m3_m3(float m1[][3], float m[][3])
756 {
757         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
758         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
759         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
760
761         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
762         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
763         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
764
765         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
766         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
767         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
768 }
769
770 void adjoint_m4_m4(float out[][4], float in[][4])       /* out = ADJ(in) */
771 {
772         float a1, a2, a3, a4, b1, b2, b3, b4;
773         float c1, c2, c3, c4, d1, d2, d3, d4;
774
775         a1= in[0][0]; 
776         b1= in[0][1];
777         c1= in[0][2]; 
778         d1= in[0][3];
779
780         a2= in[1][0]; 
781         b2= in[1][1];
782         c2= in[1][2]; 
783         d2= in[1][3];
784
785         a3= in[2][0]; 
786         b3= in[2][1];
787         c3= in[2][2]; 
788         d3= in[2][3];
789
790         a4= in[3][0]; 
791         b4= in[3][1];
792         c4= in[3][2]; 
793         d4= in[3][3];
794
795
796         out[0][0]  =   determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
797         out[1][0]  = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
798         out[2][0]  =   determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
799         out[3][0]  = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
800
801         out[0][1]  = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
802         out[1][1]  =   determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
803         out[2][1]  = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
804         out[3][1]  =   determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
805
806         out[0][2]  =   determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
807         out[1][2]  = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
808         out[2][2]  =   determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
809         out[3][2]  = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
810
811         out[0][3]  = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
812         out[1][3]  =   determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
813         out[2][3]  = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
814         out[3][3]  =   determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
815 }
816
817 float determinant_m2(float a,float b,float c,float d)
818 {
819
820         return a*d - b*c;
821 }
822
823 float determinant_m3(float a1, float a2, float a3,
824                          float b1, float b2, float b3,
825                          float c1, float c2, float c3)
826 {
827         float ans;
828
829         ans = a1 * determinant_m2(b2, b3, c2, c3)
830             - b1 * determinant_m2(a2, a3, c2, c3)
831             + c1 * determinant_m2(a2, a3, b2, b3);
832
833         return ans;
834 }
835
836 float determinant_m4(float m[][4])
837 {
838         float ans;
839         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
840
841         a1= m[0][0]; 
842         b1= m[0][1];
843         c1= m[0][2]; 
844         d1= m[0][3];
845
846         a2= m[1][0]; 
847         b2= m[1][1];
848         c2= m[1][2]; 
849         d2= m[1][3];
850
851         a3= m[2][0]; 
852         b3= m[2][1];
853         c3= m[2][2]; 
854         d3= m[2][3];
855
856         a4= m[3][0]; 
857         b4= m[3][1];
858         c4= m[3][2]; 
859         d4= m[3][3];
860
861         ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
862             - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
863             + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
864             - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
865
866         return ans;
867 }
868
869 /****************************** Transformations ******************************/
870
871 void size_to_mat3(float mat[][3], float *size)
872 {
873         mat[0][0]= size[0];
874         mat[0][1]= 0.0f;
875         mat[0][2]= 0.0f;
876         mat[1][1]= size[1];
877         mat[1][0]= 0.0f;
878         mat[1][2]= 0.0f;
879         mat[2][2]= size[2];
880         mat[2][1]= 0.0f;
881         mat[2][0]= 0.0f;
882 }
883
884 void size_to_mat4(float mat[][4], float *size)
885 {
886         float tmat[3][3];
887         
888         size_to_mat3(tmat,size);
889         unit_m4(mat);
890         copy_m4_m3(mat, tmat);
891 }
892
893 void mat3_to_size(float *size, float mat[][3])
894 {
895         size[0]= len_v3(mat[0]);
896         size[1]= len_v3(mat[1]);
897         size[2]= len_v3(mat[2]);
898 }
899
900 void mat4_to_size(float *size, float mat[][4])
901 {
902         size[0]= len_v3(mat[0]);
903         size[1]= len_v3(mat[1]);
904         size[2]= len_v3(mat[2]);
905 }
906
907 /* this gets the average scale of a matrix, only use when your scaling
908  * data that has no idea of scale axis, examples are bone-envelope-radius
909  * and curve radius */
910 float mat3_to_scale(float mat[][3])
911 {
912         /* unit length vector */
913         float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};
914         mul_m3_v3(mat, unit_vec);
915         return len_v3(unit_vec);
916 }
917
918 float mat4_to_scale(float mat[][4])
919 {
920         float tmat[3][3];
921         copy_m3_m4(tmat, mat);
922         return mat3_to_scale(tmat);
923 }
924
925 void scale_m3_fl(float m[][3], float scale)
926 {
927         m[0][0]= m[1][1]= m[2][2]= scale;
928         m[0][1]= m[0][2]= 0.0;
929         m[1][0]= m[1][2]= 0.0;
930         m[2][0]= m[2][1]= 0.0;
931 }
932
933 void scale_m4_fl(float m[][4], float scale)
934 {
935         m[0][0]= m[1][1]= m[2][2]= scale;
936         m[3][3]= 1.0;
937         m[0][1]= m[0][2]= m[0][3]= 0.0;
938         m[1][0]= m[1][2]= m[1][3]= 0.0;
939         m[2][0]= m[2][1]= m[2][3]= 0.0;
940         m[3][0]= m[3][1]= m[3][2]= 0.0;
941 }
942
943 void translate_m4(float mat[][4],float Tx, float Ty, float Tz)
944 {
945     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
946     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
947     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
948 }
949
950 void rotate_m4(float mat[][4], char axis,float angle)
951 {
952         int col;
953     float temp[4];
954     float cosine, sine;
955
956     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
957         temp[col] = 0;
958
959     angle = (float)(angle*(3.1415926535/180.0));
960     cosine = (float)cos(angle);
961     sine = (float)sin(angle);
962     switch(axis){
963     case 'x':    
964     case 'X':    
965         for(col=0 ; col<4 ; col++)
966             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
967         for(col=0 ; col<4 ; col++) {
968             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
969             mat[1][col] = temp[col];
970         }
971         break;
972
973     case 'y':
974     case 'Y':
975         for(col=0 ; col<4 ; col++)
976             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
977         for(col=0 ; col<4 ; col++) {
978             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
979             mat[0][col] = temp[col];
980         }
981         break;
982
983     case 'z':
984     case 'Z':
985         for(col=0 ; col<4 ; col++)
986             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
987         for(col=0 ; col<4 ; col++) {
988             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
989             mat[0][col] = temp[col];
990         }
991         break;
992     }
993 }
994
995 void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], float srcweight)
996 {
997         float squat[4], dquat[4], fquat[4];
998         float ssize[3], dsize[3], fsize[4];
999         float rmat[3][3], smat[3][3];
1000         
1001         mat3_to_quat(dquat,dst);
1002         mat3_to_size(dsize,dst);
1003
1004         mat3_to_quat(squat,src);
1005         mat3_to_size(ssize,src);
1006         
1007         /* do blending */
1008         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1009         interp_v3_v3v3(fsize, dsize, ssize, srcweight);
1010
1011         /* compose new matrix */
1012         quat_to_mat3(rmat,fquat);
1013         size_to_mat3(smat,fsize);
1014         mul_m3_m3m3(out, rmat, smat);
1015 }
1016
1017 void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], float srcweight)
1018 {
1019         float squat[4], dquat[4], fquat[4];
1020         float ssize[3], dsize[3], fsize[4];
1021         float sloc[3], dloc[3], floc[3];
1022         
1023         mat4_to_quat(dquat,dst);
1024         mat4_to_size(dsize,dst);
1025         copy_v3_v3(dloc, dst[3]);
1026
1027         mat4_to_quat(squat,src);
1028         mat4_to_size(ssize,src);
1029         copy_v3_v3(sloc, src[3]);
1030         
1031         /* do blending */
1032         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1033         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1034         interp_v3_v3v3(fsize, dsize, ssize, srcweight);
1035
1036         /* compose new matrix */
1037         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1038 }
1039
1040 /* make a 4x4 matrix out of 3 transform components */
1041 /* matrices are made in the order: scale * rot * loc */
1042 // TODO: need to have a version that allows for rotation order...
1043 void loc_eul_size_to_mat4(float mat[4][4], float loc[3], float eul[3], float size[3])
1044 {
1045         float rmat[3][3], smat[3][3], tmat[3][3];
1046         
1047         /* initialise new matrix */
1048         unit_m4(mat);
1049         
1050         /* make rotation + scaling part */
1051         eul_to_mat3(rmat,eul);
1052         size_to_mat3(smat,size);
1053         mul_m3_m3m3(tmat, rmat, smat);
1054         
1055         /* copy rot/scale part to output matrix*/
1056         copy_m4_m3(mat, tmat);
1057         
1058         /* copy location to matrix */
1059         mat[3][0] = loc[0];
1060         mat[3][1] = loc[1];
1061         mat[3][2] = loc[2];
1062 }
1063
1064 /* make a 4x4 matrix out of 3 transform components */
1065 /* matrices are made in the order: scale * rot * loc */
1066 void loc_eulO_size_to_mat4(float mat[4][4], float loc[3], float eul[3], float size[3], short rotOrder)
1067 {
1068         float rmat[3][3], smat[3][3], tmat[3][3];
1069         
1070         /* initialise new matrix */
1071         unit_m4(mat);
1072         
1073         /* make rotation + scaling part */
1074         eulO_to_mat3(rmat,eul, rotOrder);
1075         size_to_mat3(smat,size);
1076         mul_m3_m3m3(tmat, rmat, smat);
1077         
1078         /* copy rot/scale part to output matrix*/
1079         copy_m4_m3(mat, tmat);
1080         
1081         /* copy location to matrix */
1082         mat[3][0] = loc[0];
1083         mat[3][1] = loc[1];
1084         mat[3][2] = loc[2];
1085 }
1086
1087
1088 /* make a 4x4 matrix out of 3 transform components */
1089 /* matrices are made in the order: scale * rot * loc */
1090 void loc_quat_size_to_mat4(float mat[4][4], float loc[3], float quat[4], float size[3])
1091 {
1092         float rmat[3][3], smat[3][3], tmat[3][3];
1093         
1094         /* initialise new matrix */
1095         unit_m4(mat);
1096         
1097         /* make rotation + scaling part */
1098         quat_to_mat3(rmat,quat);
1099         size_to_mat3(smat,size);
1100         mul_m3_m3m3(tmat, rmat, smat);
1101         
1102         /* copy rot/scale part to output matrix*/
1103         copy_m4_m3(mat, tmat);
1104         
1105         /* copy location to matrix */
1106         mat[3][0] = loc[0];
1107         mat[3][1] = loc[1];
1108         mat[3][2] = loc[2];
1109 }
1110
1111 /*********************************** Other ***********************************/
1112
1113 void print_m3(char *str, float m[][3])
1114 {
1115         printf("%s\n", str);
1116         printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
1117         printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
1118         printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
1119         printf("\n");
1120 }
1121
1122 void print_m4(char *str, float m[][4])
1123 {
1124         printf("%s\n", str);
1125         printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
1126         printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
1127         printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
1128         printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
1129         printf("\n");
1130 }
1131
1132