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