Updated the Sun ifdef's basically I standardized them so they
[blender-staging.git] / source / blender / blenlib / intern / matrixops.c
1 /*
2  *
3  * Some matrix operations.
4  *
5  * Always use
6  * - vector with x components :   float x[3], int x[3], etc
7  *
8  * $Id$
9  *
10  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
11  *
12  * This program is free software; you can redistribute it and/or
13  * modify it under the terms of the GNU General Public License
14  * as published by the Free Software Foundation; either version 2
15  * of the License, or (at your option) any later version. The Blender
16  * Foundation also sells licenses for use in proprietary software under
17  * the Blender License.  See http://www.blender.org/BL/ for information
18  * about this.
19  *
20  * This program is distributed in the hope that it will be useful,
21  * but WITHOUT ANY WARRANTY; without even the implied warranty of
22  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
23  * GNU General Public License for more details.
24  *
25  * You should have received a copy of the GNU General Public License
26  * along with this program; if not, write to the Free Software Foundation,
27  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
28  *
29  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
30  * All rights reserved.
31  *
32  * The Original Code is: all of this file.
33  *
34  * Contributor(s): none yet.
35  *
36  * ***** END GPL/BL DUAL LICENSE BLOCK *****
37  */
38
39 /* ------------------------------------------------------------------------- */
40 #include <string.h>
41 #include "MTC_matrixops.h"
42 #include "MTC_vectorops.h"
43 /* ------------------------------------------------------------------------- */
44
45 #ifdef HAVE_CONFIG_H
46 #include <config.h>
47 #endif
48
49 #ifdef WIN32
50 #include "BLI_winstuff.h"
51 #endif
52
53 #if defined(__sun__) || defined( __sun ) || defined (__sparc) || defined (__sparc__)
54 #include <strings.h>
55 #endif
56
57 #define ABS(x)  ((x) < 0 ? -(x) : (x))
58 #define SWAP(type, a, b)        { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
59
60 void MTC_Mat4CpyMat4(float m1[][4], float m2[][4])
61 {
62         memcpy(m1, m2, 4*4*sizeof(float));
63 }
64
65 /* ------------------------------------------------------------------------- */
66
67 void MTC_Mat4MulSerie(float answ[][4],
68                                   float m1[][4], float m2[][4], float m3[][4],
69                                   float m4[][4], float m5[][4], float m6[][4],
70                                   float m7[][4], float m8[][4])
71 {
72         float temp[4][4];
73         
74         if(m1==0 || m2==0) return;
75         
76         MTC_Mat4MulMat4(answ, m2, m1);
77         if(m3) {
78                 MTC_Mat4MulMat4(temp, m3, answ);
79                 if(m4) {
80                         MTC_Mat4MulMat4(answ, m4, temp);
81                         if(m5) {
82                                 MTC_Mat4MulMat4(temp, m5, answ);
83                                 if(m6) {
84                                         MTC_Mat4MulMat4(answ, m6, temp);
85                                         if(m7) {
86                                                 MTC_Mat4MulMat4(temp, m7, answ);
87                                                 if(m8) {
88                                                         MTC_Mat4MulMat4(answ, m8, temp);
89                                                 }
90                                                 else MTC_Mat4CpyMat4(answ, temp);
91                                         }
92                                 }
93                                 else MTC_Mat4CpyMat4(answ, temp);
94                         }
95                 }
96                 else MTC_Mat4CpyMat4(answ, temp);
97         }
98 }
99
100 /* ------------------------------------------------------------------------- */
101 void MTC_Mat4MulMat4(float m1[][4], float m2[][4], float m3[][4])
102 {
103   /* matrix product: c[j][k] = a[j][i].b[i][k] */
104
105         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];
106         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];
107         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];
108         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];
109
110         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];
111         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];
112         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];
113         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];
114
115         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];
116         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];
117         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];
118         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];
119
120         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];
121         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];
122         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];
123         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];
124
125 }
126 /* ------------------------------------------------------------------------- */
127
128 void MTC_Mat4MulVecfl(float mat[][4], float *vec)
129 {
130         float x,y;
131
132         x=vec[0]; 
133         y=vec[1];
134         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
135         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
136         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
137 }
138
139 /* ------------------------------------------------------------------------- */
140 void MTC_Mat3MulVecfl(float mat[][3], float *vec)
141 {
142         float x,y;
143
144         x=vec[0]; 
145         y=vec[1];
146         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
147         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
148         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
149 }
150
151 /* ------------------------------------------------------------------------- */
152
153 int MTC_Mat4Invert(float inverse[][4], float mat[][4])
154 {
155         int i, j, k;
156         double temp;
157         float tempmat[4][4];
158         float max;
159         int maxj;
160
161         /* Set inverse to identity */
162         for (i=0; i<4; i++)
163                 for (j=0; j<4; j++)
164                         inverse[i][j] = 0;
165         for (i=0; i<4; i++)
166                 inverse[i][i] = 1;
167
168         /* Copy original matrix so we don't mess it up */
169         for(i = 0; i < 4; i++)
170                 for(j = 0; j <4; j++)
171                         tempmat[i][j] = mat[i][j];
172
173         for(i = 0; i < 4; i++) {
174                 /* Look for row with max pivot */
175                 max = ABS(tempmat[i][i]);
176                 maxj = i;
177                 for(j = i + 1; j < 4; j++) {
178                         if(ABS(tempmat[j][i]) > max) {
179                                 max = ABS(tempmat[j][i]);
180                                 maxj = j;
181                         }
182                 }
183                 /* Swap rows if necessary */
184                 if (maxj != i) {
185                         for( k = 0; k < 4; k++) {
186                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
187                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
188                         }
189                 }
190
191                 temp = tempmat[i][i];
192                 if (temp == 0)
193                         return 0;  /* No non-zero pivot */
194                 for(k = 0; k < 4; k++) {
195                         tempmat[i][k] /= temp;
196                         inverse[i][k] /= temp;
197                 }
198                 for(j = 0; j < 4; j++) {
199                         if(j != i) {
200                                 temp = tempmat[j][i];
201                                 for(k = 0; k < 4; k++) {
202                                         tempmat[j][k] -= tempmat[i][k]*temp;
203                                         inverse[j][k] -= inverse[i][k]*temp;
204                                 }
205                         }
206                 }
207         }
208         return 1;
209 }
210
211 /* ------------------------------------------------------------------------- */
212 void MTC_Mat3CpyMat4(float m1[][3], float m2[][4])
213 {
214         
215         m1[0][0]= m2[0][0];
216         m1[0][1]= m2[0][1];
217         m1[0][2]= m2[0][2];
218
219         m1[1][0]= m2[1][0];
220         m1[1][1]= m2[1][1];
221         m1[1][2]= m2[1][2];
222
223         m1[2][0]= m2[2][0];
224         m1[2][1]= m2[2][1];
225         m1[2][2]= m2[2][2];
226 }
227
228 /* ------------------------------------------------------------------------- */
229
230 void MTC_Mat3CpyMat3(float m1[][3], float m2[][3])
231 {       
232         memcpy(m1, m2, 3*3*sizeof(float));
233 }
234
235 /* ------------------------------------------------------------------------- */
236 /*  void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
237 void MTC_Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3])
238 {
239         /* be careful about this rewrite... */
240             /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped! */
241         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
242         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
243         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
244
245         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
246         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
247         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
248
249         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
250         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
251         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
252
253 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
254 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
255 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
256 /*      m1+=3; */
257 /*      m2+=3; */
258 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
259 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
260 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
261 /*      m1+=3; */
262 /*      m2+=3; */
263 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
264 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
265 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
266 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
267
268 /* ------------------------------------------------------------------------- */
269
270 void MTC_Mat4Ortho(float mat[][4])
271 {
272         float len;
273         
274         len= MTC_normalise3DF(mat[0]);
275         if(len!=0.0) mat[0][3]/= len;
276         len= MTC_normalise3DF(mat[1]);
277         if(len!=0.0) mat[1][3]/= len;
278         len= MTC_normalise3DF(mat[2]);
279         if(len!=0.0) mat[2][3]/= len;
280 }
281
282 /* ------------------------------------------------------------------------- */
283
284 void MTC_Mat4Mul3Vecfl(float mat[][4], float *vec)
285 {
286         float x,y;
287         /* vec = mat^T dot vec !!! or vec a row, then vec = vec dot mat*/
288
289         x= vec[0]; 
290         y= vec[1];
291         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
292         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
293         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
294 }
295
296 /* ------------------------------------------------------------------------- */
297
298 void MTC_Mat4One(float m[][4])
299 {
300
301         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
302         m[0][1]= m[0][2]= m[0][3]= 0.0;
303         m[1][0]= m[1][2]= m[1][3]= 0.0;
304         m[2][0]= m[2][1]= m[2][3]= 0.0;
305         m[3][0]= m[3][1]= m[3][2]= 0.0;
306 }
307
308
309 /* ------------------------------------------------------------------------- */
310 /* Result is a 3-vector!*/
311 void MTC_Mat3MulVecd(float mat[][3], double *vec)
312 {
313         double x,y;
314
315         /* vec = mat^T dot vec !!! or vec a row, then vec = vec dot mat*/
316         x=vec[0]; 
317         y=vec[1];
318         vec[0]= x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
319         vec[1]= x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
320         vec[2]= x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
321 }
322
323 /* ------------------------------------------------------------------------- */
324
325 void MTC_Mat3Inv(float m1[][3], float m2[][3])
326 {
327         short a,b;
328         float det;
329
330         /* first adjoint */
331         MTC_Mat3Adj(m1,m2);
332
333         /* then determinant old mat! */
334         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
335             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
336             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
337
338         if(det==0) det=1;
339         det= 1/det;
340         for(a=0;a<3;a++) {
341                 for(b=0;b<3;b++) {
342                         m1[a][b]*=det;
343                 }
344         }
345 }
346
347 /* ------------------------------------------------------------------------- */
348
349 void MTC_Mat3Adj(float m1[][3], float m[][3])
350 {
351         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
352         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
353         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
354
355         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
356         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
357         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
358
359         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
360         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
361         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
362 }
363
364 /* ------------------------------------------------------------------------- */
365
366 void MTC_Mat3One(float m[][3])
367 {
368
369         m[0][0]= m[1][1]= m[2][2]= 1.0;
370         m[0][1]= m[0][2]= 0.0;
371         m[1][0]= m[1][2]= 0.0;
372         m[2][0]= m[2][1]= 0.0;
373 }
374
375 /* ------------------------------------------------------------------------- */
376
377 void MTC_Mat4SwapMat4(float m1[][4], float m2[][4])
378 {
379         float t;
380         int i, j;
381
382         for(i = 0; i < 4; i++) {
383                 for (j = 0; j < 4; j++) {
384                         t        = m1[i][j];
385                         m1[i][j] = m2[i][j];
386                         m2[i][j] = t;
387                 }
388         }
389 }
390
391 /* ------------------------------------------------------------------------- */
392
393 void MTC_Mat4MulVec4fl(float mat[][4], float *vec)
394 {
395         float x,y,z;
396
397         x = vec[0]; 
398         y = vec[1]; 
399         z = vec[2];
400         vec[0] = x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
401         vec[1] = x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
402         vec[2] = x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
403         vec[3] = x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
404 }
405
406 /* ------------------------------------------------------------------------- */
407
408 void MTC_Mat4CpyMat3nc(float m1[][4], float m2[][3])    /* no clear */
409 {
410         m1[0][0]= m2[0][0];
411         m1[0][1]= m2[0][1];
412         m1[0][2]= m2[0][2];
413
414         m1[1][0]= m2[1][0];
415         m1[1][1]= m2[1][1];
416         m1[1][2]= m2[1][2];
417
418         m1[2][0]= m2[2][0];
419         m1[2][1]= m2[2][1];
420         m1[2][2]= m2[2][2];
421 }
422
423 /* ------------------------------------------------------------------------- */
424
425 void MTC_Mat4MulMat33(float m1[][3], float m2[][4], float m3[][3])
426 {
427         /* m1_i_j = m2_i_k * m3_k_j ? */
428         
429         m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
430         m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
431         m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
432
433         m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
434         m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
435         m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
436
437         m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
438         m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
439         m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
440
441 }
442
443 /* ------------------------------------------------------------------------- */
444
445 /* eof */