Constraints on bones working in 'local' mode, now obey the Enforce
[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 void BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
2143 {
2144         float t00, t01, t10, t11, det, detinv, xn, yn, zn;
2145         short i, j;
2146
2147         /* find best projection of face XY, XZ or YZ: barycentric weights of
2148            the 2d projected coords are the same and faster to compute */
2149         xn= fabs(n[0]);
2150         yn= fabs(n[1]);
2151         zn= fabs(n[2]);
2152         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
2153         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
2154         else {i= 1; j= 2;} 
2155
2156         /* compute determinant */
2157         t00= v3[i]-v1[i]; t01= v3[j]-v1[j];
2158         t10= v3[i]-v2[i]; t11= v3[j]-v2[j];
2159
2160         det= t00*t11 - t10*t01;
2161
2162         if(det == 0.0f) {
2163                 /* zero area triangle */
2164                 w[0]= w[1]= w[2]= 1.0f/3.0f;
2165                 return;
2166         }
2167
2168         /* compute weights */
2169         detinv= 1.0/det;
2170
2171         w[0]= ((co[j]-v3[j])*t10 - (co[i]-v3[i])*t11)*detinv;
2172         w[1]= ((co[i]-v3[i])*t01 - (co[j]-v3[j])*t00)*detinv; 
2173         w[2]= 1.0f - w[0] - w[1];
2174 }
2175
2176 void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
2177 {
2178         w[0]= w[1]= w[2]= w[3]= 0.0f;
2179
2180         /* first check for exact match */
2181         if(VecEqual(co, v1))
2182                 w[0]= 1.0f;
2183         else if(VecEqual(co, v2))
2184                 w[1]= 1.0f;
2185         else if(VecEqual(co, v3))
2186                 w[2]= 1.0f;
2187         else if(v4 && VecEqual(co, v4))
2188                 w[3]= 1.0f;
2189         else {
2190                 /* otherwise compute barycentric interpolation weights */
2191                 float n1[3], n2[3], n[3];
2192
2193                 VecSubf(n1, v1, v3);
2194                 if (v4) {
2195                         VecSubf(n2, v2, v4);
2196                 }
2197                 else {
2198                         VecSubf(n2, v2, v3);
2199                 }
2200                 Crossf(n, n1, n2);
2201
2202                 /* OpenGL seems to split this way, so we do too */
2203                 if (v4) {
2204                         BarycentricWeights(v1, v2, v4, co, n, w);
2205
2206                         if(w[0] < 0.0f) {
2207                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2208                                    so we interpolate using the other triangle */
2209                                 w[0]= 0.0f;
2210                                 BarycentricWeights(v2, v3, v4, co, n, w+1);
2211                         }
2212                         else {
2213                                 SWAP(float, w[2], w[3]);
2214                         }
2215                 }
2216                 else
2217                         BarycentricWeights(v1, v2, v3, co, n, w);
2218         }
2219
2220
2221 /* ************ EULER *************** */
2222
2223 void EulToMat3( float *eul, float mat[][3])
2224 {
2225         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2226         
2227         ci = cos(eul[0]); 
2228         cj = cos(eul[1]); 
2229         ch = cos(eul[2]);
2230         si = sin(eul[0]); 
2231         sj = sin(eul[1]); 
2232         sh = sin(eul[2]);
2233         cc = ci*ch; 
2234         cs = ci*sh; 
2235         sc = si*ch; 
2236         ss = si*sh;
2237
2238         mat[0][0] = (float)(cj*ch); 
2239         mat[1][0] = (float)(sj*sc-cs); 
2240         mat[2][0] = (float)(sj*cc+ss);
2241         mat[0][1] = (float)(cj*sh); 
2242         mat[1][1] = (float)(sj*ss+cc); 
2243         mat[2][1] = (float)(sj*cs-sc);
2244         mat[0][2] = (float)-sj;  
2245         mat[1][2] = (float)(cj*si);    
2246         mat[2][2] = (float)(cj*ci);
2247
2248 }
2249
2250 void EulToMat4( float *eul,float mat[][4])
2251 {
2252         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2253         
2254         ci = cos(eul[0]); 
2255         cj = cos(eul[1]); 
2256         ch = cos(eul[2]);
2257         si = sin(eul[0]); 
2258         sj = sin(eul[1]); 
2259         sh = sin(eul[2]);
2260         cc = ci*ch; 
2261         cs = ci*sh; 
2262         sc = si*ch; 
2263         ss = si*sh;
2264
2265         mat[0][0] = (float)(cj*ch); 
2266         mat[1][0] = (float)(sj*sc-cs); 
2267         mat[2][0] = (float)(sj*cc+ss);
2268         mat[0][1] = (float)(cj*sh); 
2269         mat[1][1] = (float)(sj*ss+cc); 
2270         mat[2][1] = (float)(sj*cs-sc);
2271         mat[0][2] = (float)-sj;  
2272         mat[1][2] = (float)(cj*si);    
2273         mat[2][2] = (float)(cj*ci);
2274
2275
2276         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2277         mat[3][3]= 1.0f;
2278 }
2279
2280 /* returns two euler calculation methods, so we can pick the best */
2281 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
2282 {
2283         float cy, quat[4], mat[3][3];
2284         
2285         Mat3ToQuat(tmat, quat);
2286         QuatToMat3(quat, mat);
2287         Mat3CpyMat3(mat, tmat);
2288         Mat3Ortho(mat);
2289         
2290         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2291         
2292         if (cy > 16.0*FLT_EPSILON) {
2293                 
2294                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2295                 eul1[1] = (float)atan2(-mat[0][2], cy);
2296                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2297                 
2298                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2299                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2300                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2301                 
2302         } else {
2303                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
2304                 eul1[1] = (float)atan2(-mat[0][2], cy);
2305                 eul1[2] = 0.0f;
2306                 
2307                 VecCopyf(eul2, eul1);
2308         }
2309 }
2310
2311 void Mat3ToEul(float tmat[][3], float *eul)
2312 {
2313         float eul1[3], eul2[3];
2314         
2315         mat3_to_eul2(tmat, eul1, eul2);
2316                 
2317         /* return best, which is just the one with lowest values it in */
2318         if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2319                 VecCopyf(eul, eul2);
2320         }
2321         else {
2322                 VecCopyf(eul, eul1);
2323         }
2324 }
2325
2326 void Mat4ToEul(float tmat[][4], float *eul)
2327 {
2328         float tempMat[3][3];
2329
2330         Mat3CpyMat4 (tempMat, tmat);
2331         Mat3Ortho(tempMat);
2332         Mat3ToEul(tempMat, eul);
2333 }
2334
2335 void QuatToEul( float *quat, float *eul)
2336 {
2337         float mat[3][3];
2338         
2339         QuatToMat3(quat, mat);
2340         Mat3ToEul(mat, eul);
2341 }
2342
2343
2344 void EulToQuat( float *eul, float *quat)
2345 {
2346     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2347  
2348     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2349     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2350     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2351     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2352         
2353         quat[0] = cj*cc + sj*ss;
2354         quat[1] = cj*sc - sj*cs;
2355         quat[2] = cj*ss + sj*cc;
2356         quat[3] = cj*cs - sj*sc;
2357 }
2358
2359 void VecRotToMat3( float *vec, float phi, float mat[][3])
2360 {
2361         /* rotation of phi radials around vec */
2362         float vx, vx2, vy, vy2, vz, vz2, co, si;
2363         
2364         vx= vec[0];
2365         vy= vec[1];
2366         vz= vec[2];
2367         vx2= vx*vx;
2368         vy2= vy*vy;
2369         vz2= vz*vz;
2370         co= (float)cos(phi);
2371         si= (float)sin(phi);
2372         
2373         mat[0][0]= vx2+co*(1.0f-vx2);
2374         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2375         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2376         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2377         mat[1][1]= vy2+co*(1.0f-vy2);
2378         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2379         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2380         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2381         mat[2][2]= vz2+co*(1.0f-vz2);
2382         
2383 }
2384
2385 void VecRotToQuat( float *vec, float phi, float *quat)
2386 {
2387         /* rotation of phi radials around vec */
2388         float si;
2389
2390         quat[1]= vec[0];
2391         quat[2]= vec[1];
2392         quat[3]= vec[2];
2393                                                                                                            
2394         if( Normalise(quat+1) == 0.0) {
2395                 QuatOne(quat);
2396         }
2397         else {
2398                 quat[0]= (float)cos( phi/2.0 );
2399                 si= (float)sin( phi/2.0 );
2400                 quat[1] *= si;
2401                 quat[2] *= si;
2402                 quat[3] *= si;
2403         }
2404 }
2405
2406 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
2407    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
2408    this would return the angle at the elbow */
2409 float VecAngle3(float *v1, float *v2, float *v3)
2410 {
2411         float vec1[3], vec2[3];
2412
2413         VecSubf(vec1, v2, v1);
2414         VecSubf(vec2, v2, v3);
2415         Normalise(vec1);
2416         Normalise(vec2);
2417
2418         return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
2419 }
2420
2421 /* Return the shortest angle in degrees between the 2 vectors */
2422 float VecAngle2(float *v1, float *v2)
2423 {
2424         float vec1[3], vec2[3];
2425
2426         VecCopyf(vec1, v1);
2427         VecCopyf(vec2, v2);
2428         Normalise(vec1);
2429         Normalise(vec2);
2430
2431         return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
2432 }
2433
2434 float NormalizedVecAngle2(float *v1, float *v2)
2435 {
2436         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
2437         if (Inpf(v1, v2) < 0.0f) {
2438                 float vec[3];
2439                 
2440                 vec[0]= -v2[0];
2441                 vec[1]= -v2[1];
2442                 vec[2]= -v2[2];
2443
2444                 return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
2445         }
2446         else
2447                 return 2.0f*saasin(VecLenf(v2, v1)/2.0);
2448 }
2449
2450 void euler_rot(float *beul, float ang, char axis)
2451 {
2452         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2453         
2454         eul[0]= eul[1]= eul[2]= 0.0;
2455         if(axis=='x') eul[0]= ang;
2456         else if(axis=='y') eul[1]= ang;
2457         else eul[2]= ang;
2458         
2459         EulToMat3(eul, mat1);
2460         EulToMat3(beul, mat2);
2461         
2462         Mat3MulMat3(totmat, mat2, mat1);
2463         
2464         Mat3ToEul(totmat, beul);
2465         
2466 }
2467
2468 /* exported to transform.c */
2469 void compatible_eul(float *eul, float *oldrot)
2470 {
2471         float dx, dy, dz;
2472         
2473         /* correct differences of about 360 degrees first */
2474         
2475         dx= eul[0] - oldrot[0];
2476         dy= eul[1] - oldrot[1];
2477         dz= eul[2] - oldrot[2];
2478         
2479         while( fabs(dx) > 5.1) {
2480                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2481                 dx= eul[0] - oldrot[0];
2482         }
2483         while( fabs(dy) > 5.1) {
2484                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2485                 dy= eul[1] - oldrot[1];
2486         }
2487         while( fabs(dz) > 5.1 ) {
2488                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2489                 dz= eul[2] - oldrot[2];
2490         }
2491         
2492         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
2493         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
2494                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2495         }
2496         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
2497                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2498         }
2499         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
2500                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2501         }
2502         
2503         /* the method below was there from ancient days... but why! probably because the code sucks :)
2504                 */
2505 #if 0   
2506         /* calc again */
2507         dx= eul[0] - oldrot[0];
2508         dy= eul[1] - oldrot[1];
2509         dz= eul[2] - oldrot[2];
2510         
2511         /* special case, tested for x-z  */
2512         
2513         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
2514                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2515                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
2516                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2517                 
2518         }
2519         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
2520                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2521                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2522                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
2523         }
2524         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
2525                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
2526                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2527                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2528         }
2529 #endif  
2530 }
2531
2532 /* uses 2 methods to retrieve eulers, and picks the closest */
2533 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
2534 {
2535         float eul1[3], eul2[3];
2536         float d1, d2;
2537         
2538         mat3_to_eul2(mat, eul1, eul2);
2539         
2540         compatible_eul(eul1, oldrot);
2541         compatible_eul(eul2, oldrot);
2542         
2543         d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
2544         d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
2545         
2546         /* return best, which is just the one with lowest difference */
2547         if( d1 > d2) {
2548                 VecCopyf(eul, eul2);
2549         }
2550         else {
2551                 VecCopyf(eul, eul1);
2552         }
2553         
2554 }
2555
2556 /* ******************************************** */
2557
2558 void SizeToMat3( float *size, float mat[][3])
2559 {
2560         mat[0][0]= size[0];
2561         mat[0][1]= 0.0;
2562         mat[0][2]= 0.0;
2563         mat[1][1]= size[1];
2564         mat[1][0]= 0.0;
2565         mat[1][2]= 0.0;
2566         mat[2][2]= size[2];
2567         mat[2][1]= 0.0;
2568         mat[2][0]= 0.0;
2569 }
2570
2571 void Mat3ToSize( float mat[][3], float *size)
2572 {
2573         float vec[3];
2574
2575         VecCopyf(vec, mat[0]);
2576         size[0]= Normalise(vec);
2577         VecCopyf(vec, mat[1]);
2578         size[1]= Normalise(vec);
2579         VecCopyf(vec, mat[2]);
2580         size[2]= Normalise(vec);
2581
2582 }
2583
2584 void Mat4ToSize( float mat[][4], float *size)
2585 {
2586         float vec[3];
2587         
2588
2589         VecCopyf(vec, mat[0]);
2590         size[0]= Normalise(vec);
2591         VecCopyf(vec, mat[1]);
2592         size[1]= Normalise(vec);
2593         VecCopyf(vec, mat[2]);
2594         size[2]= Normalise(vec);
2595 }
2596
2597 /* ************* SPECIALS ******************* */
2598
2599 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
2600 {
2601         /* imaginary x-axis, y-axis triangle is being rotated */
2602         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
2603         
2604         /* move z-axis to face-normal */
2605         CalcNormFloat(v1, v2, v3, vec);
2606
2607         n[0]= vec[1];
2608         n[1]= -vec[0];
2609         n[2]= 0.0;
2610         Normalise(n);
2611         
2612         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
2613         
2614         angle= -0.5f*saacos(vec[2]);
2615         co= (float)cos(angle);
2616         si= (float)sin(angle);
2617         q1[0]= co;
2618         q1[1]= n[0]*si;
2619         q1[2]= n[1]*si;
2620         q1[3]= 0.0f;
2621         
2622         /* rotate back line v1-v2 */
2623         QuatToMat3(q1, mat);
2624         Mat3Inv(imat, mat);
2625         VecSubf(vec, v2, v1);
2626         Mat3MulVecfl(imat, vec);
2627
2628         /* what angle has this line with x-axis? */
2629         vec[2]= 0.0;
2630         Normalise(vec);
2631
2632         angle= (float)(0.5*atan2(vec[1], vec[0]));
2633         co= (float)cos(angle);
2634         si= (float)sin(angle);
2635         q2[0]= co;
2636         q2[1]= 0.0f;
2637         q2[2]= 0.0f;
2638         q2[3]= si;
2639         
2640         QuatMul(quat, q1, q2);
2641 }
2642
2643 void MinMaxRGB(short c[])
2644 {
2645         if(c[0]>255) c[0]=255;
2646         else if(c[0]<0) c[0]=0;
2647         if(c[1]>255) c[1]=255;
2648         else if(c[1]<0) c[1]=0;
2649         if(c[2]>255) c[2]=255;
2650         else if(c[2]<0) c[2]=0;
2651 }
2652
2653 float Vec2Lenf(float *v1, float *v2)
2654 {
2655         float x, y;
2656
2657         x = v1[0]-v2[0];
2658         y = v1[1]-v2[1];
2659         return (float)sqrt(x*x+y*y);
2660 }
2661
2662 void Vec2Mulf(float *v1, float f)
2663 {
2664         v1[0]*= f;
2665         v1[1]*= f;
2666 }
2667
2668 void Vec2Addf(float *v, float *v1, float *v2)
2669 {
2670         v[0]= v1[0]+ v2[0];
2671         v[1]= v1[1]+ v2[1];
2672 }
2673
2674 void Vec2Subf(float *v, float *v1, float *v2)
2675 {
2676         v[0]= v1[0]- v2[0];
2677         v[1]= v1[1]- v2[1];
2678 }
2679
2680 void Vec2Copyf(float *v1, float *v2)
2681 {
2682         v1[0]= v2[0];
2683         v1[1]= v2[1];
2684 }
2685
2686 float Inp2f(float *v1, float *v2)
2687 {
2688         return v1[0]*v2[0]+v1[1]*v2[1];
2689 }
2690
2691 float Normalise2(float *n)
2692 {
2693         float d;
2694         
2695         d= n[0]*n[0]+n[1]*n[1];
2696
2697         if(d>1.0e-35F) {
2698                 d= (float)sqrt(d);
2699
2700                 n[0]/=d; 
2701                 n[1]/=d; 
2702         } else {
2703                 n[0]=n[1]= 0.0;
2704                 d= 0.0;
2705         }
2706         return d;
2707 }
2708
2709 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
2710 {
2711         int i;
2712         float f, p, q, t;
2713
2714         h *= 360.0f;
2715         
2716         if(s==0.0) {
2717                 *r = v;
2718                 *g = v;
2719                 *b = v;
2720         }
2721         else {
2722                 if(h==360) h = 0;
2723                 
2724                 h /= 60;
2725                 i = (int)floor(h);
2726                 f = h - i;
2727                 p = v*(1.0f-s);
2728                 q = v*(1.0f-(s*f));
2729                 t = v*(1.0f-(s*(1.0f-f)));
2730                 
2731                 switch (i) {
2732                 case 0 :
2733                         *r = v;
2734                         *g = t;
2735                         *b = p;
2736                         break;
2737                 case 1 :
2738                         *r = q;
2739                         *g = v;
2740                         *b = p;
2741                         break;
2742                 case 2 :
2743                         *r = p;
2744                         *g = v;
2745                         *b = t;
2746                         break;
2747                 case 3 :
2748                         *r = p;
2749                         *g = q;
2750                         *b = v;
2751                         break;
2752                 case 4 :
2753                         *r = t;
2754                         *g = p;
2755                         *b = v;
2756                         break;
2757                 case 5 :
2758                         *r = v;
2759                         *g = p;
2760                         *b = q;
2761                         break;
2762                 }
2763         }
2764 }
2765
2766 void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
2767 {
2768         float y, u, v;
2769         y= 0.299*r + 0.587*g + 0.114*b;
2770         u=-0.147*r - 0.289*g + 0.436*b;
2771         v= 0.615*r - 0.515*g - 0.100*b;
2772         
2773         *ly=y;
2774         *lu=u;
2775         *lv=v;
2776 }
2777
2778 void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
2779 {
2780         float r, g, b;
2781         r=y+1.140*v;
2782         g=y-0.394*u - 0.581*v;
2783         b=y+2.032*u;
2784         
2785         *lr=r;
2786         *lg=g;
2787         *lb=b;
2788 }
2789
2790 void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
2791 {
2792         float sr,sg, sb;
2793         float y, cr, cb;
2794         
2795         sr=255.0*r;
2796         sg=255.0*g;
2797         sb=255.0*b;
2798         
2799         
2800         y=(0.257*sr)+(0.504*sg)+(0.098*sb)+16.0;
2801         cb=(-0.148*sr)-(0.291*sg)+(0.439*sb)+128.0;
2802         cr=(0.439*sr)-(0.368*sg)-(0.071*sb)+128.0;
2803         
2804         *ly=y;
2805         *lcb=cb;
2806         *lcr=cr;
2807 }
2808
2809 void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
2810 {
2811         float r,g,b;
2812         
2813         r=1.164*(y-16)+1.596*(cr-128);
2814         g=1.164*(y-16)-0.813*(cr-128)-0.392*(cb-128);
2815         b=1.164*(y-16)+2.017*(cb-128);
2816         
2817         *lr=r/255.0;
2818         *lg=g/255.0;
2819         *lb=b/255.0;
2820 }
2821
2822 void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
2823 {
2824         unsigned int ri, gi, bi;
2825         
2826         if (hexcol[0] == '#') hexcol++;
2827         
2828         if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
2829                 *r = ri / 255.0;
2830                 *g = gi / 255.0;                
2831                 *b = bi / 255.0;
2832         }
2833 }
2834
2835 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
2836 {
2837         float h, s, v;
2838         float cmax, cmin, cdelta;
2839         float rc, gc, bc;
2840
2841         cmax = r;
2842         cmin = r;
2843         cmax = (g>cmax ? g:cmax);
2844         cmin = (g<cmin ? g:cmin);
2845         cmax = (b>cmax ? b:cmax);
2846         cmin = (b<cmin ? b:cmin);
2847
2848         v = cmax;               /* value */
2849         if (cmax!=0.0)
2850                 s = (cmax - cmin)/cmax;
2851         else {
2852                 s = 0.0;
2853                 h = 0.0;
2854         }
2855         if (s == 0.0)
2856                 h = -1.0;
2857         else {
2858                 cdelta = cmax-cmin;
2859                 rc = (cmax-r)/cdelta;
2860                 gc = (cmax-g)/cdelta;
2861                 bc = (cmax-b)/cdelta;
2862                 if (r==cmax)
2863                         h = bc-gc;
2864                 else
2865                         if (g==cmax)
2866                                 h = 2.0f+rc-bc;
2867                         else
2868                                 h = 4.0f+gc-rc;
2869                 h = h*60.0f;
2870                 if (h<0.0f)
2871                         h += 360.0f;
2872         }
2873         
2874         *ls = s;
2875         *lh = h/360.0f;
2876         if( *lh < 0.0) *lh= 0.0;
2877         *lv = v;
2878 }
2879
2880
2881 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
2882    for that reason it is sensitive for endianness... with this function it works correctly
2883 */
2884
2885 unsigned int hsv_to_cpack(float h, float s, float v)
2886 {
2887         short r, g, b;
2888         float rf, gf, bf;
2889         unsigned int col;
2890         
2891         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
2892         
2893         r= (short)(rf*255.0f);
2894         g= (short)(gf*255.0f);
2895         b= (short)(bf*255.0f);
2896         
2897         col= ( r + (g*256) + (b*256*256) );
2898         return col;
2899 }
2900
2901
2902 unsigned int rgb_to_cpack(float r, float g, float b)
2903 {
2904         int ir, ig, ib;
2905         
2906         ir= (int)floor(255.0*r);
2907         if(ir<0) ir= 0; else if(ir>255) ir= 255;
2908         ig= (int)floor(255.0*g);
2909         if(ig<0) ig= 0; else if(ig>255) ig= 255;
2910         ib= (int)floor(255.0*b);
2911         if(ib<0) ib= 0; else if(ib>255) ib= 255;
2912         
2913         return (ir+ (ig*256) + (ib*256*256));
2914 }
2915
2916 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
2917 {
2918         
2919         *r= (float)((col)&0xFF);
2920         *r /= 255.0f;
2921
2922         *g= (float)(((col)>>8)&0xFF);
2923         *g /= 255.0f;
2924
2925         *b= (float)(((col)>>16)&0xFF);
2926         *b /= 255.0f;
2927 }
2928
2929
2930 /* *************** PROJECTIONS ******************* */
2931
2932 void tubemap(float x, float y, float z, float *u, float *v)
2933 {
2934         float len;
2935         
2936         *v = (z + 1.0) / 2.0;
2937         
2938         len= sqrt(x*x+y*y);
2939         if(len>0) {
2940                 *u = (1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0;
2941         }
2942 }
2943
2944 /* ------------------------------------------------------------------------- */
2945
2946 void spheremap(float x, float y, float z, float *u, float *v)
2947 {
2948         float len;
2949         
2950         len= sqrt(x*x+y*y+z*z);
2951         if(len>0.0) {
2952                 
2953                 if(x==0.0 && y==0.0) *u= 0.0;   /* othwise domain error */
2954                 else *u = (1.0 - atan2(x,y)/M_PI )/2.0;
2955                 
2956                 z/=len;
2957                 *v = 1.0- saacos(z)/M_PI;
2958         }
2959 }
2960
2961 /* ------------------------------------------------------------------------- */
2962
2963 /* *****************  m1 = m2 *****************  */
2964 void cpy_m3_m3(float m1[][3], float m2[][3]) 
2965 {       
2966         memcpy(m1[0], m2[0], 9*sizeof(float));
2967 }
2968
2969 /* *****************  m1 = m2 *****************  */
2970 void cpy_m4_m4(float m1[][4], float m2[][4]) 
2971 {       
2972         memcpy(m1[0], m2[0], 16*sizeof(float));
2973 }
2974
2975 /* ***************** identity matrix *****************  */
2976 void ident_m4(float m[][4])
2977 {
2978         
2979         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
2980         m[0][1]= m[0][2]= m[0][3]= 0.0;
2981         m[1][0]= m[1][2]= m[1][3]= 0.0;
2982         m[2][0]= m[2][1]= m[2][3]= 0.0;
2983         m[3][0]= m[3][1]= m[3][2]= 0.0;
2984 }
2985
2986
2987 /* *****************  m1 = m2 (pre) * m3 (post) ***************** */
2988 void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
2989 {
2990         float m[3][3];
2991         
2992         m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2]; 
2993         m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2]; 
2994         m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2]; 
2995
2996         m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2]; 
2997         m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2]; 
2998         m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2]; 
2999
3000         m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2]; 
3001         m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2]; 
3002         m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2]; 
3003
3004         cpy_m3_m3(m1, m2);
3005 }
3006
3007 /*  ***************** m1 = m2 (pre) * m3 (post) ***************** */
3008 void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
3009 {
3010         float m[4][4];
3011         
3012         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]; 
3013         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]; 
3014         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]; 
3015         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]; 
3016
3017         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]; 
3018         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]; 
3019         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]; 
3020         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]; 
3021
3022         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]; 
3023         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]; 
3024         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]; 
3025         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]; 
3026         
3027         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]; 
3028         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]; 
3029         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]; 
3030         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]; 
3031         
3032         cpy_m4_m4(m1, m2);
3033 }
3034
3035 /*  ***************** m1 = inverse(m2)  *****************  */
3036 void inv_m3_m3(float m1[][3], float m2[][3])
3037 {
3038         short a,b;
3039         float det;
3040         
3041         /* calc adjoint */
3042         Mat3Adj(m1, m2);
3043         
3044         /* then determinant old matrix! */
3045         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
3046             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
3047             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
3048         
3049         if(det==0.0f) det=1.0f;
3050         det= 1.0f/det;
3051         for(a=0;a<3;a++) {
3052                 for(b=0;b<3;b++) {
3053                         m1[a][b]*=det;
3054                 }
3055         }
3056 }
3057
3058 /*  ***************** m1 = inverse(m2)  *****************  */
3059 int inv_m4_m4(float inverse[][4], float mat[][4])
3060 {
3061         int i, j, k;
3062         double temp;
3063         float tempmat[4][4];
3064         float max;
3065         int maxj;
3066         
3067         /* Set inverse to identity */
3068         ident_m4(inverse);
3069         
3070         /* Copy original matrix so we don't mess it up */
3071         cpy_m4_m4(tempmat, mat);
3072         
3073         for(i = 0; i < 4; i++) {
3074                 /* Look for row with max pivot */
3075                 max = ABS(tempmat[i][i]);
3076                 maxj = i;
3077                 for(j = i + 1; j < 4; j++) {
3078                         if(ABS(tempmat[j][i]) > max) {
3079                                 max = ABS(tempmat[j][i]);
3080                                 maxj = j;
3081                         }
3082                 }
3083                 /* Swap rows if necessary */
3084                 if (maxj != i) {
3085                         for( k = 0; k < 4; k++) {
3086                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
3087                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
3088                         }
3089                 }
3090                 
3091                 temp = tempmat[i][i];
3092                 if (temp == 0)
3093                         return 0;  /* No non-zero pivot */
3094                 for(k = 0; k < 4; k++) {
3095                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
3096                         inverse[i][k] = (float)(inverse[i][k]/temp);
3097                 }
3098                 for(j = 0; j < 4; j++) {
3099                         if(j != i) {
3100                                 temp = tempmat[j][i];
3101                                 for(k = 0; k < 4; k++) {
3102                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
3103                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
3104                                 }
3105                         }
3106                 }
3107         }
3108         return 1;
3109 }
3110
3111 /*  ***************** v1 = v2 * mat  ***************** */
3112 void mul_v3_v3m4(float *v1, float *v2, float mat[][4])
3113 {
3114         float x, y;
3115         
3116         x= v2[0];       /* work with a copy, v1 can be same as v2 */
3117         y= v2[1];
3118         v1[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*v2[2] + mat[3][0];
3119         v1[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*v2[2] + mat[3][1];
3120         v1[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*v2[2] + mat[3][2];
3121         
3122 }
3123
3124 /* moved from effect.c
3125    test if the line starting at p1 ending at p2 intersects the triangle v0..v2
3126    return non zero if it does 
3127 */
3128 int LineIntersectsTriangle(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)
3129 {
3130
3131         float p[3], s[3], d[3], e1[3], e2[3], q[3];
3132         float a, f, u, v;
3133         
3134         VecSubf(e1, v1, v0);
3135         VecSubf(e2, v2, v0);
3136         VecSubf(d, p2, p1);
3137         
3138         Crossf(p, d, e2);
3139         a = Inpf(e1, p);
3140         if ((a > -0.000001) && (a < 0.000001)) return 0;
3141         f = 1.0f/a;
3142         
3143         VecSubf(s, p1, v0);
3144         
3145         Crossf(q, s, e1);
3146         *lambda = f * Inpf(e2, q);
3147         if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
3148         
3149         u = f * Inpf(s, p);
3150         if ((u < 0.0)||(u > 1.0)) return 0;
3151         
3152         v = f * Inpf(d, q);
3153         if ((v < 0.0)||((u + v) > 1.0)) return 0;
3154         
3155         return 1;
3156 }
3157
3158
3159 /*
3160 find closest point to p on line through l1,l2
3161 and return lambda, where (0 <= lambda <= 1) when cp is in the line segement l1,l2
3162 */
3163 float lambda_cp_line_ex(float p[3], float l1[3], float l2[3], float cp[3])
3164 {
3165         float h[3],u[3],lambda;
3166         VecSubf(u, l2, l1);
3167         VecSubf(h, p, l1);
3168         lambda =Inpf(u,h)/Inpf(u,u);
3169         cp[0] = l1[0] + u[0] * lambda;
3170         cp[1] = l1[1] + u[1] * lambda;
3171         cp[2] = l1[2] + u[2] * lambda;
3172         return lambda;
3173 }
3174 /* little sister we only need to know lambda */
3175 float lambda_cp_line(float p[3], float l1[3], float l2[3])
3176 {
3177         float h[3],u[3];
3178         VecSubf(u, l2, l1);
3179         VecSubf(h, p, l1);
3180         return(Inpf(u,h)/Inpf(u,u));
3181 }
3182
3183
3184
3185 int point_in_slice(float p[3], float v1[3], float l1[3], float l2[3])
3186 {
3187 /* 
3188 what is a slice ?
3189 some maths:
3190 a line including l1,l2 and a point not on the line 
3191 define a subset of R3 delimeted by planes parallel to the line and orthogonal 
3192 to the (point --> line) distance vector,one plane on the line one on the point, 
3193 the room inside usually is rather small compared to R3 though still infinte
3194 useful for restricting (speeding up) searches 
3195 e.g. all points of triangular prism are within the intersection of 3 'slices'
3196 onother trivial case : cube 
3197 but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
3198 */
3199         float h,rp[3],cp[3],q[3];
3200
3201         lambda_cp_line_ex(v1,l1,l2,cp);
3202         VecSubf(q,cp,v1);
3203
3204         VecSubf(rp,p,v1);
3205         h=Inpf(q,rp)/Inpf(q,q);
3206         if (h < 0.0f || h > 1.0f) return 0;
3207         return 1;
3208 }
3209
3210 /*adult sister defining the slice planes by the origin and the normal  
3211 NOTE |normal| may not be 1 but defining the thickness of the slice*/
3212 int point_in_slice_as(float p[3],float origin[3],float normal[3])
3213 {
3214         float h,rp[3];
3215         VecSubf(rp,p,origin);
3216         h=Inpf(normal,rp)/Inpf(normal,normal);
3217         if (h < 0.0f || h > 1.0f) return 0;
3218         return 1;
3219 }
3220
3221 /*mama (knowing the squared lenght of the normal)*/
3222 int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
3223 {
3224         float h,rp[3];
3225         VecSubf(rp,p,origin);
3226         h=Inpf(normal,rp)/lns;
3227         if (h < 0.0f || h > 1.0f) return 0;
3228         return 1;
3229 }
3230
3231
3232 int point_in_tri_prism(float p[3], float v1[3], float v2[3], float v3[3])
3233 {
3234         if(!point_in_slice(p,v1,v2,v3)) return 0;
3235         if(!point_in_slice(p,v2,v3,v1)) return 0;
3236         if(!point_in_slice(p,v3,v1,v2)) return 0;
3237         return 1;
3238 }
3239
3240 /********************************************************/
3241
3242 /* make a 4x4 matrix out of 3 transform components */
3243 void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3])
3244 {
3245         float tmat[3][3];
3246         
3247         /* make base matrix */
3248         EulToMat3(eul, tmat);
3249
3250         /* make new matrix */
3251         Mat4One(mat);
3252         
3253         mat[0][0] = tmat[0][0] * size[0];
3254         mat[0][1] = tmat[0][1] * size[1];
3255         mat[0][2] = tmat[0][2] * size[2];
3256         
3257         mat[1][0] = tmat[1][0] * size[0];
3258         mat[1][1] = tmat[1][1] * size[1];
3259         mat[1][2] = tmat[1][2] * size[2];
3260         
3261         mat[2][0] = tmat[2][0] * size[0];
3262         mat[2][1] = tmat[2][1] * size[1];
3263         mat[2][2] = tmat[2][2] * size[2];
3264         
3265         mat[3][0] = loc[0];
3266         mat[3][1] = loc[1];
3267         mat[3][2] = loc[2];
3268 }