Bone Heat Weighting
[blender-staging.git] / source / blender / blenlib / intern / arithb.c
1 /* arithb.c
2  *
3  * simple math for blender code
4  *
5  * sort of cleaned up mar-01 nzc
6  *
7  * $Id$
8  *
9  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
10  *
11  * This program is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU General Public License
13  * as published by the Free Software Foundation; either version 2
14  * of the License, or (at your option) any later version. The Blender
15  * Foundation also sells licenses for use in proprietary software under
16  * the Blender License.  See http://www.blender.org/BL/ for information
17  * about this.
18  *
19  * This program is distributed in the hope that it will be useful,
20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
22  * GNU General Public License for more details.
23  *
24  * You should have received a copy of the GNU General Public License
25  * along with this program; if not, write to the Free Software Foundation,
26  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
27  *
28  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
29  * All rights reserved.
30  *
31  * The Original Code is: all of this file.
32  *
33  * Contributor(s): none yet.
34  *
35  * ***** END GPL/BL DUAL LICENSE BLOCK *****
36  */
37
38 /* ************************ FUNKTIES **************************** */
39
40 #include <math.h>
41 #include <sys/types.h>
42 #include <string.h> 
43 #include <float.h>
44
45 #ifdef HAVE_CONFIG_H
46 #include <config.h>
47 #endif
48
49 #if defined(__sun__) || defined( __sun ) || defined (__sparc) || defined (__sparc__)
50 #include <strings.h>
51 #endif
52
53 #if !defined(__sgi) && !defined(WIN32)
54 #include <sys/time.h>
55 #include <unistd.h>
56 #endif
57
58 #include <stdio.h>
59 #include "BLI_arithb.h"
60
61 /* A few small defines. Keep'em local! */
62 #define SMALL_NUMBER    1.e-8
63 #define ABS(x)  ((x) < 0 ? -(x) : (x))
64 #define SWAP(type, a, b)        { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
65
66
67 #if defined(WIN32) || defined(__APPLE__)
68 #include <stdlib.h>
69 #define M_PI 3.14159265358979323846
70 #define M_SQRT2 1.41421356237309504880   
71
72 #endif /* defined(WIN32) || defined(__APPLE__) */
73
74
75 float saacos(float fac)
76 {
77         if(fac<= -1.0f) return (float)M_PI;
78         else if(fac>=1.0f) return 0.0;
79         else return (float)acos(fac);
80 }
81
82 float saasin(float fac)
83 {
84         if(fac<= -1.0f) return (float)-M_PI/2.0f;
85         else if(fac>=1.0f) return (float)M_PI/2.0f;
86         else return (float)asin(fac);
87 }
88
89 float sasqrt(float fac)
90 {
91         if(fac<=0.0) return 0.0;
92         return (float)sqrt(fac);
93 }
94
95 float Normalize(float *n)
96 {
97         float d;
98         
99         d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
100         /* A larger value causes normalize errors in a scaled down models with camera xtreme close */
101         if(d>1.0e-35F) {
102                 d= (float)sqrt(d);
103
104                 n[0]/=d; 
105                 n[1]/=d; 
106                 n[2]/=d;
107         } else {
108                 n[0]=n[1]=n[2]= 0.0;
109                 d= 0.0;
110         }
111         return d;
112 }
113
114 void Crossf(float *c, float *a, float *b)
115 {
116         c[0] = a[1] * b[2] - a[2] * b[1];
117         c[1] = a[2] * b[0] - a[0] * b[2];
118         c[2] = a[0] * b[1] - a[1] * b[0];
119 }
120
121 /* Inpf returns the dot product, also called the scalar product and inner product */
122 float Inpf( float *v1, float *v2)
123 {
124         return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
125 }
126
127 /* Project v1 on v2 */
128 void Projf(float *c, float *v1, float *v2)
129 {
130         float mul;
131         mul = Inpf(v1, v2) / Inpf(v2, v2);
132         
133         c[0] = mul * v2[0];
134         c[1] = mul * v2[1];
135         c[2] = mul * v2[2];
136 }
137
138 void Mat3Transp(float mat[][3])
139 {
140         float t;
141
142         t = mat[0][1] ; 
143         mat[0][1] = mat[1][0] ; 
144         mat[1][0] = t;
145         t = mat[0][2] ; 
146         mat[0][2] = mat[2][0] ; 
147         mat[2][0] = t;
148         t = mat[1][2] ; 
149         mat[1][2] = mat[2][1] ; 
150         mat[2][1] = t;
151 }
152
153 void Mat4Transp(float mat[][4])
154 {
155         float t;
156
157         t = mat[0][1] ; 
158         mat[0][1] = mat[1][0] ; 
159         mat[1][0] = t;
160         t = mat[0][2] ; 
161         mat[0][2] = mat[2][0] ; 
162         mat[2][0] = t;
163         t = mat[0][3] ; 
164         mat[0][3] = mat[3][0] ; 
165         mat[3][0] = t;
166
167         t = mat[1][2] ; 
168         mat[1][2] = mat[2][1] ; 
169         mat[2][1] = t;
170         t = mat[1][3] ; 
171         mat[1][3] = mat[3][1] ; 
172         mat[3][1] = t;
173
174         t = mat[2][3] ; 
175         mat[2][3] = mat[3][2] ; 
176         mat[3][2] = t;
177 }
178
179
180 /*
181  * invertmat - 
182  *              computes the inverse of mat and puts it in inverse.  Returns 
183  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
184  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
185  *
186  *                                      Mark Segal - 1992
187  */
188
189 int Mat4Invert(float inverse[][4], float mat[][4])
190 {
191         int i, j, k;
192         double temp;
193         float tempmat[4][4];
194         float max;
195         int maxj;
196
197         /* Set inverse to identity */
198         for (i=0; i<4; i++)
199                 for (j=0; j<4; j++)
200                         inverse[i][j] = 0;
201         for (i=0; i<4; i++)
202                 inverse[i][i] = 1;
203
204         /* Copy original matrix so we don't mess it up */
205         for(i = 0; i < 4; i++)
206                 for(j = 0; j <4; j++)
207                         tempmat[i][j] = mat[i][j];
208
209         for(i = 0; i < 4; i++) {
210                 /* Look for row with max pivot */
211                 max = ABS(tempmat[i][i]);
212                 maxj = i;
213                 for(j = i + 1; j < 4; j++) {
214                         if(ABS(tempmat[j][i]) > max) {
215                                 max = ABS(tempmat[j][i]);
216                                 maxj = j;
217                         }
218                 }
219                 /* Swap rows if necessary */
220                 if (maxj != i) {
221                         for( k = 0; k < 4; k++) {
222                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
223                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
224                         }
225                 }
226
227                 temp = tempmat[i][i];
228                 if (temp == 0)
229                         return 0;  /* No non-zero pivot */
230                 for(k = 0; k < 4; k++) {
231                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
232                         inverse[i][k] = (float)(inverse[i][k]/temp);
233                 }
234                 for(j = 0; j < 4; j++) {
235                         if(j != i) {
236                                 temp = tempmat[j][i];
237                                 for(k = 0; k < 4; k++) {
238                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
239                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
240                                 }
241                         }
242                 }
243         }
244         return 1;
245 }
246 #ifdef TEST_ACTIVE
247 void Mat4InvertSimp(float inverse[][4], float mat[][4])
248 {
249         /* only for Matrices that have a rotation */
250         /* based at GG IV pag 205 */
251         float scale;
252         
253         scale= mat[0][0]*mat[0][0] + mat[1][0]*mat[1][0] + mat[2][0]*mat[2][0];
254         if(scale==0.0) return;
255         
256         scale= 1.0/scale;
257         
258         /* transpose and scale */
259         inverse[0][0]= scale*mat[0][0];
260         inverse[1][0]= scale*mat[0][1];
261         inverse[2][0]= scale*mat[0][2];
262         inverse[0][1]= scale*mat[1][0];
263         inverse[1][1]= scale*mat[1][1];
264         inverse[2][1]= scale*mat[1][2];
265         inverse[0][2]= scale*mat[2][0];
266         inverse[1][2]= scale*mat[2][1];
267         inverse[2][2]= scale*mat[2][2];
268
269         inverse[3][0]= -(inverse[0][0]*mat[3][0] + inverse[1][0]*mat[3][1] + inverse[2][0]*mat[3][2]);
270         inverse[3][1]= -(inverse[0][1]*mat[3][0] + inverse[1][1]*mat[3][1] + inverse[2][1]*mat[3][2]);
271         inverse[3][2]= -(inverse[0][2]*mat[3][0] + inverse[1][2]*mat[3][1] + inverse[2][2]*mat[3][2]);
272         
273         inverse[0][3]= inverse[1][3]= inverse[2][3]= 0.0;
274         inverse[3][3]= 1.0;
275 }
276 #endif
277 /*  struct Matrix4; */
278
279 #ifdef TEST_ACTIVE
280 /* this seems to be unused.. */
281
282 void Mat4Inv(float *m1, float *m2)
283 {
284
285 /* This gets me into trouble:  */
286         float mat1[3][3], mat2[3][3]; 
287         
288 /*      void Mat3Inv(); */
289 /*      void Mat3CpyMat4(); */
290 /*      void Mat4CpyMat3(); */
291
292         Mat3CpyMat4((float*)mat2,m2);
293         Mat3Inv((float*)mat1, (float*) mat2);
294         Mat4CpyMat3(m1, mat1);
295
296 }
297 #endif
298
299
300 float Det2x2(float a,float b,float c,float d)
301 {
302
303         return a*d - b*c;
304 }
305
306
307
308 float Det3x3(float a1, float a2, float a3,
309                          float b1, float b2, float b3,
310                          float c1, float c2, float c3 )
311 {
312         float ans;
313
314         ans = a1 * Det2x2( b2, b3, c2, c3 )
315             - b1 * Det2x2( a2, a3, c2, c3 )
316             + c1 * Det2x2( a2, a3, b2, b3 );
317
318         return ans;
319 }
320
321 float Det4x4(float m[][4])
322 {
323         float ans;
324         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
325
326         a1= m[0][0]; 
327         b1= m[0][1];
328         c1= m[0][2]; 
329         d1= m[0][3];
330
331         a2= m[1][0]; 
332         b2= m[1][1];
333         c2= m[1][2]; 
334         d2= m[1][3];
335
336         a3= m[2][0]; 
337         b3= m[2][1];
338         c3= m[2][2]; 
339         d3= m[2][3];
340
341         a4= m[3][0]; 
342         b4= m[3][1];
343         c4= m[3][2]; 
344         d4= m[3][3];
345
346         ans = a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
347             - b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
348             + c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
349             - d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
350
351         return ans;
352 }
353
354
355 void Mat4Adj(float out[][4], float in[][4])     /* out = ADJ(in) */
356 {
357         float a1, a2, a3, a4, b1, b2, b3, b4;
358         float c1, c2, c3, c4, d1, d2, d3, d4;
359
360         a1= in[0][0]; 
361         b1= in[0][1];
362         c1= in[0][2]; 
363         d1= in[0][3];
364
365         a2= in[1][0]; 
366         b2= in[1][1];
367         c2= in[1][2]; 
368         d2= in[1][3];
369
370         a3= in[2][0]; 
371         b3= in[2][1];
372         c3= in[2][2]; 
373         d3= in[2][3];
374
375         a4= in[3][0]; 
376         b4= in[3][1];
377         c4= in[3][2]; 
378         d4= in[3][3];
379
380
381         out[0][0]  =   Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
382         out[1][0]  = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
383         out[2][0]  =   Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
384         out[3][0]  = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
385
386         out[0][1]  = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
387         out[1][1]  =   Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
388         out[2][1]  = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
389         out[3][1]  =   Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
390
391         out[0][2]  =   Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
392         out[1][2]  = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
393         out[2][2]  =   Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
394         out[3][2]  = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
395
396         out[0][3]  = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
397         out[1][3]  =   Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
398         out[2][3]  = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
399         out[3][3]  =   Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
400 }
401
402 void Mat4InvGG(float out[][4], float in[][4])   /* from Graphic Gems I, out= INV(in)  */
403 {
404         int i, j;
405         float det;
406
407         /* calculate the adjoint matrix */
408
409         Mat4Adj(out,in);
410
411         det = Det4x4(out);
412
413         if ( fabs( det ) < SMALL_NUMBER) {
414                 return;
415         }
416
417         /* scale the adjoint matrix to get the inverse */
418
419         for (i=0; i<4; i++)
420                 for(j=0; j<4; j++)
421                         out[i][j] = out[i][j] / det;
422
423         /* the last factor is not always 1. For that reason an extra division should be implemented? */
424 }
425
426
427 void Mat3Inv(float m1[][3], float m2[][3])
428 {
429         short a,b;
430         float det;
431
432         /* calc adjoint */
433         Mat3Adj(m1,m2);
434
435         /* then determinant old matrix! */
436         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
437             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
438             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
439
440         if(det==0) det=1;
441         det= 1/det;
442         for(a=0;a<3;a++) {
443                 for(b=0;b<3;b++) {
444                         m1[a][b]*=det;
445                 }
446         }
447 }
448
449 void Mat3Adj(float m1[][3], float m[][3])
450 {
451         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
452         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
453         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
454
455         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
456         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
457         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
458
459         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
460         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
461         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
462 }
463
464 void Mat4MulMat4(float m1[][4], float m2[][4], float m3[][4])
465 {
466   /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
467
468         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];
469         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];
470         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];
471         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];
472
473         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];
474         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];
475         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];
476         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];
477
478         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];
479         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];
480         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];
481         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];
482
483         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];
484         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];
485         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];
486         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];
487
488 }
489 #ifdef TEST_ACTIVE
490 void subMat4MulMat4(float *m1, float *m2, float *m3)
491 {
492
493         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
494         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
495         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
496         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
497         m1+=4;
498         m2+=4;
499         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
500         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
501         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
502         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
503         m1+=4;
504         m2+=4;
505         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
506         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
507         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
508         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
509 }
510 #endif
511
512 #ifndef TEST_ACTIVE
513 void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3])
514 #else
515 void Mat3MulMat3(float *m1, float *m3, float *m2)
516 #endif
517 {
518    /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
519 #ifndef TEST_ACTIVE
520         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 
521         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 
522         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 
523
524         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 
525         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 
526         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 
527
528         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 
529         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 
530         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
531 #else
532         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
533         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
534         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
535         m1+=3;
536         m2+=3;
537         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
538         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
539         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
540         m1+=3;
541         m2+=3;
542         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
543         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
544         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
545 #endif
546 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
547
548 void Mat4MulMat43(float (*m1)[4], float (*m3)[4], float (*m2)[3])
549 {
550         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
551         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
552         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
553         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
554         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
555         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
556         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
557         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
558         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
559 }
560 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
561 void Mat3IsMat3MulMat4(float m1[][3], float m2[][3], float m3[][4])
562 {
563     /* m1[i][j] = m2[i][k] * m3[k][j] */
564     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
565     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
566     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
567
568     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
569     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
570     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
571
572     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
573     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
574     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
575 }
576
577
578
579 void Mat4MulMat34(float (*m1)[4], float (*m3)[3], float (*m2)[4])
580 {
581         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
582         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
583         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
584         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
585         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
586         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
587         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
588         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
589         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
590 }
591
592 void Mat4CpyMat4(float m1[][4], float m2[][4]) 
593 {
594         memcpy(m1, m2, 4*4*sizeof(float));
595 }
596
597 void Mat4SwapMat4(float *m1, float *m2)
598 {
599         float t;
600         int i;
601
602         for(i=0;i<16;i++) {
603                 t= *m1;
604                 *m1= *m2;
605                 *m2= t;
606                 m1++; 
607                 m2++;
608         }
609 }
610
611 typedef float Mat3Row[3];
612 typedef float Mat4Row[4];
613
614 #ifdef TEST_ACTIVE
615 void Mat3CpyMat4(float *m1p, float *m2p)
616 #else
617 void Mat3CpyMat4(float m1[][3], float m2[][4])
618 #endif
619 {
620 #ifdef TEST_ACTIVE
621         int i, j;
622         Mat3Row *m1= (Mat3Row *)m1p; 
623         Mat4Row *m2= (Mat4Row *)m2p; 
624         for ( i = 0; i++; i < 3) {
625                 for (j = 0; j++; j < 3) {
626                         m1p[3*i + j] = m2p[4*i + j];
627                 }
628         }                       
629 #endif
630         m1[0][0]= m2[0][0];
631         m1[0][1]= m2[0][1];
632         m1[0][2]= m2[0][2];
633
634         m1[1][0]= m2[1][0];
635         m1[1][1]= m2[1][1];
636         m1[1][2]= m2[1][2];
637
638         m1[2][0]= m2[2][0];
639         m1[2][1]= m2[2][1];
640         m1[2][2]= m2[2][2];
641 }
642
643 /* Butched. See .h for comment */
644 /*  void Mat4CpyMat3(float m1[][4], float m2[][3]) */
645 #ifdef TEST_ACTIVE
646 void Mat4CpyMat3(float* m1, float *m2)
647 {
648         int i;
649         for (i = 0; i < 3; i++) {
650                 m1[(4*i)]    = m2[(3*i)];
651                 m1[(4*i) + 1]= m2[(3*i) + 1];
652                 m1[(4*i) + 2]= m2[(3*i) + 2];
653                 m1[(4*i) + 3]= 0.0;
654                 i++;
655         }
656
657         m1[12]=m1[13]= m1[14]= 0.0;
658         m1[15]= 1.0;
659 }
660 #else
661
662 void Mat4CpyMat3(float m1[][4], float m2[][3])  /* no clear */
663 {
664         m1[0][0]= m2[0][0];
665         m1[0][1]= m2[0][1];
666         m1[0][2]= m2[0][2];
667
668         m1[1][0]= m2[1][0];
669         m1[1][1]= m2[1][1];
670         m1[1][2]= m2[1][2];
671
672         m1[2][0]= m2[2][0];
673         m1[2][1]= m2[2][1];
674         m1[2][2]= m2[2][2];
675
676         /*      Reevan's Bugfix */
677         m1[0][3]=0.0F;
678         m1[1][3]=0.0F;
679         m1[2][3]=0.0F;
680
681         m1[3][0]=0.0F;  
682         m1[3][1]=0.0F;  
683         m1[3][2]=0.0F;  
684         m1[3][3]=1.0F;
685
686
687 }
688 #endif
689
690 void Mat3CpyMat3(float m1[][3], float m2[][3]) 
691 {       
692         /* destination comes first: */
693         memcpy(&m1[0], &m2[0], 9*sizeof(float));
694 }
695
696 void Mat3MulSerie(float answ[][3],
697                                    float m1[][3], float m2[][3], float m3[][3],
698                                    float m4[][3], float m5[][3], float m6[][3],
699                                    float m7[][3], float m8[][3])
700 {
701         float temp[3][3];
702         
703         if(m1==0 || m2==0) return;
704
705         
706         Mat3MulMat3(answ, m2, m1);
707         if(m3) {
708                 Mat3MulMat3(temp, m3, answ);
709                 if(m4) {
710                         Mat3MulMat3(answ, m4, temp);
711                         if(m5) {
712                                 Mat3MulMat3(temp, m5, answ);
713                                 if(m6) {
714                                         Mat3MulMat3(answ, m6, temp);
715                                         if(m7) {
716                                                 Mat3MulMat3(temp, m7, answ);
717                                                 if(m8) {
718                                                         Mat3MulMat3(answ, m8, temp);
719                                                 }
720                                                 else Mat3CpyMat3(answ, temp);
721                                         }
722                                 }
723                                 else Mat3CpyMat3(answ, temp);
724                         }
725                 }
726                 else Mat3CpyMat3(answ, temp);
727         }
728 }
729
730 void Mat4MulSerie(float answ[][4], float m1[][4],
731                                 float m2[][4], float m3[][4], float m4[][4],
732                                 float m5[][4], float m6[][4], float m7[][4],
733                                 float m8[][4])
734 {
735         float temp[4][4];
736         
737         if(m1==0 || m2==0) return;
738         
739         Mat4MulMat4(answ, m2, m1);
740         if(m3) {
741                 Mat4MulMat4(temp, m3, answ);
742                 if(m4) {
743                         Mat4MulMat4(answ, m4, temp);
744                         if(m5) {
745                                 Mat4MulMat4(temp, m5, answ);
746                                 if(m6) {
747                                         Mat4MulMat4(answ, m6, temp);
748                                         if(m7) {
749                                                 Mat4MulMat4(temp, m7, answ);
750                                                 if(m8) {
751                                                         Mat4MulMat4(answ, m8, temp);
752                                                 }
753                                                 else Mat4CpyMat4(answ, temp);
754                                         }
755                                 }
756                                 else Mat4CpyMat4(answ, temp);
757                         }
758                 }
759                 else Mat4CpyMat4(answ, temp);
760         }
761 }
762
763 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
764 {
765         float squat[4], dquat[4], fquat[4];
766         float ssize[3], dsize[3], fsize[4];
767         float sloc[3], dloc[3], floc[3];
768         
769         Mat4ToQuat(dst, dquat);
770         Mat4ToSize(dst, dsize);
771         VecCopyf(dloc, dst[3]);
772
773         Mat4ToQuat(src, squat);
774         Mat4ToSize(src, ssize);
775         VecCopyf(sloc, src[3]);
776         
777         /* do blending */
778         VecLerpf(floc, dloc, sloc, srcweight);
779         QuatInterpol(fquat, dquat, squat, srcweight);
780         VecLerpf(fsize, dsize, ssize, srcweight);
781
782         /* compose new matrix */
783         LocQuatSizeToMat4(out, floc, fquat, fsize);
784 }
785
786 void Mat4Clr(float *m)
787 {
788         memset(m, 0, 4*4*sizeof(float));
789 }
790
791 void Mat3Clr(float *m)
792 {
793         memset(m, 0, 3*3*sizeof(float));
794 }
795
796 void Mat4One(float m[][4])
797 {
798
799         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
800         m[0][1]= m[0][2]= m[0][3]= 0.0;
801         m[1][0]= m[1][2]= m[1][3]= 0.0;
802         m[2][0]= m[2][1]= m[2][3]= 0.0;
803         m[3][0]= m[3][1]= m[3][2]= 0.0;
804 }
805
806 void Mat3One(float m[][3])
807 {
808
809         m[0][0]= m[1][1]= m[2][2]= 1.0;
810         m[0][1]= m[0][2]= 0.0;
811         m[1][0]= m[1][2]= 0.0;
812         m[2][0]= m[2][1]= 0.0;
813 }
814
815 void Mat4MulVec( float mat[][4], int *vec)
816 {
817         int x,y;
818
819         x=vec[0]; 
820         y=vec[1];
821         vec[0]=(int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]);
822         vec[1]=(int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]);
823         vec[2]=(int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]);
824 }
825
826 void Mat4MulVecfl( float mat[][4], float *vec)
827 {
828         float x,y;
829
830         x=vec[0]; 
831         y=vec[1];
832         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
833         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
834         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
835 }
836
837 void VecMat4MulVecfl(float *in, float mat[][4], float *vec)
838 {
839         float x,y;
840
841         x=vec[0]; 
842         y=vec[1];
843         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
844         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
845         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
846 }
847
848 void Mat4Mul3Vecfl( float mat[][4], float *vec)
849 {
850         float x,y;
851
852         x= vec[0]; 
853         y= vec[1];
854         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
855         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
856         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
857 }
858
859 void Mat4MulVec3Project(float mat[][4], float *vec)
860 {
861         float w;
862
863         w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
864         Mat4MulVecfl(mat, vec);
865
866         vec[0] /= w;
867         vec[1] /= w;
868         vec[2] /= w;
869 }
870
871 void Mat4MulVec4fl( float mat[][4], float *vec)
872 {
873         float x,y,z;
874
875         x=vec[0]; 
876         y=vec[1]; 
877         z= vec[2];
878         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
879         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
880         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
881         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
882 }
883
884 void Mat3MulVec( float mat[][3], int *vec)
885 {
886         int x,y;
887
888         x=vec[0]; 
889         y=vec[1];
890         vec[0]= (int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]);
891         vec[1]= (int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]);
892         vec[2]= (int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]);
893 }
894
895 void Mat3MulVecfl( float mat[][3], float *vec)
896 {
897         float x,y;
898
899         x=vec[0]; 
900         y=vec[1];
901         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
902         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
903         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
904 }
905
906 void Mat3MulVecd( float mat[][3], double *vec)
907 {
908         double x,y;
909
910         x=vec[0]; 
911         y=vec[1];
912         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
913         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
914         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
915 }
916
917 void Mat3TransMulVecfl( float mat[][3], float *vec)
918 {
919         float x,y;
920
921         x=vec[0]; 
922         y=vec[1];
923         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
924         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
925         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
926 }
927
928 void Mat3MulFloat(float *m, float f)
929 {
930         int i;
931
932         for(i=0;i<9;i++) m[i]*=f;
933 }
934
935 void Mat4MulFloat(float *m, float f)
936 {
937         int i;
938
939         for(i=0;i<12;i++) m[i]*=f;      /* count to 12: without vector component */
940 }
941
942
943 void Mat4MulFloat3(float *m, float f)           /* only scale component */
944 {
945         int i,j;
946
947         for(i=0; i<3; i++) {
948                 for(j=0; j<3; j++) {
949                         
950                         m[4*i+j] *= f;
951                 }
952         }
953 }
954
955 void VecStar(float mat[][3], float *vec)
956 {
957
958         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
959         mat[0][1]= -vec[2];     
960         mat[0][2]= vec[1];
961         mat[1][0]= vec[2];      
962         mat[1][2]= -vec[0];
963         mat[2][0]= -vec[1];     
964         mat[2][1]= vec[0];
965         
966 }
967 #ifdef TEST_ACTIVE
968 short EenheidsMat(float mat[][3])
969 {
970
971         if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0)
972                 if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0)
973                         if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0)
974                                 return 1;
975         return 0;
976 }
977 #endif
978
979 int FloatCompare( float *v1,  float *v2, float limit)
980 {
981
982         if( fabs(v1[0]-v2[0])<limit ) {
983                 if( fabs(v1[1]-v2[1])<limit ) {
984                         if( fabs(v1[2]-v2[2])<limit ) return 1;
985                 }
986         }
987         return 0;
988 }
989
990 float FloatLerpf( float target, float origin, float fac)
991 {
992         return (fac*target) + (1.0f-fac)*origin;
993 }
994
995 void printvecf( char *str,  float v[3])
996 {
997         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
998
999 }
1000
1001 void printquat( char *str,  float q[4])
1002 {
1003         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
1004
1005 }
1006
1007 void printvec4f( char *str,  float v[4])
1008 {
1009         printf("%s\n", str);
1010         printf("%f %f %f %f\n",v[0],v[1],v[2], v[3]);
1011         printf("\n");
1012
1013 }
1014
1015 void printmatrix4( char *str,  float m[][4])
1016 {
1017         printf("%s\n", str);
1018         printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
1019         printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
1020         printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
1021         printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
1022         printf("\n");
1023
1024 }
1025
1026 void printmatrix3( char *str,  float m[][3])
1027 {
1028         printf("%s\n", str);
1029         printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
1030         printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
1031         printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
1032         printf("\n");
1033
1034 }
1035
1036 /* **************** QUATERNIONS ********** */
1037
1038
1039 void QuatMul(float *q, float *q1, float *q2)
1040 {
1041         float t0,t1,t2;
1042
1043         t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
1044         t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
1045         t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
1046         q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
1047         q[0]=t0; 
1048         q[1]=t1; 
1049         q[2]=t2;
1050 }
1051
1052 /* Assumes a unit quaternion */
1053 void QuatMulVecf(float *q, float *v)
1054 {
1055         float t0, t1, t2;
1056
1057         t0=  -q[1]*v[0]-q[2]*v[1]-q[3]*v[2];
1058         t1=   q[0]*v[0]+q[2]*v[2]-q[3]*v[1];
1059         t2=   q[0]*v[1]+q[3]*v[0]-q[1]*v[2];
1060         v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0];
1061         v[0]=t1; 
1062         v[1]=t2;
1063
1064         t1=   t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
1065         t2=   t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
1066         v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
1067         v[0]=t1; 
1068         v[1]=t2;
1069 }
1070
1071 void QuatConj(float *q)
1072 {
1073         q[1] = -q[1];
1074         q[2] = -q[2];
1075         q[3] = -q[3];
1076 }
1077
1078 float QuatDot(float *q1, float *q2)
1079 {
1080         return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
1081 }
1082
1083 void QuatInv(float *q)
1084 {
1085         float f = QuatDot(q, q);
1086
1087         if (f == 0.0f)
1088                 return;
1089
1090         QuatConj(q);
1091         QuatMulf(q, 1.0f/f);
1092 }
1093
1094 void QuatMulf(float *q, float f)
1095 {
1096         q[0] *= f;
1097         q[1] *= f;
1098         q[2] *= f;
1099         q[3] *= f;
1100 }
1101
1102 void QuatSub(float *q, float *q1, float *q2)
1103 {
1104         q2[0]= -q2[0];
1105         QuatMul(q, q1, q2);
1106         q2[0]= -q2[0];
1107 }
1108
1109
1110 void QuatToMat3( float *q, float m[][3])
1111 {
1112         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1113
1114         q0= M_SQRT2 * q[0];
1115         q1= M_SQRT2 * q[1];
1116         q2= M_SQRT2 * q[2];
1117         q3= M_SQRT2 * q[3];
1118
1119         qda= q0*q1;
1120         qdb= q0*q2;
1121         qdc= q0*q3;
1122         qaa= q1*q1;
1123         qab= q1*q2;
1124         qac= q1*q3;
1125         qbb= q2*q2;
1126         qbc= q2*q3;
1127         qcc= q3*q3;
1128
1129         m[0][0]= (float)(1.0-qbb-qcc);
1130         m[0][1]= (float)(qdc+qab);
1131         m[0][2]= (float)(-qdb+qac);
1132
1133         m[1][0]= (float)(-qdc+qab);
1134         m[1][1]= (float)(1.0-qaa-qcc);
1135         m[1][2]= (float)(qda+qbc);
1136
1137         m[2][0]= (float)(qdb+qac);
1138         m[2][1]= (float)(-qda+qbc);
1139         m[2][2]= (float)(1.0-qaa-qbb);
1140 }
1141
1142
1143 void QuatToMat4( float *q, float m[][4])
1144 {
1145         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1146
1147         q0= M_SQRT2 * q[0];
1148         q1= M_SQRT2 * q[1];
1149         q2= M_SQRT2 * q[2];
1150         q3= M_SQRT2 * q[3];
1151
1152         qda= q0*q1;
1153         qdb= q0*q2;
1154         qdc= q0*q3;
1155         qaa= q1*q1;
1156         qab= q1*q2;
1157         qac= q1*q3;
1158         qbb= q2*q2;
1159         qbc= q2*q3;
1160         qcc= q3*q3;
1161
1162         m[0][0]= (float)(1.0-qbb-qcc);
1163         m[0][1]= (float)(qdc+qab);
1164         m[0][2]= (float)(-qdb+qac);
1165         m[0][3]= 0.0f;
1166
1167         m[1][0]= (float)(-qdc+qab);
1168         m[1][1]= (float)(1.0-qaa-qcc);
1169         m[1][2]= (float)(qda+qbc);
1170         m[1][3]= 0.0f;
1171
1172         m[2][0]= (float)(qdb+qac);
1173         m[2][1]= (float)(-qda+qbc);
1174         m[2][2]= (float)(1.0-qaa-qbb);
1175         m[2][3]= 0.0f;
1176         
1177         m[3][0]= m[3][1]= m[3][2]= 0.0f;
1178         m[3][3]= 1.0f;
1179 }
1180
1181 void Mat3ToQuat( float wmat[][3], float *q)             /* from Sig.Proc.85 pag 253 */
1182 {
1183         double tr, s;
1184         float mat[3][3];
1185
1186         /* work on a copy */
1187         Mat3CpyMat3(mat, wmat);
1188         Mat3Ortho(mat);                 /* this is needed AND a NormalQuat in the end */
1189         
1190         tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
1191         
1192         if(tr>FLT_EPSILON) {
1193                 s= sqrt( tr);
1194                 q[0]= (float)s;
1195                 s*= 4.0;
1196                 q[1]= (float)((mat[1][2]-mat[2][1])/s);
1197                 q[2]= (float)((mat[2][0]-mat[0][2])/s);
1198                 q[3]= (float)((mat[0][1]-mat[1][0])/s);
1199         }
1200         else {
1201                 q[0]= 0.0f;
1202                 s= -0.5*(mat[1][1]+mat[2][2]);
1203                 
1204                 if(s>FLT_EPSILON) {
1205                         s= sqrt(s);
1206                         q[1]= (float)s;
1207                         q[2]= (float)(mat[0][1]/(2*s));
1208                         q[3]= (float)(mat[0][2]/(2*s));
1209                 }
1210                 else {
1211                         q[1]= 0.0f;
1212                         s= 0.5*(1.0-mat[2][2]);
1213                         
1214                         if(s>FLT_EPSILON) {
1215                                 s= sqrt(s);
1216                                 q[2]= (float)s;
1217                                 q[3]= (float)(mat[1][2]/(2*s));
1218                         }
1219                         else {
1220                                 q[2]= 0.0f;
1221                                 q[3]= 1.0f;
1222                         }
1223                 }
1224         }
1225         NormalQuat(q);
1226 }
1227
1228 void Mat3ToQuat_is_ok( float wmat[][3], float *q)
1229 {
1230         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
1231
1232         /* work on a copy */
1233         Mat3CpyMat3(mat, wmat);
1234         Mat3Ortho(mat);
1235         
1236         /* rotate z-axis of matrix to z-axis */
1237
1238         nor[0] = mat[2][1];             /* cross product with (0,0,1) */
1239         nor[1] =  -mat[2][0];
1240         nor[2] = 0.0;
1241         Normalize(nor);
1242         
1243         co= mat[2][2];
1244         angle= 0.5f*saacos(co);
1245         
1246         co= (float)cos(angle);
1247         si= (float)sin(angle);
1248         q1[0]= co;
1249         q1[1]= -nor[0]*si;              /* negative here, but why? */
1250         q1[2]= -nor[1]*si;
1251         q1[3]= -nor[2]*si;
1252
1253         /* rotate back x-axis from mat, using inverse q1 */
1254         QuatToMat3(q1, matr);
1255         Mat3Inv(matn, matr);
1256         Mat3MulVecfl(matn, mat[0]);
1257         
1258         /* and align x-axes */
1259         angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1260         
1261         co= (float)cos(angle);
1262         si= (float)sin(angle);
1263         q2[0]= co;
1264         q2[1]= 0.0f;
1265         q2[2]= 0.0f;
1266         q2[3]= si;
1267         
1268         QuatMul(q, q1, q2);
1269 }
1270
1271
1272 void Mat4ToQuat( float m[][4], float *q)
1273 {
1274         float mat[3][3];
1275         
1276         Mat3CpyMat4(mat, m);
1277         Mat3ToQuat(mat, q);
1278         
1279 }
1280
1281 void QuatOne(float *q)
1282 {
1283         q[0]= q[2]= q[3]= 0.0;
1284         q[1]= 1.0;
1285 }
1286
1287 void NormalQuat(float *q)
1288 {
1289         float len;
1290         
1291         len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
1292         if(len!=0.0) {
1293                 q[0]/= len;
1294                 q[1]/= len;
1295                 q[2]/= len;
1296                 q[3]/= len;
1297         } else {
1298                 q[1]= 1.0f;
1299                 q[0]= q[2]= q[3]= 0.0f;                 
1300         }
1301 }
1302
1303 float *vectoquat( float *vec, short axis, short upflag)
1304 {
1305         static float q1[4];
1306         float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
1307         
1308         /* first rotate to axis */
1309         if(axis>2) {    
1310                 x2= vec[0] ; y2= vec[1] ; z2= vec[2];
1311                 axis-= 3;
1312         }
1313         else {
1314                 x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
1315         }
1316         
1317         q1[0]=1.0; 
1318         q1[1]=q1[2]=q1[3]= 0.0;
1319
1320         len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
1321         if(len1 == 0.0) return(q1);
1322
1323         /* nasty! I need a good routine for this...
1324          * problem is a rotation of an Y axis to the negative Y-axis for example.
1325          */
1326
1327         if(axis==0) {   /* x-axis */
1328                 nor[0]= 0.0;
1329                 nor[1]= -z2;
1330                 nor[2]= y2;
1331
1332                 if( fabs(y2)+fabs(z2)<0.0001 ) {
1333                         nor[1]= 1.0;
1334                 }
1335
1336                 co= x2;
1337         }
1338         else if(axis==1) {      /* y-axis */
1339                 nor[0]= z2;
1340                 nor[1]= 0.0;
1341                 nor[2]= -x2;
1342                 
1343                 if( fabs(x2)+fabs(z2)<0.0001 ) {
1344                         nor[2]= 1.0;
1345                 }
1346                 
1347                 co= y2;
1348         }
1349         else {                  /* z-axis */
1350                 nor[0]= -y2;
1351                 nor[1]= x2;
1352                 nor[2]= 0.0;
1353
1354                 if( fabs(x2)+fabs(y2)<0.0001 ) {
1355                         nor[0]= 1.0;
1356                 }
1357
1358                 co= z2;
1359         }
1360         co/= len1;
1361
1362         Normalize(nor);
1363         
1364         angle= 0.5f*saacos(co);
1365         si= (float)sin(angle);
1366         q1[0]= (float)cos(angle);
1367         q1[1]= nor[0]*si;
1368         q1[2]= nor[1]*si;
1369         q1[3]= nor[2]*si;
1370         
1371         if(axis!=upflag) {
1372                 QuatToMat3(q1, mat);
1373
1374                 fp= mat[2];
1375                 if(axis==0) {
1376                         if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1]));
1377                         else angle= (float)(-0.5*atan2(fp[1], fp[2]));
1378                 }
1379                 else if(axis==1) {
1380                         if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0]));
1381                         else angle= (float)(0.5*atan2(fp[0], fp[2]));
1382                 }
1383                 else {
1384                         if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0]));
1385                         else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
1386                 }
1387                                 
1388                 co= (float)cos(angle);
1389                 si= (float)(sin(angle)/len1);
1390                 q2[0]= co;
1391                 q2[1]= x2*si;
1392                 q2[2]= y2*si;
1393                 q2[3]= z2*si;
1394                         
1395                 QuatMul(q1,q2,q1);
1396         }
1397
1398         return(q1);
1399 }
1400
1401 void VecUpMat3old( float *vec, float mat[][3], short axis)
1402 {
1403         float inp, up[3];
1404         short cox = 0, coy = 0, coz = 0;
1405         
1406         /* using different up's is not useful, infact there is no real 'up'!
1407          */
1408
1409         up[0]= 0.0;
1410         up[1]= 0.0;
1411         up[2]= 1.0;
1412
1413         if(axis==0) {
1414                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1415         }
1416         if(axis==1) {
1417                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1418         }
1419         if(axis==2) {
1420                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1421         }
1422         if(axis==3) {
1423                 cox= 0; coy= 2; coz= 1;         /*  */
1424         }
1425         if(axis==4) {
1426                 cox= 1; coy= 0; coz= 2;         /*  */
1427         }
1428         if(axis==5) {
1429                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1430         }
1431
1432         mat[coz][0]= vec[0];
1433         mat[coz][1]= vec[1];
1434         mat[coz][2]= vec[2];
1435         Normalize((float *)mat[coz]);
1436         
1437         inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
1438         mat[coy][0]= up[0] - inp*mat[coz][0];
1439         mat[coy][1]= up[1] - inp*mat[coz][1];
1440         mat[coy][2]= up[2] - inp*mat[coz][2];
1441
1442         Normalize((float *)mat[coy]);
1443         
1444         Crossf(mat[cox], mat[coy], mat[coz]);
1445         
1446 }
1447
1448 void VecUpMat3(float *vec, float mat[][3], short axis)
1449 {
1450         float inp;
1451         short cox = 0, coy = 0, coz = 0;
1452
1453         /* using different up's is not useful, infact there is no real 'up'!
1454         */
1455
1456         if(axis==0) {
1457                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1458         }
1459         if(axis==1) {
1460                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1461         }
1462         if(axis==2) {
1463                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1464         }
1465         if(axis==3) {
1466                 cox= 0; coy= 1; coz= 2;         /* Y op -Z tr */
1467                 vec[0]= -vec[0];
1468                 vec[1]= -vec[1];
1469                 vec[2]= -vec[2];
1470         }
1471         if(axis==4) {
1472                 cox= 1; coy= 0; coz= 2;         /*  */
1473         }
1474         if(axis==5) {
1475                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1476         }
1477
1478         mat[coz][0]= vec[0];
1479         mat[coz][1]= vec[1];
1480         mat[coz][2]= vec[2];
1481         Normalize((float *)mat[coz]);
1482         
1483         inp= mat[coz][2];
1484         mat[coy][0]= - inp*mat[coz][0];
1485         mat[coy][1]= - inp*mat[coz][1];
1486         mat[coy][2]= 1.0f - inp*mat[coz][2];
1487
1488         Normalize((float *)mat[coy]);
1489         
1490         Crossf(mat[cox], mat[coy], mat[coz]);
1491         
1492 }
1493
1494 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
1495 void QuatInterpolW(float *, float *, float *, float );
1496
1497 void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
1498 {
1499         float omega, cosom, sinom, sc1, sc2;
1500
1501         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1502         
1503         /* rotate around shortest angle */
1504         if ((1.0 + cosom) > 0.0001) {
1505                 
1506                 if ((1.0 - cosom) > 0.0001) {
1507                         omega = acos(cosom);
1508                         sinom = sin(omega);
1509                         sc1 = sin((1.0 - t) * omega) / sinom;
1510                         sc2 = sin(t * omega) / sinom;
1511                 } 
1512                 else {
1513                         sc1 = 1.0 - t;
1514                         sc2 = t;
1515                 }
1516                 result[0] = sc1*quat1[0] + sc2*quat2[0];
1517                 result[1] = sc1*quat1[1] + sc2*quat2[1];
1518                 result[2] = sc1*quat1[2] + sc2*quat2[2];
1519                 result[3] = sc1*quat1[3] + sc2*quat2[3];
1520         } 
1521         else {
1522                 result[0] = quat2[3];
1523                 result[1] = -quat2[2];
1524                 result[2] = quat2[1];
1525                 result[3] = -quat2[0];
1526                 
1527                 sc1 = sin((1.0 - t)*M_PI_2);
1528                 sc2 = sin(t*M_PI_2);
1529
1530                 result[0] = sc1*quat1[0] + sc2*result[0];
1531                 result[1] = sc1*quat1[1] + sc2*result[1];
1532                 result[2] = sc1*quat1[2] + sc2*result[2];
1533                 result[3] = sc1*quat1[3] + sc2*result[3];
1534         }
1535 }
1536
1537 void QuatInterpol(float *result, float *quat1, float *quat2, float t)
1538 {
1539         float quat[4], omega, cosom, sinom, sc1, sc2;
1540
1541         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1542         
1543         /* rotate around shortest angle */
1544         if (cosom < 0.0) {
1545                 cosom = -cosom;
1546                 quat[0]= -quat1[0];
1547                 quat[1]= -quat1[1];
1548                 quat[2]= -quat1[2];
1549                 quat[3]= -quat1[3];
1550         } 
1551         else {
1552                 quat[0]= quat1[0];
1553                 quat[1]= quat1[1];
1554                 quat[2]= quat1[2];
1555                 quat[3]= quat1[3];
1556         }
1557         
1558         if ((1.0 - cosom) > 0.0001) {
1559                 omega = acos(cosom);
1560                 sinom = sin(omega);
1561                 sc1 = sin((1 - t) * omega) / sinom;
1562                 sc2 = sin(t * omega) / sinom;
1563         } else {
1564                 sc1= 1.0 - t;
1565                 sc2= t;
1566         }
1567         
1568         result[0] = sc1 * quat[0] + sc2 * quat2[0];
1569         result[1] = sc1 * quat[1] + sc2 * quat2[1];
1570         result[2] = sc1 * quat[2] + sc2 * quat2[2];
1571         result[3] = sc1 * quat[3] + sc2 * quat2[3];
1572 }
1573
1574 void QuatAdd(float *result, float *quat1, float *quat2, float t)
1575 {
1576         result[0]= quat1[0] + t*quat2[0];
1577         result[1]= quat1[1] + t*quat2[1];
1578         result[2]= quat1[2] + t*quat2[2];
1579         result[3]= quat1[3] + t*quat2[3];
1580 }
1581
1582 /* **************** VIEW / PROJECTION ********************************  */
1583
1584
1585 void i_ortho(
1586         float left, float right,
1587         float bottom, float top,
1588         float nearClip, float farClip,
1589         float matrix[][4]
1590 ){
1591     float Xdelta, Ydelta, Zdelta;
1592  
1593     Xdelta = right - left;
1594     Ydelta = top - bottom;
1595     Zdelta = farClip - nearClip;
1596     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1597                 return;
1598     }
1599     Mat4One(matrix);
1600     matrix[0][0] = 2.0f/Xdelta;
1601     matrix[3][0] = -(right + left)/Xdelta;
1602     matrix[1][1] = 2.0f/Ydelta;
1603     matrix[3][1] = -(top + bottom)/Ydelta;
1604     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1605     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1606 }
1607
1608 void i_window(
1609         float left, float right,
1610         float bottom, float top,
1611         float nearClip, float farClip,
1612         float mat[][4]
1613 ){
1614         float Xdelta, Ydelta, Zdelta;
1615
1616         Xdelta = right - left;
1617         Ydelta = top - bottom;
1618         Zdelta = farClip - nearClip;
1619
1620         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1621                 return;
1622         }
1623         mat[0][0] = nearClip * 2.0f/Xdelta;
1624         mat[1][1] = nearClip * 2.0f/Ydelta;
1625         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1626         mat[2][1] = (top + bottom)/Ydelta;
1627         mat[2][2] = -(farClip + nearClip)/Zdelta;
1628         mat[2][3] = -1.0f;
1629         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1630         mat[0][1] = mat[0][2] = mat[0][3] =
1631             mat[1][0] = mat[1][2] = mat[1][3] =
1632             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1633
1634 }
1635
1636 void i_translate(float Tx, float Ty, float Tz, float mat[][4])
1637 {
1638     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1639     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1640     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1641 }
1642
1643 void i_multmatrix( float icand[][4], float Vm[][4])
1644 {
1645     int row, col;
1646     float temp[4][4];
1647
1648     for(row=0 ; row<4 ; row++) 
1649         for(col=0 ; col<4 ; col++)
1650             temp[row][col] = icand[row][0] * Vm[0][col]
1651                            + icand[row][1] * Vm[1][col]
1652                            + icand[row][2] * Vm[2][col]
1653                            + icand[row][3] * Vm[3][col];
1654         Mat4CpyMat4(Vm, temp);
1655 }
1656
1657 void i_rotate(float angle, char axis, float mat[][4])
1658 {
1659         int col;
1660     float temp[4];
1661     float cosine, sine;
1662
1663     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
1664         temp[col] = 0;
1665
1666     angle = (float)(angle*(3.1415926535/180.0));
1667     cosine = (float)cos(angle);
1668     sine = (float)sin(angle);
1669     switch(axis){
1670     case 'x':    
1671     case 'X':    
1672         for(col=0 ; col<4 ; col++)
1673             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1674         for(col=0 ; col<4 ; col++) {
1675             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1676             mat[1][col] = temp[col];
1677         }
1678         break;
1679
1680     case 'y':
1681     case 'Y':
1682         for(col=0 ; col<4 ; col++)
1683             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1684         for(col=0 ; col<4 ; col++) {
1685             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1686             mat[0][col] = temp[col];
1687         }
1688         break;
1689
1690     case 'z':
1691     case 'Z':
1692         for(col=0 ; col<4 ; col++)
1693             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
1694         for(col=0 ; col<4 ; col++) {
1695             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
1696             mat[0][col] = temp[col];
1697         }
1698         break;
1699     }
1700 }
1701
1702 void i_polarview(float dist, float azimuth, float incidence, float twist, float Vm[][4])
1703 {
1704
1705         Mat4One(Vm);
1706
1707     i_translate(0.0, 0.0, -dist, Vm);
1708     i_rotate(-twist,'z', Vm);   
1709     i_rotate(-incidence,'x', Vm);
1710     i_rotate(-azimuth,'z', Vm);
1711 }
1712
1713 void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float twist, float mat[][4])
1714 {
1715         float sine, cosine, hyp, hyp1, dx, dy, dz;
1716         float mat1[4][4];
1717         
1718         Mat4One(mat);
1719         Mat4One(mat1);
1720
1721         i_rotate(-twist,'z', mat);
1722
1723         dx = px - vx;
1724         dy = py - vy;
1725         dz = pz - vz;
1726         hyp = dx * dx + dz * dz;        /* hyp squared  */
1727         hyp1 = (float)sqrt(dy*dy + hyp);
1728         hyp = (float)sqrt(hyp);         /* the real hyp */
1729         
1730         if (hyp1 != 0.0) {              /* rotate X     */
1731                 sine = -dy / hyp1;
1732                 cosine = hyp /hyp1;
1733         } else {
1734                 sine = 0;
1735                 cosine = 1.0f;
1736         }
1737         mat1[1][1] = cosine;
1738         mat1[1][2] = sine;
1739         mat1[2][1] = -sine;
1740         mat1[2][2] = cosine;
1741         
1742         i_multmatrix(mat1, mat);
1743
1744     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
1745     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
1746         
1747         /* paragraph    */
1748         if (hyp != 0.0f) {                      /* rotate Y     */
1749                 sine = dx / hyp;
1750                 cosine = -dz / hyp;
1751         } else {
1752                 sine = 0;
1753                 cosine = 1.0f;
1754         }
1755         mat1[0][0] = cosine;
1756         mat1[0][2] = -sine;
1757         mat1[2][0] = sine;
1758         mat1[2][2] = cosine;
1759         
1760         i_multmatrix(mat1, mat);
1761         i_translate(-vx,-vy,-vz, mat);  /* translate viewpoint to origin */
1762 }
1763
1764
1765
1766
1767
1768 /* ************************************************  */
1769
1770 void Mat3Ortho(float mat[][3])
1771 {       
1772         Normalize(mat[0]);
1773         Normalize(mat[1]);
1774         Normalize(mat[2]);
1775 }
1776
1777 void Mat4Ortho(float mat[][4])
1778 {
1779         float len;
1780         
1781         len= Normalize(mat[0]);
1782         if(len!=0.0) mat[0][3]/= len;
1783         len= Normalize(mat[1]);
1784         if(len!=0.0) mat[1][3]/= len;
1785         len= Normalize(mat[2]);
1786         if(len!=0.0) mat[2][3]/= len;
1787 }
1788
1789 void VecCopyf(float *v1, float *v2)
1790 {
1791         v1[0]= v2[0];
1792         v1[1]= v2[1];
1793         v1[2]= v2[2];
1794 }
1795
1796 int VecLen( int *v1, int *v2)
1797 {
1798         float x,y,z;
1799
1800         x=(float)(v1[0]-v2[0]);
1801         y=(float)(v1[1]-v2[1]);
1802         z=(float)(v1[2]-v2[2]);
1803         return (int)floor(sqrt(x*x+y*y+z*z));
1804 }
1805
1806 float VecLenf( float *v1, float *v2)
1807 {
1808         float x,y,z;
1809
1810         x=v1[0]-v2[0];
1811         y=v1[1]-v2[1];
1812         z=v1[2]-v2[2];
1813         return (float)sqrt(x*x+y*y+z*z);
1814 }
1815
1816 float VecLength(float *v)
1817 {
1818         return (float) sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
1819 }
1820
1821 void VecAddf(float *v, float *v1, float *v2)
1822 {
1823         v[0]= v1[0]+ v2[0];
1824         v[1]= v1[1]+ v2[1];
1825         v[2]= v1[2]+ v2[2];
1826 }
1827
1828 void VecSubf(float *v, float *v1, float *v2)
1829 {
1830         v[0]= v1[0]- v2[0];
1831         v[1]= v1[1]- v2[1];
1832         v[2]= v1[2]- v2[2];
1833 }
1834
1835 void VecLerpf(float *target, float *a, float *b, float t)
1836 {
1837         float s = 1.0f-t;
1838
1839         target[0]= s*a[0] + t*b[0];
1840         target[1]= s*a[1] + t*b[1];
1841         target[2]= s*a[2] + t*b[2];
1842 }
1843
1844 void VecMidf(float *v, float *v1, float *v2)
1845 {
1846         v[0]= 0.5f*(v1[0]+ v2[0]);
1847         v[1]= 0.5f*(v1[1]+ v2[1]);
1848         v[2]= 0.5f*(v1[2]+ v2[2]);
1849 }
1850
1851 void VecMulf(float *v1, float f)
1852 {
1853
1854         v1[0]*= f;
1855         v1[1]*= f;
1856         v1[2]*= f;
1857 }
1858
1859 int VecLenCompare(float *v1, float *v2, float limit)
1860 {
1861     float x,y,z;
1862
1863         x=v1[0]-v2[0];
1864         y=v1[1]-v2[1];
1865         z=v1[2]-v2[2];
1866
1867         return ((x*x + y*y + z*z) < (limit*limit));
1868 }
1869
1870 int VecCompare( float *v1, float *v2, float limit)
1871 {
1872         if( fabs(v1[0]-v2[0])<limit )
1873                 if( fabs(v1[1]-v2[1])<limit )
1874                         if( fabs(v1[2]-v2[2])<limit ) return 1;
1875         return 0;
1876 }
1877
1878 int VecEqual(float *v1, float *v2)
1879 {
1880         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
1881 }
1882
1883 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
1884 {
1885         float n1[3],n2[3];
1886
1887         n1[0]= (float)(v1[0]-v2[0]);
1888         n2[0]= (float)(v2[0]-v3[0]);
1889         n1[1]= (float)(v1[1]-v2[1]);
1890         n2[1]= (float)(v2[1]-v3[1]);
1891         n1[2]= (float)(v1[2]-v2[2]);
1892         n2[2]= (float)(v2[2]-v3[2]);
1893         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1894         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1895         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1896         Normalize(n);
1897 }
1898
1899 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
1900 {
1901         float n1[3],n2[3];
1902
1903         n1[0]= (float)(v1[0]-v2[0]);
1904         n2[0]= (float)(v2[0]-v3[0]);
1905         n1[1]= (float)(v1[1]-v2[1]);
1906         n2[1]= (float)(v2[1]-v3[1]);
1907         n1[2]= (float)(v1[2]-v2[2]);
1908         n2[2]= (float)(v2[2]-v3[2]);
1909         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1910         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1911         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1912         Normalize(n);
1913 }
1914
1915 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
1916 {
1917         float n1[3],n2[3];
1918
1919         n1[0]= v1[0]-v2[0];
1920         n2[0]= v2[0]-v3[0];
1921         n1[1]= v1[1]-v2[1];
1922         n2[1]= v2[1]-v3[1];
1923         n1[2]= v1[2]-v2[2];
1924         n2[2]= v2[2]-v3[2];
1925         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1926         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1927         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1928         return Normalize(n);
1929 }
1930
1931 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
1932 {
1933         /* real cross! */
1934         float n1[3],n2[3];
1935
1936         n1[0]= v1[0]-v3[0];
1937         n1[1]= v1[1]-v3[1];
1938         n1[2]= v1[2]-v3[2];
1939
1940         n2[0]= v2[0]-v4[0];
1941         n2[1]= v2[1]-v4[1];
1942         n2[2]= v2[2]-v4[2];
1943
1944         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1945         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1946         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1947
1948         return Normalize(n);
1949 }
1950
1951
1952 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
1953 {
1954
1955         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
1956         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
1957         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
1958 }
1959
1960 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
1961 {
1962
1963         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
1964         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
1965         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
1966 }
1967
1968 float Sqrt3f(float f)
1969 {
1970         if(f==0.0) return 0;
1971         if(f<0) return (float)(-exp(log(-f)/3));
1972         else return (float)(exp(log(f)/3));
1973 }
1974
1975 double Sqrt3d(double d)
1976 {
1977         if(d==0.0) return 0;
1978         if(d<0) return -exp(log(-d)/3);
1979         else return exp(log(d)/3);
1980 }
1981
1982 /* distance v1 to line v2-v3 */
1983 /* using Hesse formula, NO LINE PIECE! */
1984 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
1985         float a[2],deler;
1986
1987         a[0]= v2[1]-v3[1];
1988         a[1]= v3[0]-v2[0];
1989         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
1990         if(deler== 0.0f) return 0;
1991
1992         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
1993
1994 }
1995
1996 /* distance v1 to line-piece v2-v3 */
1997 float PdistVL2Dfl( float *v1, float *v2, float *v3) 
1998 {
1999         float labda, rc[2], pt[2], len;
2000         
2001         rc[0]= v3[0]-v2[0];
2002         rc[1]= v3[1]-v2[1];
2003         len= rc[0]*rc[0]+ rc[1]*rc[1];
2004         if(len==0.0) {
2005                 rc[0]= v1[0]-v2[0];
2006                 rc[1]= v1[1]-v2[1];
2007                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
2008         }
2009         
2010         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
2011         if(labda<=0.0) {
2012                 pt[0]= v2[0];
2013                 pt[1]= v2[1];
2014         }
2015         else if(labda>=1.0) {
2016                 pt[0]= v3[0];
2017                 pt[1]= v3[1];
2018         }
2019         else {
2020                 pt[0]= labda*rc[0]+v2[0];
2021                 pt[1]= labda*rc[1]+v2[1];
2022         }
2023
2024         rc[0]= pt[0]-v1[0];
2025         rc[1]= pt[1]-v1[1];
2026         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
2027 }
2028
2029 float AreaF2Dfl( float *v1, float *v2, float *v3)
2030 {
2031         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
2032 }
2033
2034
2035 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
2036 {
2037         float len, vec1[3], vec2[3], n[3];
2038
2039         VecSubf(vec1, v2, v1);
2040         VecSubf(vec2, v4, v1);
2041         Crossf(n, vec1, vec2);
2042         len= Normalize(n);
2043
2044         VecSubf(vec1, v4, v3);
2045         VecSubf(vec2, v2, v3);
2046         Crossf(n, vec1, vec2);
2047         len+= Normalize(n);
2048
2049         return (len/2.0f);
2050 }
2051
2052 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
2053 {
2054         float len, vec1[3], vec2[3], n[3];
2055
2056         VecSubf(vec1, v3, v2);
2057         VecSubf(vec2, v1, v2);
2058         Crossf(n, vec1, vec2);
2059         len= Normalize(n);
2060
2061         return (len/2.0f);
2062 }
2063
2064 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
2065 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
2066
2067
2068 float AreaPoly3Dfl(int nr, float *verts, float *normal)
2069 {
2070         float x, y, z, area, max;
2071         float *cur, *prev;
2072         int a, px=0, py=1;
2073
2074         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
2075         x= (float)fabs(normal[0]);
2076         y= (float)fabs(normal[1]);
2077         z= (float)fabs(normal[2]);
2078         max = MAX3(x, y, z);
2079         if(max==y) py=2;
2080         else if(max==x) {
2081                 px=1; 
2082                 py= 2;
2083         }
2084
2085         /* The Trapezium Area Rule */
2086         prev= verts+3*(nr-1);
2087         cur= verts;
2088         area= 0;
2089         for(a=0; a<nr; a++) {
2090                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
2091                 prev= cur;
2092                 cur+=3;
2093         }
2094
2095         return (float)fabs(0.5*area/max);
2096 }
2097
2098 /* intersect Line-Line, shorts */
2099 short IsectLL2Ds(short *v1, short *v2, short *v3, short *v4)
2100 {
2101         /* return:
2102         -1: colliniar
2103          0: no intersection of segments
2104          1: exact intersection of segments
2105          2: cross-intersection of segments
2106         */
2107         float div, labda, mu;
2108         
2109         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2110         if(div==0.0) return -1;
2111         
2112         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2113         
2114         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2115         
2116         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2117                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2118                 return 2;
2119         }
2120         return 0;
2121 }
2122
2123 /* intersect Line-Line, floats */
2124 short IsectLL2Df(float *v1, float *v2, float *v3, float *v4)
2125 {
2126         /* return:
2127         -1: colliniar
2128 0: no intersection of segments
2129 1: exact intersection of segments
2130 2: cross-intersection of segments
2131         */
2132         float div, labda, mu;
2133         
2134         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2135         if(div==0.0) return -1;
2136         
2137         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2138         
2139         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2140         
2141         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2142                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2143                 return 2;
2144         }
2145         return 0;
2146 }
2147
2148 void MinMax3(float *min, float *max, float *vec)
2149 {
2150         if(min[0]>vec[0]) min[0]= vec[0];
2151         if(min[1]>vec[1]) min[1]= vec[1];
2152         if(min[2]>vec[2]) min[2]= vec[2];
2153
2154         if(max[0]<vec[0]) max[0]= vec[0];
2155         if(max[1]<vec[1]) max[1]= vec[1];
2156         if(max[2]<vec[2]) max[2]= vec[2];
2157 }
2158
2159 static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
2160 {
2161         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
2162 }
2163
2164 static int BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
2165 {
2166         float xn, yn, zn, a1, a2, a3, asum;
2167         short i, j;
2168
2169         /* find best projection of face XY, XZ or YZ: barycentric weights of
2170            the 2d projected coords are the same and faster to compute */
2171         xn= fabs(n[0]);
2172         yn= fabs(n[1]);
2173         zn= fabs(n[2]);
2174         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
2175         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
2176         else {i= 1; j= 2;} 
2177
2178         a1= TriSignedArea(v2, v3, co, i, j);
2179         a2= TriSignedArea(v3, v1, co, i, j);
2180         a3= TriSignedArea(v1, v2, co, i, j);
2181
2182         asum= a1 + a2 + a3;
2183
2184         if (fabs(asum) < FLT_EPSILON) {
2185                 /* zero area triangle */
2186                 w[0]= w[1]= w[2]= 1.0f/3.0f;
2187                 return 1;
2188         }
2189
2190         asum= 1.0f/asum;
2191         w[0]= a1*asum;
2192         w[1]= a2*asum;
2193         w[2]= a3*asum;
2194
2195         return 0;
2196 }
2197
2198 void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
2199 {
2200         float w2[3];
2201
2202         w[0]= w[1]= w[2]= w[3]= 0.0f;
2203
2204         /* first check for exact match */
2205         if(VecEqual(co, v1))
2206                 w[0]= 1.0f;
2207         else if(VecEqual(co, v2))
2208                 w[1]= 1.0f;
2209         else if(VecEqual(co, v3))
2210                 w[2]= 1.0f;
2211         else if(v4 && VecEqual(co, v4))
2212                 w[3]= 1.0f;
2213         else {
2214                 /* otherwise compute barycentric interpolation weights */
2215                 float n1[3], n2[3], n[3];
2216                 int degenerate;
2217
2218                 VecSubf(n1, v1, v3);
2219                 if (v4) {
2220                         VecSubf(n2, v2, v4);
2221                 }
2222                 else {
2223                         VecSubf(n2, v2, v3);
2224                 }
2225                 Crossf(n, n1, n2);
2226
2227                 /* OpenGL seems to split this way, so we do too */
2228                 if (v4) {
2229                         degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
2230                         SWAP(float, w[2], w[3]);
2231
2232                         if(degenerate || (w[0] < 0.0f)) {
2233                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2234                                    so we interpolate using the other triangle */
2235                                 degenerate= BarycentricWeights(v2, v3, v4, co, n, w2);
2236
2237                                 if(!degenerate) {
2238                                         w[0]= 0.0f;
2239                                         w[1]= w2[0];
2240                                         w[2]= w2[1];
2241                                         w[3]= w2[2];
2242                                 }
2243                         }
2244                 }
2245                 else
2246                         BarycentricWeights(v1, v2, v3, co, n, w);
2247         }
2248
2249
2250 /* ************ EULER *************** */
2251
2252 void EulToMat3( float *eul, float mat[][3])
2253 {
2254         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2255         
2256         ci = cos(eul[0]); 
2257         cj = cos(eul[1]); 
2258         ch = cos(eul[2]);
2259         si = sin(eul[0]); 
2260         sj = sin(eul[1]); 
2261         sh = sin(eul[2]);
2262         cc = ci*ch; 
2263         cs = ci*sh; 
2264         sc = si*ch; 
2265         ss = si*sh;
2266
2267         mat[0][0] = (float)(cj*ch); 
2268         mat[1][0] = (float)(sj*sc-cs); 
2269         mat[2][0] = (float)(sj*cc+ss);
2270         mat[0][1] = (float)(cj*sh); 
2271         mat[1][1] = (float)(sj*ss+cc); 
2272         mat[2][1] = (float)(sj*cs-sc);
2273         mat[0][2] = (float)-sj;  
2274         mat[1][2] = (float)(cj*si);    
2275         mat[2][2] = (float)(cj*ci);
2276
2277 }
2278
2279 void EulToMat4( float *eul,float mat[][4])
2280 {
2281         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2282         
2283         ci = cos(eul[0]); 
2284         cj = cos(eul[1]); 
2285         ch = cos(eul[2]);
2286         si = sin(eul[0]); 
2287         sj = sin(eul[1]); 
2288         sh = sin(eul[2]);
2289         cc = ci*ch; 
2290         cs = ci*sh; 
2291         sc = si*ch; 
2292         ss = si*sh;
2293
2294         mat[0][0] = (float)(cj*ch); 
2295         mat[1][0] = (float)(sj*sc-cs); 
2296         mat[2][0] = (float)(sj*cc+ss);
2297         mat[0][1] = (float)(cj*sh); 
2298         mat[1][1] = (float)(sj*ss+cc); 
2299         mat[2][1] = (float)(sj*cs-sc);
2300         mat[0][2] = (float)-sj;  
2301         mat[1][2] = (float)(cj*si);    
2302         mat[2][2] = (float)(cj*ci);
2303
2304
2305         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2306         mat[3][3]= 1.0f;
2307 }
2308
2309 /* returns two euler calculation methods, so we can pick the best */
2310 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
2311 {
2312         float cy, quat[4], mat[3][3];
2313         
2314         Mat3ToQuat(tmat, quat);
2315         QuatToMat3(quat, mat);
2316         Mat3CpyMat3(mat, tmat);
2317         Mat3Ortho(mat);
2318         
2319         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2320         
2321         if (cy > 16.0*FLT_EPSILON) {
2322                 
2323                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2324                 eul1[1] = (float)atan2(-mat[0][2], cy);
2325                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2326                 
2327                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2328                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2329                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2330                 
2331         } else {
2332                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
2333                 eul1[1] = (float)atan2(-mat[0][2], cy);
2334                 eul1[2] = 0.0f;
2335                 
2336                 VecCopyf(eul2, eul1);
2337         }
2338 }
2339
2340 void Mat3ToEul(float tmat[][3], float *eul)
2341 {
2342         float eul1[3], eul2[3];
2343         
2344         mat3_to_eul2(tmat, eul1, eul2);
2345                 
2346         /* return best, which is just the one with lowest values it in */
2347         if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2348                 VecCopyf(eul, eul2);
2349         }
2350         else {
2351                 VecCopyf(eul, eul1);
2352         }
2353 }
2354
2355 void Mat4ToEul(float tmat[][4], float *eul)
2356 {
2357         float tempMat[3][3];
2358
2359         Mat3CpyMat4 (tempMat, tmat);
2360         Mat3Ortho(tempMat);
2361         Mat3ToEul(tempMat, eul);
2362 }
2363
2364 void QuatToEul( float *quat, float *eul)
2365 {
2366         float mat[3][3];
2367         
2368         QuatToMat3(quat, mat);
2369         Mat3ToEul(mat, eul);
2370 }
2371
2372
2373 void EulToQuat( float *eul, float *quat)
2374 {
2375     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2376  
2377     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2378     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2379     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2380     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2381         
2382         quat[0] = cj*cc + sj*ss;
2383         quat[1] = cj*sc - sj*cs;
2384         quat[2] = cj*ss + sj*cc;
2385         quat[3] = cj*cs - sj*sc;
2386 }
2387
2388 void VecRotToMat3( float *vec, float phi, float mat[][3])
2389 {
2390         /* rotation of phi radials around vec */
2391         float vx, vx2, vy, vy2, vz, vz2, co, si;
2392         
2393         vx= vec[0];
2394         vy= vec[1];
2395         vz= vec[2];
2396         vx2= vx*vx;
2397         vy2= vy*vy;
2398         vz2= vz*vz;
2399         co= (float)cos(phi);
2400         si= (float)sin(phi);
2401         
2402         mat[0][0]= vx2+co*(1.0f-vx2);
2403         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2404         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2405         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2406         mat[1][1]= vy2+co*(1.0f-vy2);
2407         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2408         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2409         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2410         mat[2][2]= vz2+co*(1.0f-vz2);
2411         
2412 }
2413
2414 void VecRotToMat4( float *vec, float phi, float mat[][4])
2415 {
2416         float tmat[3][3];
2417         
2418         VecRotToMat3(vec, phi, tmat);
2419         Mat4One(mat);
2420         Mat4CpyMat3(mat, tmat);
2421 }
2422
2423 void VecRotToQuat( float *vec, float phi, float *quat)
2424 {
2425         /* rotation of phi radials around vec */
2426         float si;
2427
2428         quat[1]= vec[0];
2429         quat[2]= vec[1];
2430         quat[3]= vec[2];
2431         
2432         if( Normalize(quat+1) == 0.0) {
2433                 QuatOne(quat);
2434         }
2435         else {
2436                 quat[0]= (float)cos( phi/2.0 );
2437                 si= (float)sin( phi/2.0 );
2438                 quat[1] *= si;
2439                 quat[2] *= si;
2440                 quat[3] *= si;
2441         }
2442 }
2443
2444 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
2445    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
2446    this would return the angle at the elbow */
2447 float VecAngle3(float *v1, float *v2, float *v3)
2448 {
2449         float vec1[3], vec2[3];
2450
2451         VecSubf(vec1, v2, v1);
2452         VecSubf(vec2, v2, v3);
2453         Normalize(vec1);
2454         Normalize(vec2);
2455
2456         return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
2457 }
2458
2459 /* Return the shortest angle in degrees between the 2 vectors */
2460 float VecAngle2(float *v1, float *v2)
2461 {
2462         float vec1[3], vec2[3];
2463
2464         VecCopyf(vec1, v1);
2465         VecCopyf(vec2, v2);
2466         Normalize(vec1);
2467         Normalize(vec2);
2468
2469         return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
2470 }
2471
2472 float NormalizedVecAngle2(float *v1, float *v2)
2473 {
2474         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
2475         if (Inpf(v1, v2) < 0.0f) {
2476                 float vec[3];
2477                 
2478                 vec[0]= -v2[0];
2479                 vec[1]= -v2[1];
2480                 vec[2]= -v2[2];
2481
2482                 return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
2483         }
2484         else
2485                 return 2.0f*saasin(VecLenf(v2, v1)/2.0);
2486 }
2487
2488 void euler_rot(float *beul, float ang, char axis)
2489 {
2490         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2491         
2492         eul[0]= eul[1]= eul[2]= 0.0;
2493         if(axis=='x') eul[0]= ang;
2494         else if(axis=='y') eul[1]= ang;
2495         else eul[2]= ang;
2496         
2497         EulToMat3(eul, mat1);
2498         EulToMat3(beul, mat2);
2499         
2500         Mat3MulMat3(totmat, mat2, mat1);
2501         
2502         Mat3ToEul(totmat, beul);
2503         
2504 }
2505
2506 /* exported to transform.c */
2507 void compatible_eul(float *eul, float *oldrot)
2508 {
2509         float dx, dy, dz;
2510         
2511         /* correct differences of about 360 degrees first */
2512         
2513         dx= eul[0] - oldrot[0];
2514         dy= eul[1] - oldrot[1];
2515         dz= eul[2] - oldrot[2];
2516         
2517         while( fabs(dx) > 5.1) {
2518                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2519                 dx= eul[0] - oldrot[0];
2520         }
2521         while( fabs(dy) > 5.1) {
2522                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2523                 dy= eul[1] - oldrot[1];
2524         }
2525         while( fabs(dz) > 5.1 ) {
2526                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2527                 dz= eul[2] - oldrot[2];
2528         }
2529         
2530         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
2531         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
2532                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2533         }
2534         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
2535                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2536         }
2537         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
2538                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2539         }
2540         
2541         /* the method below was there from ancient days... but why! probably because the code sucks :)
2542                 */
2543 #if 0   
2544         /* calc again */
2545         dx= eul[0] - oldrot[0];
2546         dy= eul[1] - oldrot[1];
2547         dz= eul[2] - oldrot[2];
2548         
2549         /* special case, tested for x-z  */
2550         
2551         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
2552                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2553                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
2554                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2555                 
2556         }
2557         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
2558                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2559                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2560                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
2561         }
2562         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
2563                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
2564                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2565                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2566         }
2567 #endif  
2568 }
2569
2570 /* uses 2 methods to retrieve eulers, and picks the closest */
2571 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
2572 {
2573         float eul1[3], eul2[3];
2574         float d1, d2;
2575         
2576         mat3_to_eul2(mat, eul1, eul2);
2577         
2578         compatible_eul(eul1, oldrot);
2579         compatible_eul(eul2, oldrot);
2580         
2581         d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
2582         d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
2583         
2584         /* return best, which is just the one with lowest difference */
2585         if( d1 > d2) {
2586                 VecCopyf(eul, eul2);
2587         }
2588         else {
2589                 VecCopyf(eul, eul1);
2590         }
2591         
2592 }
2593
2594 /* ******************************************** */
2595
2596 void SizeToMat3( float *size, float mat[][3])
2597 {
2598         mat[0][0]= size[0];
2599         mat[0][1]= 0.0;
2600         mat[0][2]= 0.0;
2601         mat[1][1]= size[1];
2602         mat[1][0]= 0.0;
2603         mat[1][2]= 0.0;
2604         mat[2][2]= size[2];
2605         mat[2][1]= 0.0;
2606         mat[2][0]= 0.0;
2607 }
2608
2609 void SizeToMat4( float *size, float mat[][4])
2610 {
2611         float tmat[3][3];
2612         
2613         SizeToMat3(size, tmat);
2614         Mat4One(mat);
2615         Mat4CpyMat3(mat, tmat);
2616 }
2617
2618 void Mat3ToSize( float mat[][3], float *size)
2619 {
2620         float vec[3];
2621
2622         VecCopyf(vec, mat[0]);
2623         size[0]= Normalize(vec);
2624         VecCopyf(vec, mat[1]);
2625         size[1]= Normalize(vec);
2626         VecCopyf(vec, mat[2]);
2627         size[2]= Normalize(vec);
2628
2629 }
2630
2631 void Mat4ToSize( float mat[][4], float *size)
2632 {
2633         float vec[3];
2634         
2635
2636         VecCopyf(vec, mat[0]);
2637         size[0]= Normalize(vec);
2638         VecCopyf(vec, mat[1]);
2639         size[1]= Normalize(vec);
2640         VecCopyf(vec, mat[2]);
2641         size[2]= Normalize(vec);
2642 }
2643
2644 /* ************* SPECIALS ******************* */
2645
2646 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
2647 {
2648         /* imaginary x-axis, y-axis triangle is being rotated */
2649         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
2650         
2651         /* move z-axis to face-normal */
2652         CalcNormFloat(v1, v2, v3, vec);
2653
2654         n[0]= vec[1];
2655         n[1]= -vec[0];
2656         n[2]= 0.0;
2657         Normalize(n);
2658         
2659         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
2660         
2661         angle= -0.5f*saacos(vec[2]);
2662         co= (float)cos(angle);
2663         si= (float)sin(angle);
2664         q1[0]= co;
2665         q1[1]= n[0]*si;
2666         q1[2]= n[1]*si;
2667         q1[3]= 0.0f;
2668         
2669         /* rotate back line v1-v2 */
2670         QuatToMat3(q1, mat);
2671         Mat3Inv(imat, mat);
2672         VecSubf(vec, v2, v1);
2673         Mat3MulVecfl(imat, vec);
2674
2675         /* what angle has this line with x-axis? */
2676         vec[2]= 0.0;
2677         Normalize(vec);
2678
2679         angle= (float)(0.5*atan2(vec[1], vec[0]));
2680         co= (float)cos(angle);
2681         si= (float)sin(angle);
2682         q2[0]= co;
2683         q2[1]= 0.0f;
2684         q2[2]= 0.0f;
2685         q2[3]= si;
2686         
2687         QuatMul(quat, q1, q2);
2688 }
2689
2690 void MinMaxRGB(short c[])
2691 {
2692         if(c[0]>255) c[0]=255;
2693         else if(c[0]<0) c[0]=0;
2694         if(c[1]>255) c[1]=255;
2695         else if(c[1]<0) c[1]=0;
2696         if(c[2]>255) c[2]=255;
2697         else if(c[2]<0) c[2]=0;
2698 }
2699
2700 float Vec2Lenf(float *v1, float *v2)
2701 {
2702         float x, y;
2703
2704         x = v1[0]-v2[0];
2705         y = v1[1]-v2[1];
2706         return (float)sqrt(x*x+y*y);
2707 }
2708
2709 void Vec2Mulf(float *v1, float f)
2710 {
2711         v1[0]*= f;
2712         v1[1]*= f;
2713 }
2714
2715 void Vec2Addf(float *v, float *v1, float *v2)
2716 {
2717         v[0]= v1[0]+ v2[0];
2718         v[1]= v1[1]+ v2[1];
2719 }
2720
2721 void Vec2Subf(float *v, float *v1, float *v2)
2722 {
2723         v[0]= v1[0]- v2[0];
2724         v[1]= v1[1]- v2[1];
2725 }
2726
2727 void Vec2Copyf(float *v1, float *v2)
2728 {
2729         v1[0]= v2[0];
2730         v1[1]= v2[1];
2731 }
2732
2733 float Inp2f(float *v1, float *v2)
2734 {
2735         return v1[0]*v2[0]+v1[1]*v2[1];
2736 }
2737
2738 float Normalize2(float *n)
2739 {
2740         float d;
2741         
2742         d= n[0]*n[0]+n[1]*n[1];
2743
2744         if(d>1.0e-35F) {
2745                 d= (float)sqrt(d);
2746
2747                 n[0]/=d; 
2748                 n[1]/=d; 
2749         } else {
2750                 n[0]=n[1]= 0.0;
2751                 d= 0.0;
2752         }
2753         return d;
2754 }
2755
2756 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
2757 {
2758         int i;
2759         float f, p, q, t;
2760
2761         h *= 360.0f;
2762         
2763         if(s==0.0) {
2764                 *r = v;
2765                 *g = v;
2766                 *b = v;
2767         }
2768         else {
2769                 if(h==360) h = 0;
2770                 
2771                 h /= 60;
2772                 i = (int)floor(h);
2773                 f = h - i;
2774                 p = v*(1.0f-s);
2775                 q = v*(1.0f-(s*f));
2776                 t = v*(1.0f-(s*(1.0f-f)));
2777                 
2778                 switch (i) {
2779                 case 0 :
2780                         *r = v;
2781                         *g = t;
2782                         *b = p;
2783                         break;
2784                 case 1 :
2785                         *r = q;
2786                         *g = v;
2787                         *b = p;
2788                         break;
2789                 case 2 :
2790                         *r = p;
2791                         *g = v;
2792                         *b = t;
2793                         break;
2794                 case 3 :
2795                         *r = p;
2796                         *g = q;
2797                         *b = v;
2798                         break;
2799                 case 4 :
2800                         *r = t;
2801                         *g = p;
2802                         *b = v;
2803                         break;
2804                 case 5 :
2805                         *r = v;
2806                         *g = p;
2807                         *b = q;
2808                         break;
2809                 }
2810         }
2811 }
2812
2813 void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
2814 {
2815         float y, u, v;
2816         y= 0.299*r + 0.587*g + 0.114*b;
2817         u=-0.147*r - 0.289*g + 0.436*b;
2818         v= 0.615*r - 0.515*g - 0.100*b;
2819         
2820         *ly=y;
2821         *lu=u;
2822         *lv=v;
2823 }
2824
2825 void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
2826 {
2827         float r, g, b;
2828         r=y+1.140*v;
2829         g=y-0.394*u - 0.581*v;
2830         b=y+2.032*u;
2831         
2832         *lr=r;
2833         *lg=g;
2834         *lb=b;
2835 }
2836
2837 void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
2838 {
2839         float sr,sg, sb;
2840         float y, cr, cb;
2841         
2842         sr=255.0*r;
2843         sg=255.0*g;
2844         sb=255.0*b;
2845         
2846         
2847         y=(0.257*sr)+(0.504*sg)+(0.098*sb)+16.0;
2848         cb=(-0.148*sr)-(0.291*sg)+(0.439*sb)+128.0;
2849         cr=(0.439*sr)-(0.368*sg)-(0.071*sb)+128.0;
2850         
2851         *ly=y;
2852         *lcb=cb;
2853         *lcr=cr;
2854 }
2855
2856 void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
2857 {
2858         float r,g,b;
2859         
2860         r=1.164*(y-16)+1.596*(cr-128);
2861         g=1.164*(y-16)-0.813*(cr-128)-0.392*(cb-128);
2862         b=1.164*(y-16)+2.017*(cb-128);
2863         
2864         *lr=r/255.0;
2865         *lg=g/255.0;
2866         *lb=b/255.0;
2867 }
2868
2869 void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
2870 {
2871         unsigned int ri, gi, bi;
2872         
2873         if (hexcol[0] == '#') hexcol++;
2874         
2875         if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
2876                 *r = ri / 255.0;
2877                 *g = gi / 255.0;                
2878                 *b = bi / 255.0;
2879         }
2880 }
2881
2882 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
2883 {
2884         float h, s, v;
2885         float cmax, cmin, cdelta;
2886         float rc, gc, bc;
2887
2888         cmax = r;
2889         cmin = r;
2890         cmax = (g>cmax ? g:cmax);
2891         cmin = (g<cmin ? g:cmin);
2892         cmax = (b>cmax ? b:cmax);
2893         cmin = (b<cmin ? b:cmin);
2894
2895         v = cmax;               /* value */
2896         if (cmax!=0.0)
2897                 s = (cmax - cmin)/cmax;
2898         else {
2899                 s = 0.0;
2900                 h = 0.0;
2901         }
2902         if (s == 0.0)
2903                 h = -1.0;
2904         else {
2905                 cdelta = cmax-cmin;
2906                 rc = (cmax-r)/cdelta;
2907                 gc = (cmax-g)/cdelta;
2908                 bc = (cmax-b)/cdelta;
2909                 if (r==cmax)
2910                         h = bc-gc;
2911                 else
2912                         if (g==cmax)
2913                                 h = 2.0f+rc-bc;
2914                         else
2915                                 h = 4.0f+gc-rc;
2916                 h = h*60.0f;
2917                 if (h<0.0f)
2918                         h += 360.0f;
2919         }
2920         
2921         *ls = s;
2922         *lh = h/360.0f;
2923         if( *lh < 0.0) *lh= 0.0;
2924         *lv = v;
2925 }
2926
2927
2928 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
2929    for that reason it is sensitive for endianness... with this function it works correctly
2930 */
2931
2932 unsigned int hsv_to_cpack(float h, float s, float v)
2933 {
2934         short r, g, b;
2935         float rf, gf, bf;
2936         unsigned int col;
2937         
2938         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
2939         
2940         r= (short)(rf*255.0f);
2941         g= (short)(gf*255.0f);
2942         b= (short)(bf*255.0f);
2943         
2944         col= ( r + (g*256) + (b*256*256) );
2945         return col;
2946 }
2947
2948
2949 unsigned int rgb_to_cpack(float r, float g, float b)
2950 {
2951         int ir, ig, ib;
2952         
2953         ir= (int)floor(255.0*r);
2954         if(ir<0) ir= 0; else if(ir>255) ir= 255;
2955         ig= (int)floor(255.0*g);
2956         if(ig<0) ig= 0; else if(ig>255) ig= 255;
2957         ib= (int)floor(255.0*b);
2958         if(ib<0) ib= 0; else if(ib>255) ib= 255;
2959         
2960         return (ir+ (ig*256) + (ib*256*256));
2961 }
2962
2963 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
2964 {
2965         
2966         *r= (float)((col)&0xFF);
2967         *r /= 255.0f;
2968
2969         *g= (float)(((col)>>8)&0xFF);
2970         *g /= 255.0f;
2971
2972         *b= (float)(((col)>>16)&0xFF);
2973         *b /= 255.0f;
2974 }
2975
2976
2977 /* *************** PROJECTIONS ******************* */
2978
2979 void tubemap(float x, float y, float z, float *u, float *v)
2980 {
2981         float len;
2982         
2983         *v = (z + 1.0) / 2.0;
2984         
2985         len= sqrt(x*x+y*y);
2986         if(len>0) {
2987                 *u = (1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0;
2988         }
2989 }
2990
2991 /* ------------------------------------------------------------------------- */
2992
2993 void spheremap(float x, float y, float z, float *u, float *v)
2994 {
2995         float len;
2996         
2997         len= sqrt(x*x+y*y+z*z);
2998         if(len>0.0) {
2999                 
3000                 if(x==0.0 && y==0.0) *u= 0.0;   /* othwise domain error */
3001                 else *u = (1.0 - atan2(x,y)/M_PI )/2.0;
3002                 
3003                 z/=len;
3004                 *v = 1.0- saacos(z)/M_PI;
3005         }
3006 }
3007
3008 /* ------------------------------------------------------------------------- */
3009
3010 /* *****************  m1 = m2 *****************  */
3011 void cpy_m3_m3(float m1[][3], float m2[][3]) 
3012 {       
3013         memcpy(m1[0], m2[0], 9*sizeof(float));
3014 }
3015
3016 /* *****************  m1 = m2 *****************  */
3017 void cpy_m4_m4(float m1[][4], float m2[][4]) 
3018 {       
3019         memcpy(m1[0], m2[0], 16*sizeof(float));
3020 }
3021
3022 /* ***************** identity matrix *****************  */
3023 void ident_m4(float m[][4])
3024 {
3025         
3026         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
3027         m[0][1]= m[0][2]= m[0][3]= 0.0;
3028         m[1][0]= m[1][2]= m[1][3]= 0.0;
3029         m[2][0]= m[2][1]= m[2][3]= 0.0;
3030         m[3][0]= m[3][1]= m[3][2]= 0.0;
3031 }
3032
3033
3034 /* *****************  m1 = m2 (pre) * m3 (post) ***************** */
3035 void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
3036 {
3037         float m[3][3];
3038         
3039         m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2]; 
3040         m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2]; 
3041         m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2]; 
3042
3043         m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2]; 
3044         m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2]; 
3045         m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2]; 
3046
3047         m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2]; 
3048         m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2]; 
3049         m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2]; 
3050
3051         cpy_m3_m3(m1, m2);
3052 }
3053
3054 /*  ***************** m1 = m2 (pre) * m3 (post) ***************** */
3055 void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
3056 {
3057         float m[4][4];
3058         
3059         m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2] + m2[3][0]*m3[0][3]; 
3060         m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2] + m2[3][1]*m3[0][3]; 
3061         m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2] + m2[3][2]*m3[0][3]; 
3062         m[0][3]= m2[0][3]*m3[0][0] + m2[1][3]*m3[0][1] + m2[2][3]*m3[0][2] + m2[3][3]*m3[0][3]; 
3063
3064         m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2] + m2[3][0]*m3[1][3]; 
3065         m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2] + m2[3][1]*m3[1][3]; 
3066         m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2] + m2[3][2]*m3[1][3]; 
3067         m[1][3]= m2[0][3]*m3[1][0] + m2[1][3]*m3[1][1] + m2[2][3]*m3[1][2] + m2[3][3]*m3[1][3]; 
3068
3069         m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2] + m2[3][0]*m3[2][3]; 
3070         m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2] + m2[3][1]*m3[2][3]; 
3071         m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2] + m2[3][2]*m3[2][3]; 
3072         m[2][3]= m2[0][3]*m3[2][0] + m2[1][3]*m3[2][1] + m2[2][3]*m3[2][2] + m2[3][3]*m3[2][3]; 
3073         
3074         m[3][0]= m2[0][0]*m3[3][0] + m2[1][0]*m3[3][1] + m2[2][0]*m3[3][2] + m2[3][0]*m3[3][3]; 
3075         m[3][1]= m2[0][1]*m3[3][0] + m2[1][1]*m3[3][1] + m2[2][1]*m3[3][2] + m2[3][1]*m3[3][3]; 
3076         m[3][2]= m2[0][2]*m3[3][0] + m2[1][2]*m3[3][1] + m2[2][2]*m3[3][2] + m2[3][2]*m3[3][3]; 
3077         m[3][3]= m2[0][3]*m3[3][0] + m2[1][3]*m3[3][1] + m2[2][3]*m3[3][2] + m2[3][3]*m3[3][3]; 
3078         
3079         cpy_m4_m4(m1, m2);
3080 }
3081
3082 /*  ***************** m1 = inverse(m2)  *****************  */
3083 void inv_m3_m3(float m1[][3], float m2[][3])
3084 {
3085         short a,b;
3086         float det;
3087         
3088         /* calc adjoint */
3089         Mat3Adj(m1, m2);
3090         
3091         /* then determinant old matrix! */
3092         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
3093             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
3094             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
3095         
3096         if(det==0.0f) det=1.0f;
3097         det= 1.0f/det;
3098         for(a=0;a<3;a++) {
3099                 for(b=0;b<3;b++) {
3100                         m1[a][b]*=det;
3101                 }
3102         }
3103 }
3104
3105 /*  ***************** m1 = inverse(m2)  *****************  */
3106 int inv_m4_m4(float inverse[][4], float mat[][4])
3107 {
3108         int i, j, k;
3109         double temp;
3110         float tempmat[4][4];
3111         float max;
3112         int maxj;
3113         
3114         /* Set inverse to identity */
3115         ident_m4(inverse);
3116         
3117         /* Copy original matrix so we don't mess it up */
3118         cpy_m4_m4(tempmat, mat);
3119         
3120         for(i = 0; i < 4; i++) {
3121                 /* Look for row with max pivot */
3122                 max = ABS(tempmat[i][i]);
3123                 maxj = i;
3124                 for(j = i + 1; j < 4; j++) {
3125                         if(ABS(tempmat[j][i]) > max) {
3126                                 max = ABS(tempmat[j][i]);
3127                                 maxj = j;
3128                         }
3129                 }
3130                 /* Swap rows if necessary */
3131                 if (maxj != i) {
3132                         for( k = 0; k < 4; k++) {
3133                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
3134                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
3135                         }
3136                 }
3137                 
3138                 temp = tempmat[i][i];
3139                 if (temp == 0)
3140                         return 0;  /* No non-zero pivot */
3141                 for(k = 0; k < 4; k++) {
3142                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
3143                         inverse[i][k] = (float)(inverse[i][k]/temp);
3144                 }
3145                 for(j = 0; j < 4; j++) {
3146                         if(j != i) {
3147                                 temp = tempmat[j][i];
3148                                 for(k = 0; k < 4; k++) {
3149                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
3150                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
3151                                 }
3152                         }
3153                 }
3154         }
3155         return 1;
3156 }
3157
3158 /*  ***************** v1 = v2 * mat  ***************** */
3159 void mul_v3_v3m4(float *v1, float *v2, float mat[][4])
3160 {
3161         float x, y;
3162         
3163         x= v2[0];       /* work with a copy, v1 can be same as v2 */
3164         y= v2[1];
3165         v1[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*v2[2] + mat[3][0];
3166         v1[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*v2[2] + mat[3][1];
3167         v1[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*v2[2] + mat[3][2];
3168         
3169 }
3170
3171 /* moved from effect.c
3172    test if the line starting at p1 ending at p2 intersects the triangle v0..v2
3173    return non zero if it does 
3174 */
3175 int LineIntersectsTriangle(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)
3176 {
3177
3178         float p[3], s[3], d[3], e1[3], e2[3], q[3];
3179         float a, f, u, v;
3180         
3181         VecSubf(e1, v1, v0);
3182         VecSubf(e2, v2, v0);
3183         VecSubf(d, p2, p1);
3184         
3185         Crossf(p, d, e2);
3186         a = Inpf(e1, p);
3187         if ((a > -0.000001) && (a < 0.000001)) return 0;
3188         f = 1.0f/a;
3189         
3190         VecSubf(s, p1, v0);
3191         
3192         Crossf(q, s, e1);
3193         *lambda = f * Inpf(e2, q);
3194         if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
3195         
3196         u = f * Inpf(s, p);
3197         if ((u < 0.0)||(u > 1.0)) return 0;
3198         
3199         v = f * Inpf(d, q);
3200         if ((v < 0.0)||((u + v) > 1.0)) return 0;
3201         
3202         return 1;
3203 }
3204
3205
3206 /*
3207 find closest point to p on line through l1,l2
3208 and return lambda, where (0 <= lambda <= 1) when cp is in the line segement l1,l2
3209 */
3210 float lambda_cp_line_ex(float p[3], float l1[3], float l2[3], float cp[3])
3211 {
3212         float h[3],u[3],lambda;
3213         VecSubf(u, l2, l1);
3214         VecSubf(h, p, l1);
3215         lambda =Inpf(u,h)/Inpf(u,u);
3216         cp[0] = l1[0] + u[0] * lambda;
3217         cp[1] = l1[1] + u[1] * lambda;
3218         cp[2] = l1[2] + u[2] * lambda;
3219         return lambda;
3220 }
3221 /* little sister we only need to know lambda */
3222 float lambda_cp_line(float p[3], float l1[3], float l2[3])
3223 {
3224         float h[3],u[3];
3225         VecSubf(u, l2, l1);
3226         VecSubf(h, p, l1);
3227         return(Inpf(u,h)/Inpf(u,u));
3228 }
3229
3230
3231
3232 int point_in_slice(float p[3], float v1[3], float l1[3], float l2[3])
3233 {
3234 /* 
3235 what is a slice ?
3236 some maths:
3237 a line including l1,l2 and a point not on the line 
3238 define a subset of R3 delimeted by planes parallel to the line and orthogonal 
3239 to the (point --> line) distance vector,one plane on the line one on the point, 
3240 the room inside usually is rather small compared to R3 though still infinte
3241 useful for restricting (speeding up) searches 
3242 e.g. all points of triangular prism are within the intersection of 3 'slices'
3243 onother trivial case : cube 
3244 but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
3245 */
3246         float h,rp[3],cp[3],q[3];
3247
3248         lambda_cp_line_ex(v1,l1,l2,cp);
3249         VecSubf(q,cp,v1);
3250
3251         VecSubf(rp,p,v1);
3252         h=Inpf(q,rp)/Inpf(q,q);
3253         if (h < 0.0f || h > 1.0f) return 0;
3254         return 1;
3255 }
3256
3257 /*adult sister defining the slice planes by the origin and the normal  
3258 NOTE |normal| may not be 1 but defining the thickness of the slice*/
3259 int point_in_slice_as(float p[3],float origin[3],float normal[3])
3260 {
3261         float h,rp[3];
3262         VecSubf(rp,p,origin);
3263         h=Inpf(normal,rp)/Inpf(normal,normal);
3264         if (h < 0.0f || h > 1.0f) return 0;
3265         return 1;
3266 }
3267
3268 /*mama (knowing the squared lenght of the normal)*/
3269 int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
3270 {
3271         float h,rp[3];
3272         VecSubf(rp,p,origin);
3273         h=Inpf(normal,rp)/lns;
3274         if (h < 0.0f || h > 1.0f) return 0;
3275         return 1;
3276 }
3277
3278
3279 int point_in_tri_prism(float p[3], float v1[3], float v2[3], float v3[3])
3280 {
3281         if(!point_in_slice(p,v1,v2,v3)) return 0;
3282         if(!point_in_slice(p,v2,v3,v1)) return 0;
3283         if(!point_in_slice(p,v3,v1,v2)) return 0;
3284         return 1;
3285 }
3286
3287 /* point closest to v1 on line v2-v3 in 3D */
3288 void PclosestVL3Dfl(float *closest, float *v1, float *v2, float *v3)
3289 {
3290         float lambda, cp[3], len;
3291
3292         lambda= lambda_cp_line_ex(v1, v2, v3, cp);
3293
3294         if(lambda <= 0.0f)
3295                 VecCopyf(closest, v2);
3296         else if(lambda >= 1.0f)
3297                 VecCopyf(closest, v3);
3298         else
3299                 VecCopyf(closest, cp);
3300 }
3301
3302 /* distance v1 to line-piece v2-v3 in 3D */
3303 float PdistVL3Dfl(float *v1, float *v2, float *v3) 
3304 {
3305         float closest[3];
3306
3307         PclosestVL3Dfl(closest, v1, v2, v3);
3308
3309         return VecLenf(closest, v1);
3310 }
3311
3312 /********************************************************/
3313
3314 /* make a 4x4 matrix out of 3 transform components */
3315 /* matrices are made in the order: scale * rot * loc */
3316 void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3])
3317 {
3318         float rmat[3][3], smat[3][3], tmat[3][3];
3319         
3320         /* initialise new matrix */
3321         Mat4One(mat);
3322         
3323         /* make rotation + scaling part */
3324         EulToMat3(eul, rmat);
3325         SizeToMat3(size, smat);
3326         Mat3MulMat3(tmat, rmat, smat);
3327         
3328         /* copy rot/scale part to output matrix*/
3329         Mat4CpyMat3(mat, tmat);
3330         
3331         /* copy location to matrix */
3332         mat[3][0] = loc[0];
3333         mat[3][1] = loc[1];
3334         mat[3][2] = loc[2];
3335 }
3336
3337 /* make a 4x4 matrix out of 3 transform components */
3338 /* matrices are made in the order: scale * rot * loc */
3339 void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3])
3340 {
3341         float rmat[3][3], smat[3][3], tmat[3][3];
3342         
3343         /* initialise new matrix */
3344         Mat4One(mat);
3345         
3346         /* make rotation + scaling part */
3347         QuatToMat3(quat, rmat);
3348         SizeToMat3(size, smat);
3349         Mat3MulMat3(tmat, rmat, smat);
3350         
3351         /* copy rot/scale part to output matrix*/
3352         Mat4CpyMat3(mat, tmat);
3353         
3354         /* copy location to matrix */
3355         mat[3][0] = loc[0];
3356         mat[3][1] = loc[1];
3357         mat[3][2] = loc[2];
3358 }
3359