88838342046a79f83d8483a8b57a30c92eb82700
[blender.git] / source / blender / blenlib / intern / arithb.c
1
2 /*      formules voor blender   
3  *
4  * sort of cleaned up mar-01 nzc
5  *
6  * Functions here get counterparts with MTC prefixes. Basically, we phase
7  * out the calls here in favour of fully prototyped versions.
8  *
9  * $Id$
10  *
11  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
12  *
13  * This program is free software; you can redistribute it and/or
14  * modify it under the terms of the GNU General Public License
15  * as published by the Free Software Foundation; either version 2
16  * of the License, or (at your option) any later version. The Blender
17  * Foundation also sells licenses for use in proprietary software under
18  * the Blender License.  See http://www.blender.org/BL/ for information
19  * about this.
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software Foundation,
28  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
29  *
30  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
31  * All rights reserved.
32  *
33  * The Original Code is: all of this file.
34  *
35  * Contributor(s): none yet.
36  *
37  * ***** END GPL/BL DUAL LICENSE BLOCK *****
38  */
39
40 /* ************************ FUNKTIES **************************** */
41
42 #include <math.h>
43 #include <sys/types.h>
44 #include <string.h> 
45 #include <float.h>
46
47 #ifdef HAVE_CONFIG_H
48 #include <config.h>
49 #endif
50
51 #ifdef __sun__
52 #include <strings.h>
53 #endif
54
55 #if !defined(__sgi) && !defined(WIN32)
56 #include <sys/time.h>
57 #include <unistd.h>
58 #endif
59
60 #include <stdio.h>
61 #include "BLI_arithb.h"
62
63 /* A few small defines. Keep'em local! */
64 #define SMALL_NUMBER    1.e-8
65 #define ABS(x)  ((x) < 0 ? -(x) : (x))
66 #define SWAP(type, a, b)        { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
67
68
69 #if defined(WIN32) || defined(__APPLE__)
70 #include <stdlib.h>
71 #define M_PI 3.14159265358979323846
72 #define M_SQRT2 1.41421356237309504880   
73
74 #endif /* defined(WIN32) || defined(__APPLE__) */
75
76
77 float saacos(float fac)
78 {
79         if(fac<= -1.0f) return (float)M_PI;
80         else if(fac>=1.0f) return 0.0;
81         else return (float)acos(fac);
82 }
83
84 float sasqrt(float fac)
85 {
86         if(fac<=0.0) return 0.0;
87         return (float)sqrt(fac);
88 }
89
90 float Normalise(float *n)
91 {
92         float d;
93         
94         d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
95         /* FLT_EPSILON is too large! A larger value causes normalise errors in a scaled down utah teapot */
96         if(d>0.0000000000001) {
97                 d= (float)sqrt(d);
98
99                 n[0]/=d; 
100                 n[1]/=d; 
101                 n[2]/=d;
102         } else {
103                 n[0]=n[1]=n[2]= 0.0;
104                 d= 0.0;
105         }
106         return d;
107 }
108
109 void Crossf(float *c, const float *a, const float *b)
110 {
111         c[0] = a[1] * b[2] - a[2] * b[1];
112         c[1] = a[2] * b[0] - a[0] * b[2];
113         c[2] = a[0] * b[1] - a[1] * b[0];
114 }
115
116 float Inpf(const float *v1, const float *v2)
117 {
118         return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
119 }
120
121 void Mat3Transp(float mat[][3])
122 {
123         float t;
124
125         t = mat[0][1] ; 
126         mat[0][1] = mat[1][0] ; 
127         mat[1][0] = t;
128         t = mat[0][2] ; 
129         mat[0][2] = mat[2][0] ; 
130         mat[2][0] = t;
131         t = mat[1][2] ; 
132         mat[1][2] = mat[2][1] ; 
133         mat[2][1] = t;
134 }
135
136 void Mat4Transp(float mat[][4])
137 {
138         float t;
139
140         t = mat[0][1] ; 
141         mat[0][1] = mat[1][0] ; 
142         mat[1][0] = t;
143         t = mat[0][2] ; 
144         mat[0][2] = mat[2][0] ; 
145         mat[2][0] = t;
146         t = mat[0][3] ; 
147         mat[0][3] = mat[3][0] ; 
148         mat[3][0] = t;
149
150         t = mat[1][2] ; 
151         mat[1][2] = mat[2][1] ; 
152         mat[2][1] = t;
153         t = mat[1][3] ; 
154         mat[1][3] = mat[3][1] ; 
155         mat[3][1] = t;
156
157         t = mat[2][3] ; 
158         mat[2][3] = mat[3][2] ; 
159         mat[3][2] = t;
160 }
161
162
163 /*
164  * invertmat - 
165  *              computes the inverse of mat and puts it in inverse.  Returns 
166  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
167  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
168  *
169  *                                      Mark Segal - 1992
170  */
171
172 int Mat4Invert(float inverse[][4], const float mat[][4])
173 {
174         int i, j, k;
175         double temp;
176         float tempmat[4][4];
177         float max;
178         int maxj;
179
180         /* Set inverse to identity */
181         for (i=0; i<4; i++)
182                 for (j=0; j<4; j++)
183                         inverse[i][j] = 0;
184         for (i=0; i<4; i++)
185                 inverse[i][i] = 1;
186
187         /* Copy original matrix so we don't mess it up */
188         for(i = 0; i < 4; i++)
189                 for(j = 0; j <4; j++)
190                         tempmat[i][j] = mat[i][j];
191
192         for(i = 0; i < 4; i++) {
193                 /* Look for row with max pivot */
194                 max = ABS(tempmat[i][i]);
195                 maxj = i;
196                 for(j = i + 1; j < 4; j++) {
197                         if(ABS(tempmat[j][i]) > max) {
198                                 max = ABS(tempmat[j][i]);
199                                 maxj = j;
200                         }
201                 }
202                 /* Swap rows if necessary */
203                 if (maxj != i) {
204                         for( k = 0; k < 4; k++) {
205                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
206                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
207                         }
208                 }
209
210                 temp = tempmat[i][i];
211                 if (temp == 0)
212                         return 0;  /* No non-zero pivot */
213                 for(k = 0; k < 4; k++) {
214                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
215                         inverse[i][k] = (float)(inverse[i][k]/temp);
216                 }
217                 for(j = 0; j < 4; j++) {
218                         if(j != i) {
219                                 temp = tempmat[j][i];
220                                 for(k = 0; k < 4; k++) {
221                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
222                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
223                                 }
224                         }
225                 }
226         }
227         return 1;
228 }
229 #ifdef TEST_ACTIVE
230 void Mat4InvertSimp(float inverse[][4], const float mat[][4])
231 {
232         /* alleen HOEK bewarende Matrices */
233         /* gebaseerd op GG IV pag 205 */
234         float scale;
235         
236         scale= mat[0][0]*mat[0][0] + mat[1][0]*mat[1][0] + mat[2][0]*mat[2][0];
237         if(scale==0.0) return;
238         
239         scale= 1.0/scale;
240         
241         /* transpose en scale */
242         inverse[0][0]= scale*mat[0][0];
243         inverse[1][0]= scale*mat[0][1];
244         inverse[2][0]= scale*mat[0][2];
245         inverse[0][1]= scale*mat[1][0];
246         inverse[1][1]= scale*mat[1][1];
247         inverse[2][1]= scale*mat[1][2];
248         inverse[0][2]= scale*mat[2][0];
249         inverse[1][2]= scale*mat[2][1];
250         inverse[2][2]= scale*mat[2][2];
251
252         inverse[3][0]= -(inverse[0][0]*mat[3][0] + inverse[1][0]*mat[3][1] + inverse[2][0]*mat[3][2]);
253         inverse[3][1]= -(inverse[0][1]*mat[3][0] + inverse[1][1]*mat[3][1] + inverse[2][1]*mat[3][2]);
254         inverse[3][2]= -(inverse[0][2]*mat[3][0] + inverse[1][2]*mat[3][1] + inverse[2][2]*mat[3][2]);
255         
256         inverse[0][3]= inverse[1][3]= inverse[2][3]= 0.0;
257         inverse[3][3]= 1.0;
258 }
259 #endif
260 /*  struct Matrix4; */
261
262 #ifdef TEST_ACTIVE
263 /* this seems to be unused.. */
264
265 void Mat4Inv(float *m1, const float *m2)
266 {
267
268 /* This gets me into trouble:  */
269         float mat1[3][3], mat2[3][3]; 
270         
271 /*      void Mat3Inv(); */
272 /*      void Mat3CpyMat4(); */
273 /*      void Mat4CpyMat3(); */
274
275         Mat3CpyMat4((float*)mat2,m2);
276         Mat3Inv((float*)mat1, (float*) mat2);
277         Mat4CpyMat3(m1, mat1);
278
279 }
280 #endif
281
282
283 float Det2x2(float a,float b,float c,float d)
284 {
285
286         return a*d - b*c;
287 }
288
289
290
291 float Det3x3(float a1, float a2, float a3,
292                          float b1, float b2, float b3,
293                          float c1, float c2, float c3 )
294 {
295         float ans;
296
297         ans = a1 * Det2x2( b2, b3, c2, c3 )
298             - b1 * Det2x2( a2, a3, c2, c3 )
299             + c1 * Det2x2( a2, a3, b2, b3 );
300
301         return ans;
302 }
303
304 float Det4x4(const float m[][4])
305 {
306         float ans;
307         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
308
309         a1= m[0][0]; 
310         b1= m[0][1];
311         c1= m[0][2]; 
312         d1= m[0][3];
313
314         a2= m[1][0]; 
315         b2= m[1][1];
316         c2= m[1][2]; 
317         d2= m[1][3];
318
319         a3= m[2][0]; 
320         b3= m[2][1];
321         c3= m[2][2]; 
322         d3= m[2][3];
323
324         a4= m[3][0]; 
325         b4= m[3][1];
326         c4= m[3][2]; 
327         d4= m[3][3];
328
329         ans = a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
330             - b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
331             + c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
332             - d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
333
334         return ans;
335 }
336
337
338 void Mat4Adj(float out[][4], const float in[][4])       /* out = ADJ(in) */
339 {
340         float a1, a2, a3, a4, b1, b2, b3, b4;
341         float c1, c2, c3, c4, d1, d2, d3, d4;
342
343         a1= in[0][0]; 
344         b1= in[0][1];
345         c1= in[0][2]; 
346         d1= in[0][3];
347
348         a2= in[1][0]; 
349         b2= in[1][1];
350         c2= in[1][2]; 
351         d2= in[1][3];
352
353         a3= in[2][0]; 
354         b3= in[2][1];
355         c3= in[2][2]; 
356         d3= in[2][3];
357
358         a4= in[3][0]; 
359         b4= in[3][1];
360         c4= in[3][2]; 
361         d4= in[3][3];
362
363
364         out[0][0]  =   Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
365         out[1][0]  = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
366         out[2][0]  =   Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
367         out[3][0]  = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
368
369         out[0][1]  = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
370         out[1][1]  =   Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
371         out[2][1]  = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
372         out[3][1]  =   Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
373
374         out[0][2]  =   Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
375         out[1][2]  = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
376         out[2][2]  =   Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
377         out[3][2]  = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
378
379         out[0][3]  = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
380         out[1][3]  =   Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
381         out[2][3]  = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
382         out[3][3]  =   Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
383 }
384
385 void Mat4InvGG(float out[][4], const float in[][4])     /* van Graphic Gems I, out= INV(in)  */
386 {
387         int i, j;
388         float det;
389
390         /* calculate the adjoint matrix */
391
392         Mat4Adj(out,in);
393
394         det = Det4x4(out);
395
396         if ( fabs( det ) < SMALL_NUMBER) {
397                 return;
398         }
399
400         /* scale the adjoint matrix to get the inverse */
401
402         for (i=0; i<4; i++)
403                 for(j=0; j<4; j++)
404                         out[i][j] = out[i][j] / det;
405
406         /* de laatste factor is niet altijd 1. Hierdoor moet eigenlijk nog gedeeld worden */
407 }
408
409
410 void Mat3Inv(float m1[][3], const float m2[][3])
411 {
412         short a,b;
413         float det;
414
415         /* eerst adjoint */
416         Mat3Adj(m1,m2);
417
418         /* dan det oude mat! */
419         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
420             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
421             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
422
423         if(det==0) det=1;
424         det= 1/det;
425         for(a=0;a<3;a++) {
426                 for(b=0;b<3;b++) {
427                         m1[a][b]*=det;
428                 }
429         }
430 }
431
432 void Mat3Adj(float m1[][3], const float m[][3])
433 {
434         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
435         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
436         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
437
438         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
439         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
440         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
441
442         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
443         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
444         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
445 }
446
447 void Mat4MulMat4(float m1[][4], const float m2[][4], const float m3[][4])
448 {
449   /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
450
451         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];
452         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];
453         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];
454         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];
455
456         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];
457         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];
458         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];
459         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];
460
461         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];
462         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];
463         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];
464         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];
465
466         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];
467         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];
468         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];
469         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];
470
471 }
472 #ifdef TEST_ACTIVE
473 void subMat4MulMat4(float *m1, const float *m2, const float *m3)
474 {
475
476         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
477         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
478         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
479         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
480         m1+=4;
481         m2+=4;
482         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
483         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
484         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
485         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
486         m1+=4;
487         m2+=4;
488         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
489         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
490         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
491         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
492 }
493 #endif
494
495 #ifndef TEST_ACTIVE
496 void Mat3MulMat3(float m1[][3], const float m3[][3], const float m2[][3])
497 #else
498 void Mat3MulMat3(float *m1, const float *m3, const float *m2)
499 #endif
500 {
501    /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
502 #ifndef TEST_ACTIVE
503         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 
504         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 
505         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 
506
507         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 
508         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 
509         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 
510
511         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 
512         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 
513         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
514 #else
515         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
516         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
517         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
518         m1+=3;
519         m2+=3;
520         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
521         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
522         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
523         m1+=3;
524         m2+=3;
525         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
526         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
527         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
528 #endif
529 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
530
531 void Mat4MulMat43(float (*m1)[4], const float (*m3)[4], const float (*m2)[3])
532 {
533         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
534         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
535         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
536         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
537         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
538         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
539         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
540         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
541         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
542 }
543 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
544 void Mat3IsMat3MulMat4(float m1[][3], const float m2[][3], const float m3[][4])
545 {
546     /* m1[i][j] = m2[i][k] * m3[k][j] */
547     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
548     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
549     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
550
551     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
552     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
553     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
554
555     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
556     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
557     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
558 }
559
560
561
562 void Mat4MulMat34(float (*m1)[4], const float (*m3)[3], const float (*m2)[4])
563 {
564         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
565         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
566         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
567         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
568         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
569         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
570         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
571         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
572         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
573 }
574
575 void Mat4CpyMat4(float m1[][4], const float m2[][4]) 
576 {
577         memcpy(m1, m2, 4*4*sizeof(float));
578 }
579
580 void Mat4SwapMat4(float *m1, float *m2)
581 {
582         float t;
583         int i;
584
585         for(i=0;i<16;i++) {
586                 t= *m1;
587                 *m1= *m2;
588                 *m2= t;
589                 m1++; 
590                 m2++;
591         }
592 }
593
594 typedef float Mat3Row[3];
595 typedef float Mat4Row[4];
596
597 #ifdef TEST_ACTIVE
598 void Mat3CpyMat4(float *m1p, const float *m2p)
599 #else
600 void Mat3CpyMat4(float m1[][3], const float m2[][4])
601 #endif
602 {
603 #ifdef TEST_ACTIVE
604         int i, j;
605         Mat3Row *m1= (Mat3Row *)m1p; 
606         Mat4Row *m2= (Mat4Row *)m2p; 
607         for ( i = 0; i++; i < 3) {
608                 for (j = 0; j++; j < 3) {
609                         m1p[3*i + j] = m2p[4*i + j];
610                 }
611         }                       
612 #endif
613         m1[0][0]= m2[0][0];
614         m1[0][1]= m2[0][1];
615         m1[0][2]= m2[0][2];
616
617         m1[1][0]= m2[1][0];
618         m1[1][1]= m2[1][1];
619         m1[1][2]= m2[1][2];
620
621         m1[2][0]= m2[2][0];
622         m1[2][1]= m2[2][1];
623         m1[2][2]= m2[2][2];
624 }
625
626 /* Butched. See .h for comment */
627 /*  void Mat4CpyMat3(float m1[][4], float m2[][3]) */
628 #ifdef TEST_ACTIVE
629 void Mat4CpyMat3(float* m1, const float *m2)
630 {
631         int i;
632         for (i = 0; i < 3; i++) {
633                 m1[(4*i)]    = m2[(3*i)];
634                 m1[(4*i) + 1]= m2[(3*i) + 1];
635                 m1[(4*i) + 2]= m2[(3*i) + 2];
636                 m1[(4*i) + 3]= 0.0;
637                 i++;
638         }
639
640         m1[12]=m1[13]= m1[14]= 0.0;
641         m1[15]= 1.0;
642 }
643 #else
644
645 void Mat4CpyMat3(float m1[][4], const float m2[][3])    /* no clear */
646 {
647         m1[0][0]= m2[0][0];
648         m1[0][1]= m2[0][1];
649         m1[0][2]= m2[0][2];
650
651         m1[1][0]= m2[1][0];
652         m1[1][1]= m2[1][1];
653         m1[1][2]= m2[1][2];
654
655         m1[2][0]= m2[2][0];
656         m1[2][1]= m2[2][1];
657         m1[2][2]= m2[2][2];
658
659         /*      Reevan's Bugfix */
660         m1[0][3]=0.0F;
661         m1[1][3]=0.0F;
662         m1[2][3]=0.0F;
663
664         m1[3][0]=0.0F;  
665         m1[3][1]=0.0F;  
666         m1[3][2]=0.0F;  
667         m1[3][3]=1.0F;
668
669
670 }
671 #endif
672
673 void Mat3CpyMat3(float m1[][3], const float m2[][3]) 
674 {       
675         /* destination comes first: */
676         memcpy(&m1[0], &m2[0], 9*sizeof(float));
677 }
678
679 void Mat3MulSerie(float answ[][3],
680                                   const float m1[][3], const float m2[][3], const float m3[][3],
681                                   const float m4[][3], const float m5[][3], const float m6[][3],
682                                   const float m7[][3], const float m8[][3])
683 {
684         float temp[3][3];
685         
686         if(m1==0 || m2==0) return;
687
688         
689         Mat3MulMat3(answ, m2, m1);
690         if(m3) {
691                 Mat3MulMat3(temp, m3, answ);
692                 if(m4) {
693                         Mat3MulMat3(answ, m4, temp);
694                         if(m5) {
695                                 Mat3MulMat3(temp, m5, answ);
696                                 if(m6) {
697                                         Mat3MulMat3(answ, m6, temp);
698                                         if(m7) {
699                                                 Mat3MulMat3(temp, m7, answ);
700                                                 if(m8) {
701                                                         Mat3MulMat3(answ, m8, temp);
702                                                 }
703                                                 else Mat3CpyMat3(answ, temp);
704                                         }
705                                 }
706                                 else Mat3CpyMat3(answ, temp);
707                         }
708                 }
709                 else Mat3CpyMat3(answ, temp);
710         }
711 }
712
713 void Mat4MulSerie(float answ[][4], const float m1[][4],
714                                   const float m2[][4], const float m3[][4], const float m4[][4],
715                                   const float m5[][4], const float m6[][4], const float m7[][4],
716                                   const float m8[][4])
717 {
718         float temp[4][4];
719         
720         if(m1==0 || m2==0) return;
721         
722         Mat4MulMat4(answ, m2, m1);
723         if(m3) {
724                 Mat4MulMat4(temp, m3, answ);
725                 if(m4) {
726                         Mat4MulMat4(answ, m4, temp);
727                         if(m5) {
728                                 Mat4MulMat4(temp, m5, answ);
729                                 if(m6) {
730                                         Mat4MulMat4(answ, m6, temp);
731                                         if(m7) {
732                                                 Mat4MulMat4(temp, m7, answ);
733                                                 if(m8) {
734                                                         Mat4MulMat4(answ, m8, temp);
735                                                 }
736                                                 else Mat4CpyMat4(answ, temp);
737                                         }
738                                 }
739                                 else Mat4CpyMat4(answ, temp);
740                         }
741                 }
742                 else Mat4CpyMat4(answ, temp);
743         }
744 }
745
746
747
748 void Mat4Clr(float *m)
749 {
750         memset(m, 0, 4*4*sizeof(float));
751 }
752
753 void Mat3Clr(float *m)
754 {
755         memset(m, 0, 3*3*sizeof(float));
756 }
757
758 void Mat4One(float m[][4])
759 {
760
761         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
762         m[0][1]= m[0][2]= m[0][3]= 0.0;
763         m[1][0]= m[1][2]= m[1][3]= 0.0;
764         m[2][0]= m[2][1]= m[2][3]= 0.0;
765         m[3][0]= m[3][1]= m[3][2]= 0.0;
766 }
767
768 void Mat3One(float m[][3])
769 {
770
771         m[0][0]= m[1][1]= m[2][2]= 1.0;
772         m[0][1]= m[0][2]= 0.0;
773         m[1][0]= m[1][2]= 0.0;
774         m[2][0]= m[2][1]= 0.0;
775 }
776
777 void Mat4MulVec(const float mat[][4], int *vec)
778 {
779         int x,y;
780
781         x=vec[0]; 
782         y=vec[1];
783         vec[0]=(int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]);
784         vec[1]=(int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]);
785         vec[2]=(int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]);
786 }
787
788 void Mat4MulVecfl(const float mat[][4], float *vec)
789 {
790         float x,y;
791
792         x=vec[0]; 
793         y=vec[1];
794         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
795         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
796         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
797 }
798
799 void VecMat4MulVecfl(float *in, const float mat[][4], const float *vec)
800 {
801         float x,y;
802
803         x=vec[0]; 
804         y=vec[1];
805         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
806         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
807         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
808 }
809
810 void Mat4Mul3Vecfl(const float mat[][4], float *vec)
811 {
812         float x,y;
813
814         x= vec[0]; 
815         y= vec[1];
816         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
817         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
818         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
819 }
820
821 void Mat4MulVec4fl(const float mat[][4], float *vec)
822 {
823         float x,y,z;
824
825         x=vec[0]; 
826         y=vec[1]; 
827         z= vec[2];
828         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
829         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
830         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
831         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
832 }
833
834 void Mat3MulVec(const float mat[][3], int *vec)
835 {
836         int x,y;
837
838         x=vec[0]; 
839         y=vec[1];
840         vec[0]= (int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]);
841         vec[1]= (int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]);
842         vec[2]= (int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]);
843 }
844
845 void Mat3MulVecfl(const float mat[][3], float *vec)
846 {
847         float x,y;
848
849         x=vec[0]; 
850         y=vec[1];
851         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
852         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
853         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
854 }
855
856 void Mat3MulVecd(const float mat[][3], double *vec)
857 {
858         double x,y;
859
860         x=vec[0]; 
861         y=vec[1];
862         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
863         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
864         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
865 }
866
867 void Mat3TransMulVecfl(const float mat[][3], float *vec)
868 {
869         float x,y;
870
871         x=vec[0]; 
872         y=vec[1];
873         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
874         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
875         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
876 }
877
878 void Mat3MulFloat(float *m, float f)
879 {
880         int i;
881
882         for(i=0;i<9;i++) m[i]*=f;
883 }
884
885 void Mat4MulFloat(float *m, float f)
886 {
887         int i;
888
889         for(i=0;i<12;i++) m[i]*=f;      /* tot 12 tellen: zonder vector */
890 }
891
892
893 void Mat4MulFloat3(float *m, float f)           /* alleen de scale component */
894 {
895         int i,j;
896
897         for(i=0; i<3; i++) {
898                 for(j=0; j<3; j++) {
899                         
900                         m[4*i+j] *= f;
901                 }
902         }
903 }
904
905 void VecStar(float mat[][3], const float *vec)
906 {
907
908         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
909         mat[0][1]= -vec[2];     
910         mat[0][2]= vec[1];
911         mat[1][0]= vec[2];      
912         mat[1][2]= -vec[0];
913         mat[2][0]= -vec[1];     
914         mat[2][1]= vec[0];
915         
916 }
917 #ifdef TEST_ACTIVE
918 short EenheidsMat(float mat[][3])
919 {
920
921         if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0)
922                 if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0)
923                         if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0)
924                                 return 1;
925         return 0;
926 }
927 #endif
928
929 int FloatCompare(const float *v1, const float *v2, float limit)
930 {
931
932         if( fabs(v1[0]-v2[0])<limit ) {
933                 if( fabs(v1[1]-v2[1])<limit ) {
934                         if( fabs(v1[2]-v2[2])<limit ) return 1;
935                 }
936         }
937         return 0;
938 }
939
940 void printmatrix4(const char *str, const float m[][4])
941 {
942         printf("%s\n", str);
943         printf("%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3]);
944         printf("%f %f %f %f\n",m[1][0],m[1][1],m[1][2],m[1][3]);
945         printf("%f %f %f %f\n",m[2][0],m[2][1],m[2][2],m[2][3]);
946         printf("%f %f %f %f\n",m[3][0],m[3][1],m[3][2],m[3][3]);
947         printf("\n");
948
949 }
950
951 void printmatrix3(const char *str, const float m[][3])
952 {
953         printf("%s\n", str);
954         printf("%f %f %f\n",m[0][0],m[0][1],m[0][2]);
955         printf("%f %f %f\n",m[1][0],m[1][1],m[1][2]);
956         printf("%f %f %f\n",m[2][0],m[2][1],m[2][2]);
957         printf("\n");
958
959 }
960
961 /* **************** QUATERNIONS ********** */
962
963
964 void QuatMul(float *q, const float *q1, const float *q2)
965 {
966         float t0,t1,t2;
967
968         t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
969         t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
970         t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
971         q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
972         q[0]=t0; 
973         q[1]=t1; 
974         q[2]=t2;
975 }
976
977 void QuatSub(float *q, const float *q1, float *q2)
978 {
979         q2[0]= -q2[0];
980         QuatMul(q, q1, q2);
981         q2[0]= -q2[0];
982 }
983
984
985 void QuatToMat3(const float *q, float m[][3])
986 {
987         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
988
989         q0= M_SQRT2 * q[0];
990         q1= M_SQRT2 * q[1];
991         q2= M_SQRT2 * q[2];
992         q3= M_SQRT2 * q[3];
993
994         qda= q0*q1;
995         qdb= q0*q2;
996         qdc= q0*q3;
997         qaa= q1*q1;
998         qab= q1*q2;
999         qac= q1*q3;
1000         qbb= q2*q2;
1001         qbc= q2*q3;
1002         qcc= q3*q3;
1003
1004         m[0][0]= (float)(1.0-qbb-qcc);
1005         m[0][1]= (float)(qdc+qab);
1006         m[0][2]= (float)(-qdb+qac);
1007
1008         m[1][0]= (float)(-qdc+qab);
1009         m[1][1]= (float)(1.0-qaa-qcc);
1010         m[1][2]= (float)(qda+qbc);
1011
1012         m[2][0]= (float)(qdb+qac);
1013         m[2][1]= (float)(-qda+qbc);
1014         m[2][2]= (float)(1.0-qaa-qbb);
1015 }
1016
1017
1018 void QuatToMat4(const float *q, float m[][4])
1019 {
1020         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1021
1022         q0= M_SQRT2 * q[0];
1023         q1= M_SQRT2 * q[1];
1024         q2= M_SQRT2 * q[2];
1025         q3= M_SQRT2 * q[3];
1026
1027         qda= q0*q1;
1028         qdb= q0*q2;
1029         qdc= q0*q3;
1030         qaa= q1*q1;
1031         qab= q1*q2;
1032         qac= q1*q3;
1033         qbb= q2*q2;
1034         qbc= q2*q3;
1035         qcc= q3*q3;
1036
1037         m[0][0]= (float)(1.0-qbb-qcc);
1038         m[0][1]= (float)(qdc+qab);
1039         m[0][2]= (float)(-qdb+qac);
1040         m[0][3]= 0.0f;
1041
1042         m[1][0]= (float)(-qdc+qab);
1043         m[1][1]= (float)(1.0-qaa-qcc);
1044         m[1][2]= (float)(qda+qbc);
1045         m[1][3]= 0.0f;
1046
1047         m[2][0]= (float)(qdb+qac);
1048         m[2][1]= (float)(-qda+qbc);
1049         m[2][2]= (float)(1.0-qaa-qbb);
1050         m[2][3]= 0.0f;
1051         
1052         m[3][0]= m[3][1]= m[3][2]= 0.0f;
1053         m[3][3]= 1.0f;
1054 }
1055
1056 void Mat3ToQuat(const float wmat[][3], float *q)                /* uit Sig.Proc.85 pag 253 */
1057 {
1058         double tr, s;
1059         float mat[3][3];
1060
1061         /* voor de netheid: op kopie werken */
1062         Mat3CpyMat3(mat, wmat);
1063         Mat3Ortho(mat);                 /* dit moet EN op eind NormalQuat */
1064         
1065         tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
1066         
1067         if(tr>FLT_EPSILON) {
1068                 s= sqrt( tr);
1069                 q[0]= (float)s;
1070                 s*= 4.0;
1071                 q[1]= (float)((mat[1][2]-mat[2][1])/s);
1072                 q[2]= (float)((mat[2][0]-mat[0][2])/s);
1073                 q[3]= (float)((mat[0][1]-mat[1][0])/s);
1074         }
1075         else {
1076                 q[0]= 0.0f;
1077                 s= -0.5*(mat[1][1]+mat[2][2]);
1078                 
1079                 if(s>FLT_EPSILON) {
1080                         s= sqrt(s);
1081                         q[1]= (float)s;
1082                         q[2]= (float)(mat[0][1]/(2*s));
1083                         q[3]= (float)(mat[0][2]/(2*s));
1084                 }
1085                 else {
1086                         q[1]= 0.0f;
1087                         s= 0.5*(1.0-mat[2][2]);
1088                         
1089                         if(s>FLT_EPSILON) {
1090                                 s= sqrt(s);
1091                                 q[2]= (float)s;
1092                                 q[3]= (float)(mat[1][2]/(2*s));
1093                         }
1094                         else {
1095                                 q[2]= 0.0f;
1096                                 q[3]= 1.0f;
1097                         }
1098                 }
1099         }
1100         NormalQuat(q);
1101 }
1102
1103 void Mat3ToQuat_is_ok(const float wmat[][3], float *q)
1104 {
1105         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], hoek, si, co, nor[3];
1106
1107         /* voor de netheid: op kopie werken */
1108         Mat3CpyMat3(mat, wmat);
1109         Mat3Ortho(mat);
1110         
1111         /* roteer z-as matrix op z-as */
1112
1113         nor[0] = mat[2][1];             /* uitprodukt met (0,0,1) */
1114         nor[1] =  -mat[2][0];
1115         nor[2] = 0.0;
1116         Normalise(nor);
1117         
1118         co= mat[2][2];
1119         hoek= 0.5f*saacos(co);
1120         
1121         co= (float)cos(hoek);
1122         si= (float)sin(hoek);
1123         q1[0]= co;
1124         q1[1]= -nor[0]*si;              /* hier negatief, waarom is een raadsel */
1125         q1[2]= -nor[1]*si;
1126         q1[3]= -nor[2]*si;
1127
1128         /* roteer x-as van mat terug volgens inverse q1 */
1129         QuatToMat3(q1, matr);
1130         Mat3Inv(matn, matr);
1131         Mat3MulVecfl(matn, mat[0]);
1132         
1133         /* en zet de x-asssen gelijk */
1134         hoek= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1135         
1136         co= (float)cos(hoek);
1137         si= (float)sin(hoek);
1138         q2[0]= co;
1139         q2[1]= 0.0f;
1140         q2[2]= 0.0f;
1141         q2[3]= si;
1142         
1143         QuatMul(q, q1, q2);
1144 }
1145
1146
1147 void Mat4ToQuat(const float m[][4], float *q)
1148 {
1149         float mat[3][3];
1150         
1151         Mat3CpyMat4(mat, m);
1152         Mat3ToQuat(mat, q);
1153         
1154 }
1155
1156 void QuatOne(float *q)
1157 {
1158         q[0]= q[2]= q[3]= 0.0;
1159         q[1]= 1.0;
1160 }
1161
1162 void NormalQuat(float *q)
1163 {
1164         float len;
1165         
1166         len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
1167         if(len!=0.0) {
1168                 q[0]/= len;
1169                 q[1]/= len;
1170                 q[2]/= len;
1171                 q[3]/= len;
1172         } else {
1173                 q[1]= 1.0f;
1174                 q[0]= q[2]= q[3]= 0.0f;                 
1175         }
1176 }
1177
1178 float *vectoquat(const float *vec, short axis, short upflag)
1179 {
1180         static float q1[4];
1181         float q2[4], nor[3], *fp, mat[3][3], hoek, si, co, x2, y2, z2, len1;
1182         
1183         /* eerst roteer naar as */
1184         if(axis>2) {    
1185                 x2= vec[0] ; y2= vec[1] ; z2= vec[2];
1186                 axis-= 3;
1187         }
1188         else {
1189                 x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
1190         }
1191         
1192         q1[0]=1.0; 
1193         q1[1]=q1[2]=q1[3]= 0.0;
1194
1195         len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
1196         if(len1 == 0.0) return(q1);
1197
1198         /* nasty! I need a good routine for this...
1199          * problem is a rotation of an Y axis to the negative Y-axis for example.
1200          */
1201
1202         if(axis==0) {   /* x-as */
1203                 nor[0]= 0.0;
1204                 nor[1]= -z2;
1205                 nor[2]= y2;
1206
1207                 if( fabs(y2)+fabs(z2)<0.0001 ) {
1208                         nor[1]= 1.0;
1209                 }
1210
1211                 co= x2;
1212         }
1213         else if(axis==1) {      /* y-as */
1214                 nor[0]= z2;
1215                 nor[1]= 0.0;
1216                 nor[2]= -x2;
1217                 
1218                 if( fabs(x2)+fabs(z2)<0.0001 ) {
1219                         nor[2]= 1.0;
1220                 }
1221                 
1222                 co= y2;
1223         }
1224         else {                  /* z-as */
1225                 nor[0]= -y2;
1226                 nor[1]= x2;
1227                 nor[2]= 0.0;
1228
1229                 if( fabs(x2)+fabs(y2)<0.0001 ) {
1230                         nor[0]= 1.0;
1231                 }
1232
1233                 co= z2;
1234         }
1235         co/= len1;
1236
1237         Normalise(nor);
1238         
1239         hoek= 0.5f*saacos(co);
1240         si= (float)sin(hoek);
1241         q1[0]= (float)cos(hoek);
1242         q1[1]= nor[0]*si;
1243         q1[2]= nor[1]*si;
1244         q1[3]= nor[2]*si;
1245         
1246         if(axis!=upflag) {
1247                 QuatToMat3(q1, mat);
1248
1249                 fp= mat[2];
1250                 if(axis==0) {
1251                         if(upflag==1) hoek= (float)(0.5*atan2(fp[2], fp[1]));
1252                         else hoek= (float)(-0.5*atan2(fp[1], fp[2]));
1253                 }
1254                 else if(axis==1) {
1255                         if(upflag==0) hoek= (float)(-0.5*atan2(fp[2], fp[0]));
1256                         else hoek= (float)(0.5*atan2(fp[0], fp[2]));
1257                 }
1258                 else {
1259                         if(upflag==0) hoek= (float)(0.5*atan2(-fp[1], -fp[0]));
1260                         else hoek= (float)(-0.5*atan2(-fp[0], -fp[1]));
1261                 }
1262                                 
1263                 co= (float)cos(hoek);
1264                 si= (float)(sin(hoek)/len1);
1265                 q2[0]= co;
1266                 q2[1]= x2*si;
1267                 q2[2]= y2*si;
1268                 q2[3]= z2*si;
1269                         
1270                 QuatMul(q1,q2,q1);
1271         }
1272
1273         return(q1);
1274 }
1275
1276 void VecUpMat3old(const float *vec, float mat[][3], short axis)
1277 {
1278         float inp, up[3];
1279         short cox = 0, coy = 0, coz = 0;
1280         
1281         /* up varieeren heeft geen zin, is eigenlijk helemaal geen up!
1282          */
1283
1284         up[0]= 0.0;
1285         up[1]= 0.0;
1286         up[2]= 1.0;
1287
1288         if(axis==0) {
1289                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1290         }
1291         if(axis==1) {
1292                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1293         }
1294         if(axis==2) {
1295                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1296         }
1297         if(axis==3) {
1298                 cox= 0; coy= 2; coz= 1;         /*  */
1299         }
1300         if(axis==4) {
1301                 cox= 1; coy= 0; coz= 2;         /*  */
1302         }
1303         if(axis==5) {
1304                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1305         }
1306
1307         mat[coz][0]= vec[0];
1308         mat[coz][1]= vec[1];
1309         mat[coz][2]= vec[2];
1310         Normalise((float *)mat[coz]);
1311         
1312         inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
1313         mat[coy][0]= up[0] - inp*mat[coz][0];
1314         mat[coy][1]= up[1] - inp*mat[coz][1];
1315         mat[coy][2]= up[2] - inp*mat[coz][2];
1316
1317         Normalise((float *)mat[coy]);
1318         
1319         Crossf(mat[cox], mat[coy], mat[coz]);
1320         
1321 }
1322
1323 void VecUpMat3(float *vec, float mat[][3], short axis)
1324 {
1325         float inp;
1326         short cox = 0, coy = 0, coz = 0;
1327         
1328         /* up varieeren heeft geen zin, is eigenlijk helemaal geen up!
1329          * zie VecUpMat3old
1330          */
1331
1332         if(axis==0) {
1333                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1334         }
1335         if(axis==1) {
1336                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1337         }
1338         if(axis==2) {
1339                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1340         }
1341         if(axis==3) {
1342                 cox= 0; coy= 1; coz= 2;         /* Y op -Z tr */
1343                 vec[0]= -vec[0];
1344                 vec[1]= -vec[1];
1345                 vec[2]= -vec[2];
1346         }
1347         if(axis==4) {
1348                 cox= 1; coy= 0; coz= 2;         /*  */
1349         }
1350         if(axis==5) {
1351                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1352         }
1353
1354         mat[coz][0]= vec[0];
1355         mat[coz][1]= vec[1];
1356         mat[coz][2]= vec[2];
1357         Normalise((float *)mat[coz]);
1358         
1359         inp= mat[coz][2];
1360         mat[coy][0]= - inp*mat[coz][0];
1361         mat[coy][1]= - inp*mat[coz][1];
1362         mat[coy][2]= 1.0f - inp*mat[coz][2];
1363
1364         Normalise((float *)mat[coy]);
1365         
1366         Crossf(mat[cox], mat[coy], mat[coz]);
1367         
1368 }
1369
1370
1371 /* **************** VIEW / PROJEKTIE ********************************  */
1372
1373
1374 void i_ortho(
1375         float left, float right,
1376         float bottom, float top,
1377         float nearClip, float farClip,
1378         float matrix[][4]
1379 ){
1380     float Xdelta, Ydelta, Zdelta;
1381  
1382     Xdelta = right - left;
1383     Ydelta = top - bottom;
1384     Zdelta = farClip - nearClip;
1385     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1386                 return;
1387     }
1388     Mat4One(matrix);
1389     matrix[0][0] = 2.0f/Xdelta;
1390     matrix[3][0] = -(right + left)/Xdelta;
1391     matrix[1][1] = 2.0f/Ydelta;
1392     matrix[3][1] = -(top + bottom)/Ydelta;
1393     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1394     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1395 }
1396
1397 void i_window(
1398         float left, float right,
1399         float bottom, float top,
1400         float nearClip, float farClip,
1401         float mat[][4]
1402 ){
1403         float Xdelta, Ydelta, Zdelta;
1404
1405         Xdelta = right - left;
1406         Ydelta = top - bottom;
1407         Zdelta = farClip - nearClip;
1408
1409         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1410                 return;
1411         }
1412         mat[0][0] = nearClip * 2.0f/Xdelta;
1413         mat[1][1] = nearClip * 2.0f/Ydelta;
1414         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1415         mat[2][1] = (top + bottom)/Ydelta;
1416         mat[2][2] = -(farClip + nearClip)/Zdelta;
1417         mat[2][3] = -1.0f;
1418         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1419         mat[0][1] = mat[0][2] = mat[0][3] =
1420             mat[1][0] = mat[1][2] = mat[1][3] =
1421             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1422
1423 }
1424
1425 void i_translate(float Tx, float Ty, float Tz, float mat[][4])
1426 {
1427     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1428     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1429     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1430 }
1431
1432 void i_multmatrix(const float icand[][4], float Vm[][4])
1433 {
1434     int row, col;
1435     float temp[4][4];
1436
1437     for(row=0 ; row<4 ; row++) 
1438         for(col=0 ; col<4 ; col++)
1439             temp[row][col] = icand[row][0] * Vm[0][col]
1440                            + icand[row][1] * Vm[1][col]
1441                            + icand[row][2] * Vm[2][col]
1442                            + icand[row][3] * Vm[3][col];
1443         Mat4CpyMat4(Vm, temp);
1444 }
1445
1446 void i_rotate(float angle, char axis, float mat[][4])
1447 {
1448         int col;
1449     float temp[4];
1450     float cosine, sine;
1451
1452     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
1453         temp[col] = 0;
1454
1455     angle = (float)(angle*(3.1415926535/180.0));
1456     cosine = (float)cos(angle);
1457     sine = (float)sin(angle);
1458     switch(axis){
1459     case 'x':    
1460     case 'X':    
1461         for(col=0 ; col<4 ; col++)
1462             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1463         for(col=0 ; col<4 ; col++) {
1464             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1465             mat[1][col] = temp[col];
1466         }
1467         break;
1468
1469     case 'y':
1470     case 'Y':
1471         for(col=0 ; col<4 ; col++)
1472             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1473         for(col=0 ; col<4 ; col++) {
1474             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1475             mat[0][col] = temp[col];
1476         }
1477         break;
1478
1479     case 'z':
1480     case 'Z':
1481         for(col=0 ; col<4 ; col++)
1482             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
1483         for(col=0 ; col<4 ; col++) {
1484             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
1485             mat[0][col] = temp[col];
1486         }
1487         break;
1488     }
1489 }
1490
1491 void i_polarview(float dist, float azimuth, float incidence, float twist, float Vm[][4])
1492 {
1493
1494         Mat4One(Vm);
1495
1496     i_translate(0.0, 0.0, -dist, Vm);
1497     i_rotate(-twist,'z', Vm);   
1498     i_rotate(-incidence,'x', Vm);
1499     i_rotate(-azimuth,'z', Vm);
1500 }
1501
1502 void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float twist, float mat[][4])
1503 {
1504         float sine, cosine, hyp, hyp1, dx, dy, dz;
1505         float mat1[4][4];
1506         
1507         Mat4One(mat);
1508         Mat4One(mat1);
1509
1510         i_rotate(-twist,'z', mat);
1511
1512         dx = px - vx;
1513         dy = py - vy;
1514         dz = pz - vz;
1515         hyp = dx * dx + dz * dz;        /* hyp squared  */
1516         hyp1 = (float)sqrt(dy*dy + hyp);
1517         hyp = (float)sqrt(hyp);         /* the real hyp */
1518         
1519         if (hyp1 != 0.0) {              /* rotate X     */
1520                 sine = -dy / hyp1;
1521                 cosine = hyp /hyp1;
1522         } else {
1523                 sine = 0;
1524                 cosine = 1.0f;
1525         }
1526         mat1[1][1] = cosine;
1527         mat1[1][2] = sine;
1528         mat1[2][1] = -sine;
1529         mat1[2][2] = cosine;
1530         
1531         i_multmatrix(mat1, mat);
1532
1533     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
1534     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
1535         
1536         /* paragraph    */
1537         if (hyp != 0.0f) {                      /* rotate Y     */
1538                 sine = dx / hyp;
1539                 cosine = -dz / hyp;
1540         } else {
1541                 sine = 0;
1542                 cosine = 1.0f;
1543         }
1544         mat1[0][0] = cosine;
1545         mat1[0][2] = -sine;
1546         mat1[2][0] = sine;
1547         mat1[2][2] = cosine;
1548         
1549         i_multmatrix(mat1, mat);
1550         i_translate(-vx,-vy,-vz, mat);  /* translate viewpoint to origin */
1551 }
1552
1553
1554
1555
1556
1557 /* ************************************************  */
1558
1559 void Mat3Ortho(float mat[][3])
1560 {       
1561         Normalise(mat[0]);
1562         Normalise(mat[1]);
1563         Normalise(mat[2]);
1564 }
1565
1566 void Mat4Ortho(float mat[][4])
1567 {
1568         float len;
1569         
1570         len= Normalise(mat[0]);
1571         if(len!=0.0) mat[0][3]/= len;
1572         len= Normalise(mat[1]);
1573         if(len!=0.0) mat[1][3]/= len;
1574         len= Normalise(mat[2]);
1575         if(len!=0.0) mat[2][3]/= len;
1576 }
1577
1578 void VecCopyf(float *v1, const float *v2)
1579 {
1580
1581         v1[0]= v2[0];
1582         v1[1]= v2[1];
1583         v1[2]= v2[2];
1584 }
1585
1586 int VecLen(const int *v1, const int *v2)
1587 {
1588         float x,y,z;
1589
1590         x=(float)(v1[0]-v2[0]);
1591         y=(float)(v1[1]-v2[1]);
1592         z=(float)(v1[2]-v2[2]);
1593         return (int)floor(sqrt(x*x+y*y+z*z));
1594 }
1595
1596 float VecLenf(const float *v1, const float *v2)
1597 {
1598         float x,y,z;
1599
1600         x=v1[0]-v2[0];
1601         y=v1[1]-v2[1];
1602         z=v1[2]-v2[2];
1603         return (float)sqrt(x*x+y*y+z*z);
1604 }
1605
1606 void VecAddf(float *v, const float *v1, const float *v2)
1607 {
1608         v[0]= v1[0]+ v2[0];
1609         v[1]= v1[1]+ v2[1];
1610         v[2]= v1[2]+ v2[2];
1611 }
1612
1613 void VecSubf(float *v, const float *v1, const float *v2)
1614 {
1615         v[0]= v1[0]- v2[0];
1616         v[1]= v1[1]- v2[1];
1617         v[2]= v1[2]- v2[2];
1618 }
1619
1620 void VecMidf(float *v, const float *v1, const float *v2)
1621 {
1622         v[0]= 0.5f*(v1[0]+ v2[0]);
1623         v[1]= 0.5f*(v1[1]+ v2[1]);
1624         v[2]= 0.5f*(v1[2]+ v2[2]);
1625 }
1626
1627 void VecMulf(float *v1, float f)
1628 {
1629
1630         v1[0]*= f;
1631         v1[1]*= f;
1632         v1[2]*= f;
1633 }
1634
1635 int VecCompare(const float *v1, const float *v2, float limit)
1636 {
1637         if( fabs(v1[0]-v2[0])<limit )
1638                 if( fabs(v1[1]-v2[1])<limit )
1639                         if( fabs(v1[2]-v2[2])<limit ) return 1;
1640         return 0;
1641 }
1642
1643 void CalcNormShort(const short *v1, const short *v2, const short *v3, float *n) /* is ook uitprodukt */
1644 {
1645         float n1[3],n2[3];
1646
1647         n1[0]= (float)(v1[0]-v2[0]);
1648         n2[0]= (float)(v2[0]-v3[0]);
1649         n1[1]= (float)(v1[1]-v2[1]);
1650         n2[1]= (float)(v2[1]-v3[1]);
1651         n1[2]= (float)(v1[2]-v2[2]);
1652         n2[2]= (float)(v2[2]-v3[2]);
1653         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1654         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1655         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1656         Normalise(n);
1657 }
1658
1659 void CalcNormLong(const int* v1, const int*v2, const int*v3, float *n)
1660 {
1661         float n1[3],n2[3];
1662
1663         n1[0]= (float)(v1[0]-v2[0]);
1664         n2[0]= (float)(v2[0]-v3[0]);
1665         n1[1]= (float)(v1[1]-v2[1]);
1666         n2[1]= (float)(v2[1]-v3[1]);
1667         n1[2]= (float)(v1[2]-v2[2]);
1668         n2[2]= (float)(v2[2]-v3[2]);
1669         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1670         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1671         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1672         Normalise(n);
1673 }
1674
1675 float CalcNormFloat(const float *v1, const float *v2, const float *v3, float *n)
1676 {
1677         float n1[3],n2[3];
1678
1679         n1[0]= v1[0]-v2[0];
1680         n2[0]= v2[0]-v3[0];
1681         n1[1]= v1[1]-v2[1];
1682         n2[1]= v2[1]-v3[1];
1683         n1[2]= v1[2]-v2[2];
1684         n2[2]= v2[2]-v3[2];
1685         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1686         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1687         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1688         return Normalise(n);
1689 }
1690
1691 float CalcNormFloat4(const float *v1, const float *v2, const float *v3, const float *v4, float *n)
1692 {
1693         /* real cross! */
1694         float n1[3],n2[3];
1695
1696         n1[0]= v1[0]-v3[0];
1697         n1[1]= v1[1]-v3[1];
1698         n1[2]= v1[2]-v3[2];
1699
1700         n2[0]= v2[0]-v4[0];
1701         n2[1]= v2[1]-v4[1];
1702         n2[2]= v2[2]-v4[2];
1703
1704         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1705         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1706         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1707
1708         return Normalise(n);
1709 }
1710
1711
1712 void CalcCent3f(float *cent, const float *v1, const float *v2, const float *v3)
1713 {
1714
1715         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
1716         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
1717         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
1718 }
1719
1720 void CalcCent4f(float *cent, const float *v1, const float *v2, const float *v3, const float *v4)
1721 {
1722
1723         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
1724         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
1725         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
1726 }
1727
1728 float Sqrt3f(float f)
1729 {
1730         if(f==0.0) return 0;
1731         if(f<0) return (float)(-exp(log(-f)/3));
1732         else return (float)(exp(log(f)/3));
1733 }
1734
1735 double Sqrt3d(double d)
1736 {
1737         if(d==0.0) return 0;
1738         if(d<0) return -exp(log(-d)/3);
1739         else return exp(log(d)/3);
1740 }
1741                                                          /* afstand v1 tot lijn v2-v3 */
1742 float DistVL2Dfl(const float *v1,const float *v2,const float *v3)   /* met formule van Hesse :GEEN LIJNSTUK! */
1743 {
1744         float a[2],deler;
1745
1746         a[0]= v2[1]-v3[1];
1747         a[1]= v3[0]-v2[0];
1748         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
1749         if(deler== 0.0f) return 0;
1750
1751         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
1752
1753 }
1754
1755 float PdistVL2Dfl(const float *v1,const float *v2,const float *v3)  /* PointDist: WEL LIJNSTUK */
1756 {
1757         float labda, rc[2], pt[2], len;
1758         
1759         rc[0]= v3[0]-v2[0];
1760         rc[1]= v3[1]-v2[1];
1761         len= rc[0]*rc[0]+ rc[1]*rc[1];
1762         if(len==0.0) {
1763                 rc[0]= v1[0]-v2[0];
1764                 rc[1]= v1[1]-v2[1];
1765                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
1766         }
1767         
1768         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
1769         if(labda<=0.0) {
1770                 pt[0]= v2[0];
1771                 pt[1]= v2[1];
1772         }
1773         else if(labda>=1.0) {
1774                 pt[0]= v3[0];
1775                 pt[1]= v3[1];
1776         }
1777         else {
1778                 pt[0]= labda*rc[0]+v2[0];
1779                 pt[1]= labda*rc[1]+v2[1];
1780         }
1781
1782         rc[0]= pt[0]-v1[0];
1783         rc[1]= pt[1]-v1[1];
1784         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
1785 }
1786
1787 float AreaF2Dfl(const float *v1,const float *v2,const float *v3)
1788 {
1789         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
1790 }
1791
1792
1793 float AreaQ3Dfl(const float *v1,const float *v2,const float *v3, const float *v4)  /* only convex Quadrilaterals */
1794 {
1795         float len, vec1[3], vec2[3], n[3];
1796
1797         VecSubf(vec1, v2, v1);
1798         VecSubf(vec2, v4, v1);
1799         Crossf(n, vec1, vec2);
1800         len= Normalise(n);
1801
1802         VecSubf(vec1, v4, v3);
1803         VecSubf(vec2, v2, v3);
1804         Crossf(n, vec1, vec2);
1805         len+= Normalise(n);
1806
1807         return (len/2.0f);
1808 }
1809
1810 float AreaT3Dfl(const float *v1,const float *v2,const float *v3)  /* Triangles */
1811 {
1812         float len, vec1[3], vec2[3], n[3];
1813
1814         VecSubf(vec1, v3, v2);
1815         VecSubf(vec2, v1, v2);
1816         Crossf(n, vec1, vec2);
1817         len= Normalise(n);
1818
1819         return (len/2.0f);
1820 }
1821
1822 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
1823 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
1824
1825
1826 float AreaPoly3Dfl(int nr, const float *verts, const float *normal)
1827 {
1828         float x, y, z, area, max;
1829         const float *cur, *prev;
1830         int a, px=0, py=1;
1831
1832         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
1833         x= (float)fabs(normal[0]);
1834         y= (float)fabs(normal[1]);
1835         z= (float)fabs(normal[2]);
1836         max = MAX3(x, y, z);
1837         if(max==y) py=2;
1838         else if(max==x) {
1839                 px=1; 
1840                 py= 2;
1841         }
1842
1843         /* The Trapezium Area Rule */
1844         prev= verts+3*(nr-1);
1845         cur= verts;
1846         area= 0;
1847         for(a=0; a<nr; a++) {
1848                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
1849                 prev= cur;
1850                 cur+=3;
1851         }
1852
1853         return (float)fabs(0.5*area/max);
1854 }
1855
1856 void MinMax3(float *min, float *max, const float *vec)
1857 {
1858         if(min[0]>vec[0]) min[0]= vec[0];
1859         if(min[1]>vec[1]) min[1]= vec[1];
1860         if(min[2]>vec[2]) min[2]= vec[2];
1861
1862         if(max[0]<vec[0]) max[0]= vec[0];
1863         if(max[1]<vec[1]) max[1]= vec[1];
1864         if(max[2]<vec[2]) max[2]= vec[2];
1865 }
1866
1867 /* ************ EULER *************** */
1868
1869 void EulToMat3(const float *eul, float mat[][3])
1870 {
1871         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1872         
1873         ci = cos(eul[0]); 
1874         cj = cos(eul[1]); 
1875         ch = cos(eul[2]);
1876         si = sin(eul[0]); 
1877         sj = sin(eul[1]); 
1878         sh = sin(eul[2]);
1879         cc = ci*ch; 
1880         cs = ci*sh; 
1881         sc = si*ch; 
1882         ss = si*sh;
1883
1884         mat[0][0] = (float)(cj*ch); 
1885         mat[1][0] = (float)(sj*sc-cs); 
1886         mat[2][0] = (float)(sj*cc+ss);
1887         mat[0][1] = (float)(cj*sh); 
1888         mat[1][1] = (float)(sj*ss+cc); 
1889         mat[2][1] = (float)(sj*cs-sc);
1890         mat[0][2] = (float)-sj;  
1891         mat[1][2] = (float)(cj*si);    
1892         mat[2][2] = (float)(cj*ci);
1893
1894 }
1895
1896 void EulToMat4(const float *eul,float mat[][4])
1897 {
1898         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1899         
1900         ci = cos(eul[0]); 
1901         cj = cos(eul[1]); 
1902         ch = cos(eul[2]);
1903         si = sin(eul[0]); 
1904         sj = sin(eul[1]); 
1905         sh = sin(eul[2]);
1906         cc = ci*ch; 
1907         cs = ci*sh; 
1908         sc = si*ch; 
1909         ss = si*sh;
1910
1911         mat[0][0] = (float)(cj*ch); 
1912         mat[1][0] = (float)(sj*sc-cs); 
1913         mat[2][0] = (float)(sj*cc+ss);
1914         mat[0][1] = (float)(cj*sh); 
1915         mat[1][1] = (float)(sj*ss+cc); 
1916         mat[2][1] = (float)(sj*cs-sc);
1917         mat[0][2] = (float)-sj;  
1918         mat[1][2] = (float)(cj*si);    
1919         mat[2][2] = (float)(cj*ci);
1920
1921
1922         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
1923         mat[3][3]= 1.0f;
1924 }
1925
1926
1927 void Mat3ToEul(
1928         const float tmat[][3], float *eul
1929 ){
1930         float cy, quat[4], mat[3][3];
1931         
1932         Mat3ToQuat(tmat, quat);
1933         QuatToMat3(quat, mat);
1934         Mat3CpyMat3(mat, tmat);
1935         Mat3Ortho(mat);
1936         
1937         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
1938
1939         if (cy > 16.0*FLT_EPSILON) {
1940                 eul[0] = (float)atan2(mat[1][2], mat[2][2]);
1941                 eul[1] = (float)atan2(-mat[0][2], cy);
1942                 eul[2] = (float)atan2(mat[0][1], mat[0][0]);
1943         } else {
1944                 eul[0] = (float)atan2(-mat[2][1], mat[1][1]);
1945                 eul[1] = (float)atan2(-mat[0][2], cy);
1946                 eul[2] = 0.0f;
1947         }
1948 }
1949
1950 void Mat3ToEuln(const float tmat[][3], float *eul)
1951 {
1952         float sin1, cos1, sin2, cos2, sin3, cos3;
1953         
1954         sin1 = -tmat[2][0];
1955         cos1 = (float)sqrt(1 - sin1*sin1);
1956
1957         if ( fabs(cos1) > FLT_EPSILON ) {
1958                 sin2 = tmat[2][1] / cos1;
1959                 cos2 = tmat[2][2] / cos1;
1960                 sin3 = tmat[1][0] / cos1;
1961                 cos3 = tmat[0][0] / cos1;
1962     } 
1963         else {
1964                 sin2 = -tmat[1][2];
1965                 cos2 = tmat[1][1];
1966                 sin3 = 0.0;
1967                 cos3 = 1.0;
1968     }
1969         
1970         eul[0] = (float)atan2(sin3, cos3);
1971         eul[1] = (float)atan2(sin1, cos1);
1972         eul[2] = (float)atan2(sin2, cos2);
1973
1974
1975
1976
1977 void QuatToEul(const float *quat, float *eul)
1978 {
1979         float mat[3][3];
1980         
1981         QuatToMat3(quat, mat);
1982         Mat3ToEul(mat, eul);
1983 }
1984
1985 void QuatToSpher(const float *quat, float *sph)
1986 /* Not working 100% yet I don't think... */
1987 {
1988         float tx, ty, tz;
1989         float qw, qx, qy, qz;
1990         float cos_theta;
1991         float sin_theta;
1992         
1993         qx = quat[0];
1994         qy = quat[1];
1995         qz = quat[2];
1996         qw = quat[3];
1997
1998         cos_theta = qw;
1999         sin_theta = (float)sqrt(1.0 - cos_theta * cos_theta);
2000
2001         if (fabs(sin_theta) < 0.0005)
2002                 sin_theta = 1.0;
2003
2004         tx = qx / sin_theta;
2005         ty = qy / sin_theta;
2006         tz = qz / sin_theta;
2007
2008         /* Lattitude */
2009         sph[0] = -(float)asin(ty);
2010
2011         /* Longitude */
2012         if (tx*tx + tz*tz <0.0005)
2013                 sph[1] = 0.0;
2014         else
2015                 sph[1] = (float)atan2(tx, tz);
2016
2017         if (sph[1] < 0.0)
2018                 sph[1] +=(float)(M_PI*2);
2019
2020         /* Roll */
2021         sph[2] = (float)(acos(cos_theta) * 2.0) ;
2022 }
2023
2024 void Mat3ToSpher (const float *mat3, float *sph)
2025 {
2026         float quat[4];
2027
2028         Mat3ToQuat(mat3, quat);
2029         QuatToSpher(quat, sph);
2030 }
2031
2032
2033 void EulToQuat(const float *eul, float *quat)
2034 {
2035     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2036  
2037     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2038     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2039     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2040     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2041         
2042         quat[0] = cj*cc + sj*ss;
2043         quat[1] = cj*sc - sj*cs;
2044         quat[2] = cj*ss + sj*cc;
2045         quat[3] = cj*cs - sj*sc;
2046 }
2047
2048 void VecRotToMat3(const float *vec, float phi, float mat[][3])
2049 {
2050         /* rotation of phi radials around vec */
2051         float vx, vx2, vy, vy2, vz, vz2, co, si;
2052         
2053         vx= vec[0];
2054         vy= vec[1];
2055         vz= vec[2];
2056         vx2= vx*vx;
2057         vy2= vy*vy;
2058         vz2= vz*vz;
2059         co= (float)cos(phi);
2060         si= (float)sin(phi);
2061         
2062         mat[0][0]= vx2+co*(1.0f-vx2);
2063         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2064         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2065         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2066         mat[1][1]= vy2+co*(1.0f-vy2);
2067         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2068         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2069         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2070         mat[2][2]= vz2+co*(1.0f-vz2);
2071         
2072 }
2073
2074 void VecRotToQuat(const float *vec, float phi, float *quat)
2075 {
2076         /* rotation of phi radials around vec */
2077         float si;
2078
2079         quat[1]= vec[0];
2080         quat[2]= vec[1];
2081         quat[3]= vec[2];
2082                                                                                                            
2083         if( Normalise(quat+1) == 0.0) {
2084                 QuatOne(quat);
2085         }
2086         else {
2087                 quat[0]= (float)cos( phi/2.0 );
2088                 si= (float)sin( phi/2.0 );
2089                 quat[1] *= si;
2090                 quat[2] *= si;
2091                 quat[3] *= si;
2092         }
2093 }
2094
2095 void euler_rot(float *beul, float ang, char axis)
2096 {
2097         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2098         
2099         eul[0]= eul[1]= eul[2]= 0.0;
2100         if(axis=='x') eul[0]= ang;
2101         else if(axis=='y') eul[1]= ang;
2102         else eul[2]= ang;
2103         
2104         EulToMat3(eul, mat1);
2105         EulToMat3(beul, mat2);
2106         
2107         Mat3MulMat3(totmat, mat2, mat1);
2108         
2109         Mat3ToEul(totmat, beul);
2110         
2111 }
2112
2113
2114
2115 void SizeToMat3(const float *size, float mat[][3])
2116 {
2117         mat[0][0]= size[0];
2118         mat[0][1]= 0.0;
2119         mat[0][2]= 0.0;
2120         mat[1][1]= size[1];
2121         mat[1][0]= 0.0;
2122         mat[1][2]= 0.0;
2123         mat[2][2]= size[2];
2124         mat[2][1]= 0.0;
2125         mat[2][0]= 0.0;
2126 }
2127
2128 void Mat3ToSize(const float mat[][3], float *size)
2129 {
2130         float vec[3];
2131
2132
2133         VecCopyf(vec, mat[0]);
2134         size[0]= Normalise(vec);
2135         VecCopyf(vec, mat[1]);
2136         size[1]= Normalise(vec);
2137         VecCopyf(vec, mat[2]);
2138         size[2]= Normalise(vec);
2139
2140 }
2141
2142 void Mat4ToSize(const float mat[][4], float *size)
2143 {
2144         float vec[3];
2145         
2146
2147         VecCopyf(vec, mat[0]);
2148         size[0]= Normalise(vec);
2149         VecCopyf(vec, mat[1]);
2150         size[1]= Normalise(vec);
2151         VecCopyf(vec, mat[2]);
2152         size[2]= Normalise(vec);
2153 }
2154
2155 /* ************* SPECIALS ******************* */
2156
2157 void triatoquat(const float *v1, const float *v2, const float *v3, float *quat)
2158 {
2159         /* denkbeeldige x-as, y-as driehoek wordt geroteerd */
2160         float vec[3], q1[4], q2[4], n[3], si, co, hoek, mat[3][3], imat[3][3];
2161         
2162         /* eerst z-as op vlaknormaal */
2163         CalcNormFloat(v1, v2, v3, vec);
2164
2165         n[0]= vec[1];
2166         n[1]= -vec[0];
2167         n[2]= 0.0;
2168         Normalise(n);
2169         
2170         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
2171         
2172         hoek= -0.5f*saacos(vec[2]);
2173         co= (float)cos(hoek);
2174         si= (float)sin(hoek);
2175         q1[0]= co;
2176         q1[1]= n[0]*si;
2177         q1[2]= n[1]*si;
2178         q1[3]= 0.0f;
2179         
2180         /* v1-v2 lijn terug roteren */
2181         QuatToMat3(q1, mat);
2182         Mat3Inv(imat, mat);
2183         VecSubf(vec, v2, v1);
2184         Mat3MulVecfl(imat, vec);
2185
2186         /* welke hoek maakt deze lijn met x-as? */
2187         vec[2]= 0.0;
2188         Normalise(vec);
2189
2190         hoek= (float)(0.5*atan2(vec[1], vec[0]));
2191         co= (float)cos(hoek);
2192         si= (float)sin(hoek);
2193         q2[0]= co;
2194         q2[1]= 0.0f;
2195         q2[2]= 0.0f;
2196         q2[3]= si;
2197         
2198         QuatMul(quat, q1, q2);
2199 }
2200
2201 void MinMaxRGB(short c[])
2202 {
2203         if(c[0]>255) c[0]=255;
2204         else if(c[0]<0) c[0]=0;
2205         if(c[1]>255) c[1]=255;
2206         else if(c[1]<0) c[1]=0;
2207         if(c[2]>255) c[2]=255;
2208         else if(c[2]<0) c[2]=0;
2209 }
2210
2211 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
2212 {
2213         int i;
2214         float f, p, q, t;
2215
2216         h *= 360.0f;
2217         
2218         if(s==0 && 0) {
2219                 *r = v;
2220                 *g = v;
2221                 *b = v;
2222         }
2223         else {
2224                 if(h==360) h = 0;
2225                 
2226                 h /= 60;
2227                 i = (int)floor(h);
2228                 f = h - i;
2229                 p = v*(1.0f-s);
2230                 q = v*(1.0f-(s*f));
2231                 t = v*(1.0f-(s*(1.0f-f)));
2232                 
2233                 switch (i) {
2234                 case 0 :
2235                         *r = v;
2236                         *g = t;
2237                         *b = p;
2238                         break;
2239                 case 1 :
2240                         *r = q;
2241                         *g = v;
2242                         *b = p;
2243                         break;
2244                 case 2 :
2245                         *r = p;
2246                         *g = v;
2247                         *b = t;
2248                         break;
2249                 case 3 :
2250                         *r = p;
2251                         *g = q;
2252                         *b = v;
2253                         break;
2254                 case 4 :
2255                         *r = t;
2256                         *g = p;
2257                         *b = v;
2258                         break;
2259                 case 5 :
2260                         *r = v;
2261                         *g = p;
2262                         *b = q;
2263                         break;
2264                 }
2265         }
2266 }
2267
2268 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
2269 {
2270         float h, s, v;
2271         float cmax, cmin, cdelta;
2272         float rc, gc, bc;
2273
2274         cmax = r;
2275         cmin = r;
2276         cmax = (g>cmax ? g:cmax);
2277         cmin = (g<cmin ? g:cmin);
2278         cmax = (b>cmax ? b:cmax);
2279         cmin = (b<cmin ? b:cmin);
2280
2281         v = cmax;               /* value */
2282         if (cmax!=0.0)
2283                 s = (cmax - cmin)/cmax;
2284         else {
2285                 s = 0.0;
2286                 h = 0.0;
2287         }
2288         if (s == 0.0)
2289                 h = -1.0;
2290         else {
2291                 cdelta = cmax-cmin;
2292                 rc = (cmax-r)/cdelta;
2293                 gc = (cmax-g)/cdelta;
2294                 bc = (cmax-b)/cdelta;
2295                 if (r==cmax)
2296                         h = bc-gc;
2297                 else
2298                         if (g==cmax)
2299                                 h = 2.0f+rc-bc;
2300                         else
2301                                 h = 4.0f+gc-rc;
2302                 h = h*60.0f;
2303                 if (h<0.0f)
2304                         h += 360.0f;
2305         }
2306         
2307         *ls = s;
2308         *lh = h/360.0f;
2309         if( *lh < 0.0) *lh= 0.0;
2310         *lv = v;
2311 }
2312
2313 /* Bij afspraak is cpack een getal dat als 0xFFaa66 of zo kan worden uitgedrukt. Is dus gevoelig voor endian. 
2314  * Met deze define wordt het altijd goed afgebeeld
2315  */
2316
2317 unsigned int hsv_to_cpack(float h, float s, float v)
2318 {
2319         short r, g, b;
2320         float rf, gf, bf;
2321         unsigned int col;
2322         
2323         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
2324         
2325         r= (short)(rf*255.0f);
2326         g= (short)(gf*255.0f);
2327         b= (short)(bf*255.0f);
2328         
2329         col= ( r + (g*256) + (b*256*256) );
2330         return col;
2331 }
2332
2333
2334 unsigned int rgb_to_cpack(float r, float g, float b)
2335 {
2336         int ir, ig, ib;
2337         
2338         ir= (int)floor(255.0*r);
2339         if(ir<0) ir= 0; else if(ir>255) ir= 255;
2340         ig= (int)floor(255.0*g);
2341         if(ig<0) ig= 0; else if(ig>255) ig= 255;
2342         ib= (int)floor(255.0*b);
2343         if(ib<0) ib= 0; else if(ib>255) ib= 255;
2344         
2345         return (ir+ (ig*256) + (ib*256*256));
2346 }
2347
2348 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
2349 {
2350         
2351         *r= (float)((col)&0xFF);
2352         *r /= 255.0f;
2353
2354         *g= (float)(((col)>>8)&0xFF);
2355         *g /= 255.0f;
2356
2357         *b= (float)(((col)>>16)&0xFF);
2358         *b /= 255.0f;
2359 }