Rotation constraint update.
[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], hoek, 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         hoek= 0.5f*saacos(co);
1203         
1204         co= (float)cos(hoek);
1205         si= (float)sin(hoek);
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         hoek= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1218         
1219         co= (float)cos(hoek);
1220         si= (float)sin(hoek);
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], hoek, 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         hoek= 0.5f*saacos(co);
1323         si= (float)sin(hoek);
1324         q1[0]= (float)cos(hoek);
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) hoek= (float)(0.5*atan2(fp[2], fp[1]));
1335                         else hoek= (float)(-0.5*atan2(fp[1], fp[2]));
1336                 }
1337                 else if(axis==1) {
1338                         if(upflag==0) hoek= (float)(-0.5*atan2(fp[2], fp[0]));
1339                         else hoek= (float)(0.5*atan2(fp[0], fp[2]));
1340                 }
1341                 else {
1342                         if(upflag==0) hoek= (float)(0.5*atan2(-fp[1], -fp[0]));
1343                         else hoek= (float)(-0.5*atan2(-fp[0], -fp[1]));
1344                 }
1345                                 
1346                 co= (float)cos(hoek);
1347                 si= (float)(sin(hoek)/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 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
1838 {
1839         float n1[3],n2[3];
1840
1841         n1[0]= (float)(v1[0]-v2[0]);
1842         n2[0]= (float)(v2[0]-v3[0]);
1843         n1[1]= (float)(v1[1]-v2[1]);
1844         n2[1]= (float)(v2[1]-v3[1]);
1845         n1[2]= (float)(v1[2]-v2[2]);
1846         n2[2]= (float)(v2[2]-v3[2]);
1847         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1848         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1849         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1850         Normalise(n);
1851 }
1852
1853 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
1854 {
1855         float n1[3],n2[3];
1856
1857         n1[0]= (float)(v1[0]-v2[0]);
1858         n2[0]= (float)(v2[0]-v3[0]);
1859         n1[1]= (float)(v1[1]-v2[1]);
1860         n2[1]= (float)(v2[1]-v3[1]);
1861         n1[2]= (float)(v1[2]-v2[2]);
1862         n2[2]= (float)(v2[2]-v3[2]);
1863         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1864         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1865         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1866         Normalise(n);
1867 }
1868
1869 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
1870 {
1871         float n1[3],n2[3];
1872
1873         n1[0]= v1[0]-v2[0];
1874         n2[0]= v2[0]-v3[0];
1875         n1[1]= v1[1]-v2[1];
1876         n2[1]= v2[1]-v3[1];
1877         n1[2]= v1[2]-v2[2];
1878         n2[2]= v2[2]-v3[2];
1879         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1880         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1881         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1882         return Normalise(n);
1883 }
1884
1885 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
1886 {
1887         /* real cross! */
1888         float n1[3],n2[3];
1889
1890         n1[0]= v1[0]-v3[0];
1891         n1[1]= v1[1]-v3[1];
1892         n1[2]= v1[2]-v3[2];
1893
1894         n2[0]= v2[0]-v4[0];
1895         n2[1]= v2[1]-v4[1];
1896         n2[2]= v2[2]-v4[2];
1897
1898         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1899         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1900         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1901
1902         return Normalise(n);
1903 }
1904
1905
1906 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
1907 {
1908
1909         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
1910         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
1911         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
1912 }
1913
1914 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
1915 {
1916
1917         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
1918         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
1919         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
1920 }
1921
1922 float Sqrt3f(float f)
1923 {
1924         if(f==0.0) return 0;
1925         if(f<0) return (float)(-exp(log(-f)/3));
1926         else return (float)(exp(log(f)/3));
1927 }
1928
1929 double Sqrt3d(double d)
1930 {
1931         if(d==0.0) return 0;
1932         if(d<0) return -exp(log(-d)/3);
1933         else return exp(log(d)/3);
1934 }
1935
1936 /* distance v1 to line v2-v3 */
1937 /* using Hesse formula, NO LINE PIECE! */
1938 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
1939         float a[2],deler;
1940
1941         a[0]= v2[1]-v3[1];
1942         a[1]= v3[0]-v2[0];
1943         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
1944         if(deler== 0.0f) return 0;
1945
1946         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
1947
1948 }
1949
1950 /* distance v1 to line-piece v2-v3 */
1951 float PdistVL2Dfl( float *v1, float *v2, float *v3) 
1952 {
1953         float labda, rc[2], pt[2], len;
1954         
1955         rc[0]= v3[0]-v2[0];
1956         rc[1]= v3[1]-v2[1];
1957         len= rc[0]*rc[0]+ rc[1]*rc[1];
1958         if(len==0.0) {
1959                 rc[0]= v1[0]-v2[0];
1960                 rc[1]= v1[1]-v2[1];
1961                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
1962         }
1963         
1964         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
1965         if(labda<=0.0) {
1966                 pt[0]= v2[0];
1967                 pt[1]= v2[1];
1968         }
1969         else if(labda>=1.0) {
1970                 pt[0]= v3[0];
1971                 pt[1]= v3[1];
1972         }
1973         else {
1974                 pt[0]= labda*rc[0]+v2[0];
1975                 pt[1]= labda*rc[1]+v2[1];
1976         }
1977
1978         rc[0]= pt[0]-v1[0];
1979         rc[1]= pt[1]-v1[1];
1980         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
1981 }
1982
1983 float AreaF2Dfl( float *v1, float *v2, float *v3)
1984 {
1985         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
1986 }
1987
1988
1989 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
1990 {
1991         float len, vec1[3], vec2[3], n[3];
1992
1993         VecSubf(vec1, v2, v1);
1994         VecSubf(vec2, v4, v1);
1995         Crossf(n, vec1, vec2);
1996         len= Normalise(n);
1997
1998         VecSubf(vec1, v4, v3);
1999         VecSubf(vec2, v2, v3);
2000         Crossf(n, vec1, vec2);
2001         len+= Normalise(n);
2002
2003         return (len/2.0f);
2004 }
2005
2006 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
2007 {
2008         float len, vec1[3], vec2[3], n[3];
2009
2010         VecSubf(vec1, v3, v2);
2011         VecSubf(vec2, v1, v2);
2012         Crossf(n, vec1, vec2);
2013         len= Normalise(n);
2014
2015         return (len/2.0f);
2016 }
2017
2018 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
2019 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
2020
2021
2022 float AreaPoly3Dfl(int nr, float *verts, float *normal)
2023 {
2024         float x, y, z, area, max;
2025         float *cur, *prev;
2026         int a, px=0, py=1;
2027
2028         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
2029         x= (float)fabs(normal[0]);
2030         y= (float)fabs(normal[1]);
2031         z= (float)fabs(normal[2]);
2032         max = MAX3(x, y, z);
2033         if(max==y) py=2;
2034         else if(max==x) {
2035                 px=1; 
2036                 py= 2;
2037         }
2038
2039         /* The Trapezium Area Rule */
2040         prev= verts+3*(nr-1);
2041         cur= verts;
2042         area= 0;
2043         for(a=0; a<nr; a++) {
2044                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
2045                 prev= cur;
2046                 cur+=3;
2047         }
2048
2049         return (float)fabs(0.5*area/max);
2050 }
2051
2052 /* intersect Line-Line, shorts */
2053 short IsectLL2Ds(short *v1, short *v2, short *v3, short *v4)
2054 {
2055         /* return:
2056         -1: colliniar
2057          0: no intersection of segments
2058          1: exact intersection of segments
2059          2: cross-intersection of segments
2060         */
2061         float div, labda, mu;
2062         
2063         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2064         if(div==0.0) return -1;
2065         
2066         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2067         
2068         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2069         
2070         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2071                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2072                 return 2;
2073         }
2074         return 0;
2075 }
2076
2077 /* intersect Line-Line, floats */
2078 short IsectLL2Df(float *v1, float *v2, float *v3, float *v4)
2079 {
2080         /* return:
2081         -1: colliniar
2082 0: no intersection of segments
2083 1: exact intersection of segments
2084 2: cross-intersection of segments
2085         */
2086         float div, labda, mu;
2087         
2088         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2089         if(div==0.0) return -1;
2090         
2091         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2092         
2093         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2094         
2095         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2096                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2097                 return 2;
2098         }
2099         return 0;
2100 }
2101
2102 void MinMax3(float *min, float *max, float *vec)
2103 {
2104         if(min[0]>vec[0]) min[0]= vec[0];
2105         if(min[1]>vec[1]) min[1]= vec[1];
2106         if(min[2]>vec[2]) min[2]= vec[2];
2107
2108         if(max[0]<vec[0]) max[0]= vec[0];
2109         if(max[1]<vec[1]) max[1]= vec[1];
2110         if(max[2]<vec[2]) max[2]= vec[2];
2111 }
2112
2113 /* ************ EULER *************** */
2114
2115 void EulToMat3( float *eul, float mat[][3])
2116 {
2117         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2118         
2119         ci = cos(eul[0]); 
2120         cj = cos(eul[1]); 
2121         ch = cos(eul[2]);
2122         si = sin(eul[0]); 
2123         sj = sin(eul[1]); 
2124         sh = sin(eul[2]);
2125         cc = ci*ch; 
2126         cs = ci*sh; 
2127         sc = si*ch; 
2128         ss = si*sh;
2129
2130         mat[0][0] = (float)(cj*ch); 
2131         mat[1][0] = (float)(sj*sc-cs); 
2132         mat[2][0] = (float)(sj*cc+ss);
2133         mat[0][1] = (float)(cj*sh); 
2134         mat[1][1] = (float)(sj*ss+cc); 
2135         mat[2][1] = (float)(sj*cs-sc);
2136         mat[0][2] = (float)-sj;  
2137         mat[1][2] = (float)(cj*si);    
2138         mat[2][2] = (float)(cj*ci);
2139
2140 }
2141
2142 void EulToMat4( float *eul,float mat[][4])
2143 {
2144         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2145         
2146         ci = cos(eul[0]); 
2147         cj = cos(eul[1]); 
2148         ch = cos(eul[2]);
2149         si = sin(eul[0]); 
2150         sj = sin(eul[1]); 
2151         sh = sin(eul[2]);
2152         cc = ci*ch; 
2153         cs = ci*sh; 
2154         sc = si*ch; 
2155         ss = si*sh;
2156
2157         mat[0][0] = (float)(cj*ch); 
2158         mat[1][0] = (float)(sj*sc-cs); 
2159         mat[2][0] = (float)(sj*cc+ss);
2160         mat[0][1] = (float)(cj*sh); 
2161         mat[1][1] = (float)(sj*ss+cc); 
2162         mat[2][1] = (float)(sj*cs-sc);
2163         mat[0][2] = (float)-sj;  
2164         mat[1][2] = (float)(cj*si);    
2165         mat[2][2] = (float)(cj*ci);
2166
2167
2168         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2169         mat[3][3]= 1.0f;
2170 }
2171
2172
2173 void Mat3ToEul(float tmat[][3], float *eul)
2174 {
2175         float cy, quat[4], mat[3][3];
2176         
2177         Mat3ToQuat(tmat, quat);
2178         QuatToMat3(quat, mat);
2179         Mat3CpyMat3(mat, tmat);
2180         Mat3Ortho(mat);
2181         
2182         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2183
2184         if (cy > 16.0*FLT_EPSILON) {
2185                 float eul1[3], eul2[3];
2186                 
2187                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2188                 eul1[1] = (float)atan2(-mat[0][2], cy);
2189                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2190                 
2191                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2192                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2193                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2194                 
2195                 /* return best, which is just the one with lowest values it in */
2196                 if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2197                         VecCopyf(eul, eul2);
2198                 }
2199                 else {
2200                         VecCopyf(eul, eul1);
2201                 }
2202                 
2203         } else {
2204                 eul[0] = (float)atan2(-mat[2][1], mat[1][1]);
2205                 eul[1] = (float)atan2(-mat[0][2], cy);
2206                 eul[2] = 0.0f;
2207         }
2208 }
2209
2210 void Mat3ToEuln( float tmat[][3], float *eul)
2211 {
2212         float sin1, cos1, sin2, cos2, sin3, cos3;
2213         
2214         sin1 = -tmat[2][0];
2215         cos1 = (float)sqrt(1 - sin1*sin1);
2216
2217         if ( fabs(cos1) > FLT_EPSILON ) {
2218                 sin2 = tmat[2][1] / cos1;
2219                 cos2 = tmat[2][2] / cos1;
2220                 sin3 = tmat[1][0] / cos1;
2221                 cos3 = tmat[0][0] / cos1;
2222     } 
2223         else {
2224                 sin2 = -tmat[1][2];
2225                 cos2 = tmat[1][1];
2226                 sin3 = 0.0;
2227                 cos3 = 1.0;
2228     }
2229         
2230         eul[0] = (float)atan2(sin3, cos3);
2231         eul[1] = (float)atan2(sin1, cos1);
2232         eul[2] = (float)atan2(sin2, cos2);
2233
2234
2235
2236
2237 void QuatToEul( float *quat, float *eul)
2238 {
2239         float mat[3][3];
2240         
2241         QuatToMat3(quat, mat);
2242         Mat3ToEul(mat, eul);
2243 }
2244
2245
2246 void EulToQuat( float *eul, float *quat)
2247 {
2248     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2249  
2250     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2251     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2252     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2253     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2254         
2255         quat[0] = cj*cc + sj*ss;
2256         quat[1] = cj*sc - sj*cs;
2257         quat[2] = cj*ss + sj*cc;
2258         quat[3] = cj*cs - sj*sc;
2259 }
2260
2261 void VecRotToMat3( float *vec, float phi, float mat[][3])
2262 {
2263         /* rotation of phi radials around vec */
2264         float vx, vx2, vy, vy2, vz, vz2, co, si;
2265         
2266         vx= vec[0];
2267         vy= vec[1];
2268         vz= vec[2];
2269         vx2= vx*vx;
2270         vy2= vy*vy;
2271         vz2= vz*vz;
2272         co= (float)cos(phi);
2273         si= (float)sin(phi);
2274         
2275         mat[0][0]= vx2+co*(1.0f-vx2);
2276         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2277         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2278         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2279         mat[1][1]= vy2+co*(1.0f-vy2);
2280         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2281         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2282         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2283         mat[2][2]= vz2+co*(1.0f-vz2);
2284         
2285 }
2286
2287 void VecRotToQuat( float *vec, float phi, float *quat)
2288 {
2289         /* rotation of phi radials around vec */
2290         float si;
2291
2292         quat[1]= vec[0];
2293         quat[2]= vec[1];
2294         quat[3]= vec[2];
2295                                                                                                            
2296         if( Normalise(quat+1) == 0.0) {
2297                 QuatOne(quat);
2298         }
2299         else {
2300                 quat[0]= (float)cos( phi/2.0 );
2301                 si= (float)sin( phi/2.0 );
2302                 quat[1] *= si;
2303                 quat[2] *= si;
2304                 quat[3] *= si;
2305         }
2306 }
2307
2308 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees */
2309 float VecAngle3( float *v1, float *v2, float *v3)
2310 {
2311         float vec1[3], vec2[3];
2312
2313         VecSubf(vec1, v2, v1);
2314         VecSubf(vec2, v2, v3);
2315         Normalise(vec1);
2316         Normalise(vec2);
2317         return saacos(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]) * 180.0/M_PI;
2318 }
2319
2320 /* Return the angle in degrees between vecs 1-2 and 2-3 */
2321 float VecAngle2( float *v1, float *v2)
2322 {
2323         float vec1[3], vec2[3];
2324         VecCopyf(vec1, v1);
2325         VecCopyf(vec2, v2);
2326         Normalise(vec1);
2327         Normalise(vec2);
2328         return saacos(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]) * 180.0/M_PI;
2329 }
2330
2331
2332 void euler_rot(float *beul, float ang, char axis)
2333 {
2334         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2335         
2336         eul[0]= eul[1]= eul[2]= 0.0;
2337         if(axis=='x') eul[0]= ang;
2338         else if(axis=='y') eul[1]= ang;
2339         else eul[2]= ang;
2340         
2341         EulToMat3(eul, mat1);
2342         EulToMat3(beul, mat2);
2343         
2344         Mat3MulMat3(totmat, mat2, mat1);
2345         
2346         Mat3ToEul(totmat, beul);
2347         
2348 }
2349
2350 /* exported to transform.c */
2351 void compatible_eul(float *eul, float *oldrot)
2352 {
2353         float dx, dy, dz;
2354         
2355         /* correct differences of about 360 degrees first */
2356         
2357         dx= eul[0] - oldrot[0];
2358         dy= eul[1] - oldrot[1];
2359         dz= eul[2] - oldrot[2];
2360         
2361         while( fabs(dx) > 5.1) {
2362                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2363                 dx= eul[0] - oldrot[0];
2364         }
2365         while( fabs(dy) > 5.1) {
2366                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2367                 dy= eul[1] - oldrot[1];
2368         }
2369         while( fabs(dz) > 5.1 ) {
2370                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2371                 dz= eul[2] - oldrot[2];
2372         }
2373         
2374         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
2375         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
2376                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2377         }
2378         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
2379                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2380         }
2381         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
2382                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2383         }
2384         
2385         /* this return was there from ancient days... but why! probably because the code sucks :)
2386                 */
2387         return;
2388         
2389         /* calc again */
2390         dx= eul[0] - oldrot[0];
2391         dy= eul[1] - oldrot[1];
2392         dz= eul[2] - oldrot[2];
2393         
2394         /* special case, tested for x-z  */
2395         
2396         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
2397                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2398                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
2399                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2400                 
2401         }
2402         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
2403                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
2404                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2405                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
2406         }
2407         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
2408                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
2409                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
2410                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
2411         }
2412         
2413 }
2414
2415
2416
2417 /* ******************************************** */
2418
2419 void SizeToMat3( float *size, float mat[][3])
2420 {
2421         mat[0][0]= size[0];
2422         mat[0][1]= 0.0;
2423         mat[0][2]= 0.0;
2424         mat[1][1]= size[1];
2425         mat[1][0]= 0.0;
2426         mat[1][2]= 0.0;
2427         mat[2][2]= size[2];
2428         mat[2][1]= 0.0;
2429         mat[2][0]= 0.0;
2430 }
2431
2432 void Mat3ToSize( float mat[][3], float *size)
2433 {
2434         float vec[3];
2435
2436         VecCopyf(vec, mat[0]);
2437         size[0]= Normalise(vec);
2438         VecCopyf(vec, mat[1]);
2439         size[1]= Normalise(vec);
2440         VecCopyf(vec, mat[2]);
2441         size[2]= Normalise(vec);
2442
2443 }
2444
2445 void Mat4ToSize( float mat[][4], float *size)
2446 {
2447         float vec[3];
2448         
2449
2450         VecCopyf(vec, mat[0]);
2451         size[0]= Normalise(vec);
2452         VecCopyf(vec, mat[1]);
2453         size[1]= Normalise(vec);
2454         VecCopyf(vec, mat[2]);
2455         size[2]= Normalise(vec);
2456 }
2457
2458 /* ************* SPECIALS ******************* */
2459
2460 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
2461 {
2462         /* imaginary x-axis, y-axis triangle is being rotated */
2463         float vec[3], q1[4], q2[4], n[3], si, co, hoek, mat[3][3], imat[3][3];
2464         
2465         /* move z-axis to face-normal */
2466         CalcNormFloat(v1, v2, v3, vec);
2467
2468         n[0]= vec[1];
2469         n[1]= -vec[0];
2470         n[2]= 0.0;
2471         Normalise(n);
2472         
2473         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
2474         
2475         hoek= -0.5f*saacos(vec[2]);
2476         co= (float)cos(hoek);
2477         si= (float)sin(hoek);
2478         q1[0]= co;
2479         q1[1]= n[0]*si;
2480         q1[2]= n[1]*si;
2481         q1[3]= 0.0f;
2482         
2483         /* rotate back line v1-v2 */
2484         QuatToMat3(q1, mat);
2485         Mat3Inv(imat, mat);
2486         VecSubf(vec, v2, v1);
2487         Mat3MulVecfl(imat, vec);
2488
2489         /* what angle has this line with x-axis? */
2490         vec[2]= 0.0;
2491         Normalise(vec);
2492
2493         hoek= (float)(0.5*atan2(vec[1], vec[0]));
2494         co= (float)cos(hoek);
2495         si= (float)sin(hoek);
2496         q2[0]= co;
2497         q2[1]= 0.0f;
2498         q2[2]= 0.0f;
2499         q2[3]= si;
2500         
2501         QuatMul(quat, q1, q2);
2502 }
2503
2504 void MinMaxRGB(short c[])
2505 {
2506         if(c[0]>255) c[0]=255;
2507         else if(c[0]<0) c[0]=0;
2508         if(c[1]>255) c[1]=255;
2509         else if(c[1]<0) c[1]=0;
2510         if(c[2]>255) c[2]=255;
2511         else if(c[2]<0) c[2]=0;
2512 }
2513
2514 float Vec2Lenf(float *v1, float *v2)
2515 {
2516         float x, y;
2517
2518         x = v1[0]-v2[0];
2519         y = v1[1]-v2[1];
2520         return (float)sqrt(x*x+y*y);
2521 }
2522
2523 void Vec2Mulf(float *v1, float f)
2524 {
2525         v1[0]*= f;
2526         v1[1]*= f;
2527 }
2528
2529 void Vec2Addf(float *v, float *v1, float *v2)
2530 {
2531         v[0]= v1[0]+ v2[0];
2532         v[1]= v1[1]+ v2[1];
2533 }
2534
2535 void Vec2Subf(float *v, float *v1, float *v2)
2536 {
2537         v[0]= v1[0]- v2[0];
2538         v[1]= v1[1]- v2[1];
2539 }
2540
2541 void Vec2Copyf(float *v1, float *v2)
2542 {
2543         v1[0]= v2[0];
2544         v1[1]= v2[1];
2545 }
2546
2547 float Inp2f(float *v1, float *v2)
2548 {
2549         return v1[0]*v2[0]+v1[1]*v2[1];
2550 }
2551
2552 float Normalise2(float *n)
2553 {
2554         float d;
2555         
2556         d= n[0]*n[0]+n[1]*n[1];
2557
2558         if(d>1.0e-35F) {
2559                 d= (float)sqrt(d);
2560
2561                 n[0]/=d; 
2562                 n[1]/=d; 
2563         } else {
2564                 n[0]=n[1]= 0.0;
2565                 d= 0.0;
2566         }
2567         return d;
2568 }
2569
2570 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
2571 {
2572         int i;
2573         float f, p, q, t;
2574
2575         h *= 360.0f;
2576         
2577         if(s==0.0) {
2578                 *r = v;
2579                 *g = v;
2580                 *b = v;
2581         }
2582         else {
2583                 if(h==360) h = 0;
2584                 
2585                 h /= 60;
2586                 i = (int)floor(h);
2587                 f = h - i;
2588                 p = v*(1.0f-s);
2589                 q = v*(1.0f-(s*f));
2590                 t = v*(1.0f-(s*(1.0f-f)));
2591                 
2592                 switch (i) {
2593                 case 0 :
2594                         *r = v;
2595                         *g = t;
2596                         *b = p;
2597                         break;
2598                 case 1 :
2599                         *r = q;
2600                         *g = v;
2601                         *b = p;
2602                         break;
2603                 case 2 :
2604                         *r = p;
2605                         *g = v;
2606                         *b = t;
2607                         break;
2608                 case 3 :
2609                         *r = p;
2610                         *g = q;
2611                         *b = v;
2612                         break;
2613                 case 4 :
2614                         *r = t;
2615                         *g = p;
2616                         *b = v;
2617                         break;
2618                 case 5 :
2619                         *r = v;
2620                         *g = p;
2621                         *b = q;
2622                         break;
2623                 }
2624         }
2625 }
2626
2627 void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
2628 {
2629         unsigned int ri, gi, bi;
2630         
2631         if (hexcol[0] == '#') hexcol++;
2632         
2633         if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
2634                 *r = ri / 255.0;
2635                 *g = gi / 255.0;                
2636                 *b = bi / 255.0;
2637         }
2638 }
2639
2640 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
2641 {
2642         float h, s, v;
2643         float cmax, cmin, cdelta;
2644         float rc, gc, bc;
2645
2646         cmax = r;
2647         cmin = r;
2648         cmax = (g>cmax ? g:cmax);
2649         cmin = (g<cmin ? g:cmin);
2650         cmax = (b>cmax ? b:cmax);
2651         cmin = (b<cmin ? b:cmin);
2652
2653         v = cmax;               /* value */
2654         if (cmax!=0.0)
2655                 s = (cmax - cmin)/cmax;
2656         else {
2657                 s = 0.0;
2658                 h = 0.0;
2659         }
2660         if (s == 0.0)
2661                 h = -1.0;
2662         else {
2663                 cdelta = cmax-cmin;
2664                 rc = (cmax-r)/cdelta;
2665                 gc = (cmax-g)/cdelta;
2666                 bc = (cmax-b)/cdelta;
2667                 if (r==cmax)
2668                         h = bc-gc;
2669                 else
2670                         if (g==cmax)
2671                                 h = 2.0f+rc-bc;
2672                         else
2673                                 h = 4.0f+gc-rc;
2674                 h = h*60.0f;
2675                 if (h<0.0f)
2676                         h += 360.0f;
2677         }
2678         
2679         *ls = s;
2680         *lh = h/360.0f;
2681         if( *lh < 0.0) *lh= 0.0;
2682         *lv = v;
2683 }
2684
2685
2686 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
2687    for that reason it is sensitive for endianness... with this function it works correctly
2688 */
2689
2690 unsigned int hsv_to_cpack(float h, float s, float v)
2691 {
2692         short r, g, b;
2693         float rf, gf, bf;
2694         unsigned int col;
2695         
2696         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
2697         
2698         r= (short)(rf*255.0f);
2699         g= (short)(gf*255.0f);
2700         b= (short)(bf*255.0f);
2701         
2702         col= ( r + (g*256) + (b*256*256) );
2703         return col;
2704 }
2705
2706
2707 unsigned int rgb_to_cpack(float r, float g, float b)
2708 {
2709         int ir, ig, ib;
2710         
2711         ir= (int)floor(255.0*r);
2712         if(ir<0) ir= 0; else if(ir>255) ir= 255;
2713         ig= (int)floor(255.0*g);
2714         if(ig<0) ig= 0; else if(ig>255) ig= 255;
2715         ib= (int)floor(255.0*b);
2716         if(ib<0) ib= 0; else if(ib>255) ib= 255;
2717         
2718         return (ir+ (ig*256) + (ib*256*256));
2719 }
2720
2721 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
2722 {
2723         
2724         *r= (float)((col)&0xFF);
2725         *r /= 255.0f;
2726
2727         *g= (float)(((col)>>8)&0xFF);
2728         *g /= 255.0f;
2729
2730         *b= (float)(((col)>>16)&0xFF);
2731         *b /= 255.0f;
2732 }
2733
2734
2735 /* *************** PROJECTIONS ******************* */
2736
2737 void tubemap(float x, float y, float z, float *u, float *v)
2738 {
2739         float len;
2740         
2741         *v = (z + 1.0) / 2.0;
2742         
2743         len= sqrt(x*x+y*y);
2744         if(len>0) {
2745                 *u = (1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0;
2746         }
2747 }
2748
2749 /* ------------------------------------------------------------------------- */
2750
2751 void spheremap(float x, float y, float z, float *u, float *v)
2752 {
2753         float len;
2754         
2755         len= sqrt(x*x+y*y+z*z);
2756         if(len>0.0) {
2757                 
2758                 if(x==0.0 && y==0.0) *u= 0.0;   /* othwise domain error */
2759                 else *u = (1.0 - atan2(x,y)/M_PI )/2.0;
2760                 
2761                 z/=len;
2762                 *v = 1.0- saacos(z)/M_PI;
2763         }
2764 }
2765
2766 /* ------------------------------------------------------------------------- */
2767
2768 /* *****************  m1 = m2 *****************  */
2769 void cpy_m3_m3(float m1[][3], float m2[][3]) 
2770 {       
2771         memcpy(m1[0], m2[0], 9*sizeof(float));
2772 }
2773
2774 /* *****************  m1 = m2 *****************  */
2775 void cpy_m4_m4(float m1[][4], float m2[][4]) 
2776 {       
2777         memcpy(m1[0], m2[0], 16*sizeof(float));
2778 }
2779
2780 /* ***************** identity matrix *****************  */
2781 void ident_m4(float m[][4])
2782 {
2783         
2784         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
2785         m[0][1]= m[0][2]= m[0][3]= 0.0;
2786         m[1][0]= m[1][2]= m[1][3]= 0.0;
2787         m[2][0]= m[2][1]= m[2][3]= 0.0;
2788         m[3][0]= m[3][1]= m[3][2]= 0.0;
2789 }
2790
2791
2792 /* *****************  m1 = m2 (pre) * m3 (post) ***************** */
2793 void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
2794 {
2795         float m[3][3];
2796         
2797         m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2]; 
2798         m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2]; 
2799         m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2]; 
2800
2801         m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2]; 
2802         m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2]; 
2803         m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2]; 
2804
2805         m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2]; 
2806         m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2]; 
2807         m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2]; 
2808
2809         cpy_m3_m3(m1, m2);
2810 }
2811
2812 /*  ***************** m1 = m2 (pre) * m3 (post) ***************** */
2813 void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
2814 {
2815         float m[4][4];
2816         
2817         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]; 
2818         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]; 
2819         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]; 
2820         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]; 
2821
2822         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]; 
2823         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]; 
2824         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]; 
2825         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]; 
2826
2827         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]; 
2828         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]; 
2829         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]; 
2830         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]; 
2831         
2832         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]; 
2833         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]; 
2834         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]; 
2835         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]; 
2836         
2837         cpy_m4_m4(m1, m2);
2838 }
2839
2840 /*  ***************** m1 = inverse(m2)  *****************  */
2841 void inv_m3_m3(float m1[][3], float m2[][3])
2842 {
2843         short a,b;
2844         float det;
2845         
2846         /* calc adjoint */
2847         Mat3Adj(m1, m2);
2848         
2849         /* then determinant old matrix! */
2850         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
2851             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
2852             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
2853         
2854         if(det==0.0f) det=1.0f;
2855         det= 1.0f/det;
2856         for(a=0;a<3;a++) {
2857                 for(b=0;b<3;b++) {
2858                         m1[a][b]*=det;
2859                 }
2860         }
2861 }
2862
2863 /*  ***************** m1 = inverse(m2)  *****************  */
2864 int inv_m4_m4(float inverse[][4], float mat[][4])
2865 {
2866         int i, j, k;
2867         double temp;
2868         float tempmat[4][4];
2869         float max;
2870         int maxj;
2871         
2872         /* Set inverse to identity */
2873         ident_m4(inverse);
2874         
2875         /* Copy original matrix so we don't mess it up */
2876         cpy_m4_m4(tempmat, mat);
2877         
2878         for(i = 0; i < 4; i++) {
2879                 /* Look for row with max pivot */
2880                 max = ABS(tempmat[i][i]);
2881                 maxj = i;
2882                 for(j = i + 1; j < 4; j++) {
2883                         if(ABS(tempmat[j][i]) > max) {
2884                                 max = ABS(tempmat[j][i]);
2885                                 maxj = j;
2886                         }
2887                 }
2888                 /* Swap rows if necessary */
2889                 if (maxj != i) {
2890                         for( k = 0; k < 4; k++) {
2891                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
2892                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
2893                         }
2894                 }
2895                 
2896                 temp = tempmat[i][i];
2897                 if (temp == 0)
2898                         return 0;  /* No non-zero pivot */
2899                 for(k = 0; k < 4; k++) {
2900                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
2901                         inverse[i][k] = (float)(inverse[i][k]/temp);
2902                 }
2903                 for(j = 0; j < 4; j++) {
2904                         if(j != i) {
2905                                 temp = tempmat[j][i];
2906                                 for(k = 0; k < 4; k++) {
2907                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
2908                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
2909                                 }
2910                         }
2911                 }
2912         }
2913         return 1;
2914 }
2915
2916 /*  ***************** v1 = v2 * mat  ***************** */
2917 void mul_v3_v3m4(float *v1, float *v2, float mat[][4])
2918 {
2919         float x, y;
2920         
2921         x= v2[0];       // work with a copy, v1 can be same as v2
2922         y= v2[1];
2923         v1[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*v2[2] + mat[3][0];
2924         v1[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*v2[2] + mat[3][1];
2925         v1[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*v2[2] + mat[3][2];
2926         
2927 }
2928
2929 /* moved from effect.c
2930    test if the line starting at p1 ending at p2 intersects the triangle v0..v2
2931    return non zero if it does 
2932
2933    wild guess .. says did not check the math since i don't need it
2934    and if (it intersects) lambda returns intersection = p1 + lambda(p1-p2)
2935 */
2936 int LineIntersectsTriangle(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)
2937 {
2938
2939         float p[3], s[3], d[3], e1[3], e2[3], q[3];
2940         float a, f, u, v;
2941         
2942         VecSubf(e1, v1, v0);
2943         VecSubf(e2, v2, v0);
2944         VecSubf(d, p2, p1);
2945         
2946         Crossf(p, d, e2);
2947         a = Inpf(e1, p);
2948         if ((a > -0.000001) && (a < 0.000001)) return 0;
2949         f = 1.0f/a;
2950         
2951         VecSubf(s, p1, v0);
2952         
2953         Crossf(q, s, e1);
2954         *lambda = f * Inpf(e2, q);
2955         if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
2956         
2957         u = f * Inpf(s, p);
2958         if ((u < 0.0)||(u > 1.0)) return 0;
2959         
2960         v = f * Inpf(d, q);
2961         if ((v < 0.0)||((u + v) > 1.0)) return 0;
2962         
2963         return 1;
2964 }
2965