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