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