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