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