soc-2008-mxcurioni: merged changes to revision 14798, compilation works for rendering...
[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  * $Id$
8  *
9  * ***** BEGIN GPL LICENSE BLOCK *****
10  *
11  * This program is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU General Public License
13  * as published by the Free Software Foundation; either version 2
14  * of the License, or (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
24  *
25  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
26  * All rights reserved.
27  *
28  * The Original Code is: all of this file.
29  *
30  * Contributor(s): none yet.
31  *
32  * ***** END GPL LICENSE BLOCK *****
33  */
34
35 /* ************************ FUNKTIES **************************** */
36
37 #include <math.h>
38 #include <sys/types.h>
39 #include <string.h> 
40 #include <float.h>
41
42 #ifdef HAVE_CONFIG_H
43 #include <config.h>
44 #endif
45
46 #if defined(__sun__) || defined( __sun ) || defined (__sparc) || defined (__sparc__)
47 #include <strings.h>
48 #endif
49
50 #if !defined(__sgi) && !defined(WIN32)
51 #include <sys/time.h>
52 #include <unistd.h>
53 #endif
54
55 #include <stdio.h>
56 #include "BLI_arithb.h"
57 #include "BLI_memarena.h"
58
59 /* A few small defines. Keep'em local! */
60 #define SMALL_NUMBER    1.e-8
61 #define ABS(x)  ((x) < 0 ? -(x) : (x))
62 #define SWAP(type, a, b)        { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
63
64
65 #if defined(WIN32) || defined(__APPLE__)
66 #include <stdlib.h>
67 #define M_PI 3.14159265358979323846
68 #define M_SQRT2 1.41421356237309504880   
69
70 #endif /* defined(WIN32) || defined(__APPLE__) */
71
72
73 float saacos(float fac)
74 {
75         if(fac<= -1.0f) return (float)M_PI;
76         else if(fac>=1.0f) return 0.0;
77         else return (float)acos(fac);
78 }
79
80 float saasin(float fac)
81 {
82         if(fac<= -1.0f) return (float)-M_PI/2.0f;
83         else if(fac>=1.0f) return (float)M_PI/2.0f;
84         else return (float)asin(fac);
85 }
86
87 float sasqrt(float fac)
88 {
89         if(fac<=0.0) return 0.0;
90         return (float)sqrt(fac);
91 }
92
93 float Normalize(float *n)
94 {
95         float d;
96         
97         d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
98         /* A larger value causes normalize errors in a scaled down models with camera xtreme close */
99         if(d>1.0e-35F) {
100                 d= (float)sqrt(d);
101
102                 n[0]/=d; 
103                 n[1]/=d; 
104                 n[2]/=d;
105         } else {
106                 n[0]=n[1]=n[2]= 0.0;
107                 d= 0.0;
108         }
109         return d;
110 }
111
112 void Crossf(float *c, float *a, float *b)
113 {
114         c[0] = a[1] * b[2] - a[2] * b[1];
115         c[1] = a[2] * b[0] - a[0] * b[2];
116         c[2] = a[0] * b[1] - a[1] * b[0];
117 }
118
119 /* Inpf returns the dot product, also called the scalar product and inner product */
120 float Inpf( float *v1, float *v2)
121 {
122         return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
123 }
124
125 /* Project v1 on v2 */
126 void Projf(float *c, float *v1, float *v2)
127 {
128         float mul;
129         mul = Inpf(v1, v2) / Inpf(v2, v2);
130         
131         c[0] = mul * v2[0];
132         c[1] = mul * v2[1];
133         c[2] = mul * v2[2];
134 }
135
136 void Mat3Transp(float mat[][3])
137 {
138         float t;
139
140         t = mat[0][1] ; 
141         mat[0][1] = mat[1][0] ; 
142         mat[1][0] = t;
143         t = mat[0][2] ; 
144         mat[0][2] = mat[2][0] ; 
145         mat[2][0] = t;
146         t = mat[1][2] ; 
147         mat[1][2] = mat[2][1] ; 
148         mat[2][1] = t;
149 }
150
151 void Mat4Transp(float mat[][4])
152 {
153         float t;
154
155         t = mat[0][1] ; 
156         mat[0][1] = mat[1][0] ; 
157         mat[1][0] = t;
158         t = mat[0][2] ; 
159         mat[0][2] = mat[2][0] ; 
160         mat[2][0] = t;
161         t = mat[0][3] ; 
162         mat[0][3] = mat[3][0] ; 
163         mat[3][0] = t;
164
165         t = mat[1][2] ; 
166         mat[1][2] = mat[2][1] ; 
167         mat[2][1] = t;
168         t = mat[1][3] ; 
169         mat[1][3] = mat[3][1] ; 
170         mat[3][1] = t;
171
172         t = mat[2][3] ; 
173         mat[2][3] = mat[3][2] ; 
174         mat[3][2] = t;
175 }
176
177
178 /*
179  * invertmat - 
180  *              computes the inverse of mat and puts it in inverse.  Returns 
181  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
182  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
183  *
184  *                                      Mark Segal - 1992
185  */
186
187 int Mat4Invert(float inverse[][4], float mat[][4])
188 {
189         int i, j, k;
190         double temp;
191         float tempmat[4][4];
192         float max;
193         int maxj;
194
195         /* Set inverse to identity */
196         for (i=0; i<4; i++)
197                 for (j=0; j<4; j++)
198                         inverse[i][j] = 0;
199         for (i=0; i<4; i++)
200                 inverse[i][i] = 1;
201
202         /* Copy original matrix so we don't mess it up */
203         for(i = 0; i < 4; i++)
204                 for(j = 0; j <4; j++)
205                         tempmat[i][j] = mat[i][j];
206
207         for(i = 0; i < 4; i++) {
208                 /* Look for row with max pivot */
209                 max = ABS(tempmat[i][i]);
210                 maxj = i;
211                 for(j = i + 1; j < 4; j++) {
212                         if(ABS(tempmat[j][i]) > max) {
213                                 max = ABS(tempmat[j][i]);
214                                 maxj = j;
215                         }
216                 }
217                 /* Swap rows if necessary */
218                 if (maxj != i) {
219                         for( k = 0; k < 4; k++) {
220                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
221                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
222                         }
223                 }
224
225                 temp = tempmat[i][i];
226                 if (temp == 0)
227                         return 0;  /* No non-zero pivot */
228                 for(k = 0; k < 4; k++) {
229                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
230                         inverse[i][k] = (float)(inverse[i][k]/temp);
231                 }
232                 for(j = 0; j < 4; j++) {
233                         if(j != i) {
234                                 temp = tempmat[j][i];
235                                 for(k = 0; k < 4; k++) {
236                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
237                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
238                                 }
239                         }
240                 }
241         }
242         return 1;
243 }
244 #ifdef TEST_ACTIVE
245 void Mat4InvertSimp(float inverse[][4], float mat[][4])
246 {
247         /* only for Matrices that have a rotation */
248         /* based at GG IV pag 205 */
249         float scale;
250         
251         scale= mat[0][0]*mat[0][0] + mat[1][0]*mat[1][0] + mat[2][0]*mat[2][0];
252         if(scale==0.0) return;
253         
254         scale= 1.0/scale;
255         
256         /* transpose and scale */
257         inverse[0][0]= scale*mat[0][0];
258         inverse[1][0]= scale*mat[0][1];
259         inverse[2][0]= scale*mat[0][2];
260         inverse[0][1]= scale*mat[1][0];
261         inverse[1][1]= scale*mat[1][1];
262         inverse[2][1]= scale*mat[1][2];
263         inverse[0][2]= scale*mat[2][0];
264         inverse[1][2]= scale*mat[2][1];
265         inverse[2][2]= scale*mat[2][2];
266
267         inverse[3][0]= -(inverse[0][0]*mat[3][0] + inverse[1][0]*mat[3][1] + inverse[2][0]*mat[3][2]);
268         inverse[3][1]= -(inverse[0][1]*mat[3][0] + inverse[1][1]*mat[3][1] + inverse[2][1]*mat[3][2]);
269         inverse[3][2]= -(inverse[0][2]*mat[3][0] + inverse[1][2]*mat[3][1] + inverse[2][2]*mat[3][2]);
270         
271         inverse[0][3]= inverse[1][3]= inverse[2][3]= 0.0;
272         inverse[3][3]= 1.0;
273 }
274 #endif
275 /*  struct Matrix4; */
276
277 #ifdef TEST_ACTIVE
278 /* this seems to be unused.. */
279
280 void Mat4Inv(float *m1, float *m2)
281 {
282
283 /* This gets me into trouble:  */
284         float mat1[3][3], mat2[3][3]; 
285         
286 /*      void Mat3Inv(); */
287 /*      void Mat3CpyMat4(); */
288 /*      void Mat4CpyMat3(); */
289
290         Mat3CpyMat4((float*)mat2,m2);
291         Mat3Inv((float*)mat1, (float*) mat2);
292         Mat4CpyMat3(m1, mat1);
293
294 }
295 #endif
296
297
298 float Det2x2(float a,float b,float c,float d)
299 {
300
301         return a*d - b*c;
302 }
303
304
305
306 float Det3x3(float a1, float a2, float a3,
307                          float b1, float b2, float b3,
308                          float c1, float c2, float c3 )
309 {
310         float ans;
311
312         ans = a1 * Det2x2( b2, b3, c2, c3 )
313             - b1 * Det2x2( a2, a3, c2, c3 )
314             + c1 * Det2x2( a2, a3, b2, b3 );
315
316         return ans;
317 }
318
319 float Det4x4(float m[][4])
320 {
321         float ans;
322         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
323
324         a1= m[0][0]; 
325         b1= m[0][1];
326         c1= m[0][2]; 
327         d1= m[0][3];
328
329         a2= m[1][0]; 
330         b2= m[1][1];
331         c2= m[1][2]; 
332         d2= m[1][3];
333
334         a3= m[2][0]; 
335         b3= m[2][1];
336         c3= m[2][2]; 
337         d3= m[2][3];
338
339         a4= m[3][0]; 
340         b4= m[3][1];
341         c4= m[3][2]; 
342         d4= m[3][3];
343
344         ans = a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
345             - b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
346             + c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
347             - d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
348
349         return ans;
350 }
351
352
353 void Mat4Adj(float out[][4], float in[][4])     /* out = ADJ(in) */
354 {
355         float a1, a2, a3, a4, b1, b2, b3, b4;
356         float c1, c2, c3, c4, d1, d2, d3, d4;
357
358         a1= in[0][0]; 
359         b1= in[0][1];
360         c1= in[0][2]; 
361         d1= in[0][3];
362
363         a2= in[1][0]; 
364         b2= in[1][1];
365         c2= in[1][2]; 
366         d2= in[1][3];
367
368         a3= in[2][0]; 
369         b3= in[2][1];
370         c3= in[2][2]; 
371         d3= in[2][3];
372
373         a4= in[3][0]; 
374         b4= in[3][1];
375         c4= in[3][2]; 
376         d4= in[3][3];
377
378
379         out[0][0]  =   Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
380         out[1][0]  = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
381         out[2][0]  =   Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
382         out[3][0]  = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
383
384         out[0][1]  = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
385         out[1][1]  =   Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
386         out[2][1]  = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
387         out[3][1]  =   Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
388
389         out[0][2]  =   Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
390         out[1][2]  = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
391         out[2][2]  =   Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
392         out[3][2]  = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
393
394         out[0][3]  = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
395         out[1][3]  =   Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
396         out[2][3]  = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
397         out[3][3]  =   Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
398 }
399
400 void Mat4InvGG(float out[][4], float in[][4])   /* from Graphic Gems I, out= INV(in)  */
401 {
402         int i, j;
403         float det;
404
405         /* calculate the adjoint matrix */
406
407         Mat4Adj(out,in);
408
409         det = Det4x4(out);
410
411         if ( fabs( det ) < SMALL_NUMBER) {
412                 return;
413         }
414
415         /* scale the adjoint matrix to get the inverse */
416
417         for (i=0; i<4; i++)
418                 for(j=0; j<4; j++)
419                         out[i][j] = out[i][j] / det;
420
421         /* the last factor is not always 1. For that reason an extra division should be implemented? */
422 }
423
424
425 void Mat3Inv(float m1[][3], float m2[][3])
426 {
427         short a,b;
428         float det;
429
430         /* calc adjoint */
431         Mat3Adj(m1,m2);
432
433         /* then determinant old matrix! */
434         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
435             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
436             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
437
438         if(det==0) det=1;
439         det= 1/det;
440         for(a=0;a<3;a++) {
441                 for(b=0;b<3;b++) {
442                         m1[a][b]*=det;
443                 }
444         }
445 }
446
447 void Mat3Adj(float m1[][3], float m[][3])
448 {
449         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
450         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
451         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
452
453         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
454         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
455         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
456
457         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
458         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
459         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
460 }
461
462 void Mat4MulMat4(float m1[][4], float m2[][4], float m3[][4])
463 {
464   /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
465
466         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];
467         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];
468         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];
469         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];
470
471         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];
472         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];
473         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];
474         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];
475
476         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];
477         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];
478         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];
479         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];
480
481         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];
482         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];
483         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];
484         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];
485
486 }
487 #ifdef TEST_ACTIVE
488 void subMat4MulMat4(float *m1, float *m2, float *m3)
489 {
490
491         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
492         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
493         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
494         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
495         m1+=4;
496         m2+=4;
497         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
498         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
499         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
500         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
501         m1+=4;
502         m2+=4;
503         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
504         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
505         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
506         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
507 }
508 #endif
509
510 #ifndef TEST_ACTIVE
511 void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3])
512 #else
513 void Mat3MulMat3(float *m1, float *m3, float *m2)
514 #endif
515 {
516    /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
517 #ifndef TEST_ACTIVE
518         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 
519         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 
520         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 
521
522         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 
523         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 
524         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 
525
526         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 
527         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 
528         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
529 #else
530         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
531         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
532         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
533         m1+=3;
534         m2+=3;
535         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
536         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
537         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
538         m1+=3;
539         m2+=3;
540         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
541         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
542         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
543 #endif
544 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
545
546 void Mat4MulMat43(float (*m1)[4], float (*m3)[4], float (*m2)[3])
547 {
548         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
549         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
550         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
551         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
552         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
553         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
554         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
555         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
556         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
557 }
558 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
559 void Mat3IsMat3MulMat4(float m1[][3], float m2[][3], float m3[][4])
560 {
561     /* m1[i][j] = m2[i][k] * m3[k][j] */
562     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
563     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
564     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
565
566     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
567     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
568     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
569
570     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
571     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
572     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
573 }
574
575
576
577 void Mat4MulMat34(float (*m1)[4], float (*m3)[3], float (*m2)[4])
578 {
579         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
580         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
581         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
582         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
583         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
584         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
585         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
586         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
587         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
588 }
589
590 void Mat4CpyMat4(float m1[][4], float m2[][4]) 
591 {
592         memcpy(m1, m2, 4*4*sizeof(float));
593 }
594
595 void Mat4SwapMat4(float *m1, float *m2)
596 {
597         float t;
598         int i;
599
600         for(i=0;i<16;i++) {
601                 t= *m1;
602                 *m1= *m2;
603                 *m2= t;
604                 m1++; 
605                 m2++;
606         }
607 }
608
609 typedef float Mat3Row[3];
610 typedef float Mat4Row[4];
611
612 #ifdef TEST_ACTIVE
613 void Mat3CpyMat4(float *m1p, float *m2p)
614 #else
615 void Mat3CpyMat4(float m1[][3], float m2[][4])
616 #endif
617 {
618 #ifdef TEST_ACTIVE
619         int i, j;
620         Mat3Row *m1= (Mat3Row *)m1p; 
621         Mat4Row *m2= (Mat4Row *)m2p; 
622         for ( i = 0; i++; i < 3) {
623                 for (j = 0; j++; j < 3) {
624                         m1p[3*i + j] = m2p[4*i + j];
625                 }
626         }                       
627 #endif
628         m1[0][0]= m2[0][0];
629         m1[0][1]= m2[0][1];
630         m1[0][2]= m2[0][2];
631
632         m1[1][0]= m2[1][0];
633         m1[1][1]= m2[1][1];
634         m1[1][2]= m2[1][2];
635
636         m1[2][0]= m2[2][0];
637         m1[2][1]= m2[2][1];
638         m1[2][2]= m2[2][2];
639 }
640
641 /* Butched. See .h for comment */
642 /*  void Mat4CpyMat3(float m1[][4], float m2[][3]) */
643 #ifdef TEST_ACTIVE
644 void Mat4CpyMat3(float* m1, float *m2)
645 {
646         int i;
647         for (i = 0; i < 3; i++) {
648                 m1[(4*i)]    = m2[(3*i)];
649                 m1[(4*i) + 1]= m2[(3*i) + 1];
650                 m1[(4*i) + 2]= m2[(3*i) + 2];
651                 m1[(4*i) + 3]= 0.0;
652                 i++;
653         }
654
655         m1[12]=m1[13]= m1[14]= 0.0;
656         m1[15]= 1.0;
657 }
658 #else
659
660 void Mat4CpyMat3(float m1[][4], float m2[][3])  /* no clear */
661 {
662         m1[0][0]= m2[0][0];
663         m1[0][1]= m2[0][1];
664         m1[0][2]= m2[0][2];
665
666         m1[1][0]= m2[1][0];
667         m1[1][1]= m2[1][1];
668         m1[1][2]= m2[1][2];
669
670         m1[2][0]= m2[2][0];
671         m1[2][1]= m2[2][1];
672         m1[2][2]= m2[2][2];
673
674         /*      Reevan's Bugfix */
675         m1[0][3]=0.0F;
676         m1[1][3]=0.0F;
677         m1[2][3]=0.0F;
678
679         m1[3][0]=0.0F;  
680         m1[3][1]=0.0F;  
681         m1[3][2]=0.0F;  
682         m1[3][3]=1.0F;
683
684
685 }
686 #endif
687
688 void Mat3CpyMat3(float m1[][3], float m2[][3]) 
689 {       
690         /* destination comes first: */
691         memcpy(&m1[0], &m2[0], 9*sizeof(float));
692 }
693
694 void Mat3MulSerie(float answ[][3],
695                                    float m1[][3], float m2[][3], float m3[][3],
696                                    float m4[][3], float m5[][3], float m6[][3],
697                                    float m7[][3], float m8[][3])
698 {
699         float temp[3][3];
700         
701         if(m1==0 || m2==0) return;
702
703         
704         Mat3MulMat3(answ, m2, m1);
705         if(m3) {
706                 Mat3MulMat3(temp, m3, answ);
707                 if(m4) {
708                         Mat3MulMat3(answ, m4, temp);
709                         if(m5) {
710                                 Mat3MulMat3(temp, m5, answ);
711                                 if(m6) {
712                                         Mat3MulMat3(answ, m6, temp);
713                                         if(m7) {
714                                                 Mat3MulMat3(temp, m7, answ);
715                                                 if(m8) {
716                                                         Mat3MulMat3(answ, m8, temp);
717                                                 }
718                                                 else Mat3CpyMat3(answ, temp);
719                                         }
720                                 }
721                                 else Mat3CpyMat3(answ, temp);
722                         }
723                 }
724                 else Mat3CpyMat3(answ, temp);
725         }
726 }
727
728 void Mat4MulSerie(float answ[][4], float m1[][4],
729                                 float m2[][4], float m3[][4], float m4[][4],
730                                 float m5[][4], float m6[][4], float m7[][4],
731                                 float m8[][4])
732 {
733         float temp[4][4];
734         
735         if(m1==0 || m2==0) return;
736         
737         Mat4MulMat4(answ, m2, m1);
738         if(m3) {
739                 Mat4MulMat4(temp, m3, answ);
740                 if(m4) {
741                         Mat4MulMat4(answ, m4, temp);
742                         if(m5) {
743                                 Mat4MulMat4(temp, m5, answ);
744                                 if(m6) {
745                                         Mat4MulMat4(answ, m6, temp);
746                                         if(m7) {
747                                                 Mat4MulMat4(temp, m7, answ);
748                                                 if(m8) {
749                                                         Mat4MulMat4(answ, m8, temp);
750                                                 }
751                                                 else Mat4CpyMat4(answ, temp);
752                                         }
753                                 }
754                                 else Mat4CpyMat4(answ, temp);
755                         }
756                 }
757                 else Mat4CpyMat4(answ, temp);
758         }
759 }
760
761 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
762 {
763         float squat[4], dquat[4], fquat[4];
764         float ssize[3], dsize[3], fsize[4];
765         float sloc[3], dloc[3], floc[3];
766         
767         Mat4ToQuat(dst, dquat);
768         Mat4ToSize(dst, dsize);
769         VecCopyf(dloc, dst[3]);
770
771         Mat4ToQuat(src, squat);
772         Mat4ToSize(src, ssize);
773         VecCopyf(sloc, src[3]);
774         
775         /* do blending */
776         VecLerpf(floc, dloc, sloc, srcweight);
777         QuatInterpol(fquat, dquat, squat, srcweight);
778         VecLerpf(fsize, dsize, ssize, srcweight);
779
780         /* compose new matrix */
781         LocQuatSizeToMat4(out, floc, fquat, fsize);
782 }
783
784 void Mat4Clr(float *m)
785 {
786         memset(m, 0, 4*4*sizeof(float));
787 }
788
789 void Mat3Clr(float *m)
790 {
791         memset(m, 0, 3*3*sizeof(float));
792 }
793
794 void Mat4One(float m[][4])
795 {
796
797         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
798         m[0][1]= m[0][2]= m[0][3]= 0.0;
799         m[1][0]= m[1][2]= m[1][3]= 0.0;
800         m[2][0]= m[2][1]= m[2][3]= 0.0;
801         m[3][0]= m[3][1]= m[3][2]= 0.0;
802 }
803
804 void Mat3One(float m[][3])
805 {
806
807         m[0][0]= m[1][1]= m[2][2]= 1.0;
808         m[0][1]= m[0][2]= 0.0;
809         m[1][0]= m[1][2]= 0.0;
810         m[2][0]= m[2][1]= 0.0;
811 }
812
813 void Mat4MulVec( float mat[][4], int *vec)
814 {
815         int x,y;
816
817         x=vec[0]; 
818         y=vec[1];
819         vec[0]=(int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]);
820         vec[1]=(int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]);
821         vec[2]=(int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]);
822 }
823
824 void Mat4MulVecfl( float mat[][4], float *vec)
825 {
826         float x,y;
827
828         x=vec[0]; 
829         y=vec[1];
830         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
831         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
832         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
833 }
834
835 void VecMat4MulVecfl(float *in, float mat[][4], float *vec)
836 {
837         float x,y;
838
839         x=vec[0]; 
840         y=vec[1];
841         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
842         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
843         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
844 }
845
846 void Mat4Mul3Vecfl( float mat[][4], float *vec)
847 {
848         float x,y;
849
850         x= vec[0]; 
851         y= vec[1];
852         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
853         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
854         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
855 }
856
857 void Mat4MulVec3Project(float mat[][4], float *vec)
858 {
859         float w;
860
861         w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
862         Mat4MulVecfl(mat, vec);
863
864         vec[0] /= w;
865         vec[1] /= w;
866         vec[2] /= w;
867 }
868
869 void Mat4MulVec4fl( float mat[][4], float *vec)
870 {
871         float x,y,z;
872
873         x=vec[0]; 
874         y=vec[1]; 
875         z= vec[2];
876         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
877         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
878         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
879         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
880 }
881
882 void Mat3MulVec( float mat[][3], int *vec)
883 {
884         int x,y;
885
886         x=vec[0]; 
887         y=vec[1];
888         vec[0]= (int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]);
889         vec[1]= (int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]);
890         vec[2]= (int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]);
891 }
892
893 void Mat3MulVecfl( float mat[][3], float *vec)
894 {
895         float x,y;
896
897         x=vec[0]; 
898         y=vec[1];
899         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
900         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
901         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
902 }
903
904 void Mat3MulVecd( float mat[][3], double *vec)
905 {
906         double x,y;
907
908         x=vec[0]; 
909         y=vec[1];
910         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
911         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
912         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
913 }
914
915 void Mat3TransMulVecfl( float mat[][3], float *vec)
916 {
917         float x,y;
918
919         x=vec[0]; 
920         y=vec[1];
921         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
922         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
923         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
924 }
925
926 void Mat3MulFloat(float *m, float f)
927 {
928         int i;
929
930         for(i=0;i<9;i++) m[i]*=f;
931 }
932
933 void Mat4MulFloat(float *m, float f)
934 {
935         int i;
936
937         for(i=0;i<16;i++) m[i]*=f;      /* count to 12: without vector component */
938 }
939
940
941 void Mat4MulFloat3(float *m, float f)           /* only scale component */
942 {
943         int i,j;
944
945         for(i=0; i<3; i++) {
946                 for(j=0; j<3; j++) {
947                         
948                         m[4*i+j] *= f;
949                 }
950         }
951 }
952
953 void Mat3AddMat3(float m1[][3], float m2[][3], float m3[][3])
954 {
955         int i, j;
956
957         for(i=0;i<3;i++)
958                 for(j=0;j<3;j++)
959                         m1[i][j]= m2[i][j] + m3[i][j];
960 }
961
962 void Mat4AddMat4(float m1[][4], float m2[][4], float m3[][4])
963 {
964         int i, j;
965
966         for(i=0;i<4;i++)
967                 for(j=0;j<4;j++)
968                         m1[i][j]= m2[i][j] + m3[i][j];
969 }
970
971 void VecStar(float mat[][3], float *vec)
972 {
973
974         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
975         mat[0][1]= -vec[2];     
976         mat[0][2]= vec[1];
977         mat[1][0]= vec[2];      
978         mat[1][2]= -vec[0];
979         mat[2][0]= -vec[1];     
980         mat[2][1]= vec[0];
981         
982 }
983 #ifdef TEST_ACTIVE
984 short EenheidsMat(float mat[][3])
985 {
986
987         if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0)
988                 if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0)
989                         if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0)
990                                 return 1;
991         return 0;
992 }
993 #endif
994
995 int FloatCompare( float *v1,  float *v2, float limit)
996 {
997
998         if( fabs(v1[0]-v2[0])<limit ) {
999                 if( fabs(v1[1]-v2[1])<limit ) {
1000                         if( fabs(v1[2]-v2[2])<limit ) return 1;
1001                 }
1002         }
1003         return 0;
1004 }
1005
1006 float FloatLerpf( float target, float origin, float fac)
1007 {
1008         return (fac*target) + (1.0f-fac)*origin;
1009 }
1010
1011 void printvecf( char *str,  float v[3])
1012 {
1013         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
1014
1015 }
1016
1017 void printquat( char *str,  float q[4])
1018 {
1019         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
1020
1021 }
1022
1023 void printvec4f( char *str,  float v[4])
1024 {
1025         printf("%s\n", str);
1026         printf("%f %f %f %f\n",v[0],v[1],v[2], v[3]);
1027         printf("\n");
1028
1029 }
1030
1031 void printmatrix4( char *str,  float m[][4])
1032 {
1033         printf("%s\n", str);
1034         printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
1035         printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
1036         printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
1037         printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
1038         printf("\n");
1039
1040 }
1041
1042 void printmatrix3( char *str,  float m[][3])
1043 {
1044         printf("%s\n", str);
1045         printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
1046         printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
1047         printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
1048         printf("\n");
1049
1050 }
1051
1052 /* **************** QUATERNIONS ********** */
1053
1054
1055 void QuatMul(float *q, float *q1, float *q2)
1056 {
1057         float t0,t1,t2;
1058
1059         t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
1060         t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
1061         t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
1062         q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
1063         q[0]=t0; 
1064         q[1]=t1; 
1065         q[2]=t2;
1066 }
1067
1068 /* Assumes a unit quaternion */
1069 void QuatMulVecf(float *q, float *v)
1070 {
1071         float t0, t1, t2;
1072
1073         t0=  -q[1]*v[0]-q[2]*v[1]-q[3]*v[2];
1074         t1=   q[0]*v[0]+q[2]*v[2]-q[3]*v[1];
1075         t2=   q[0]*v[1]+q[3]*v[0]-q[1]*v[2];
1076         v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0];
1077         v[0]=t1; 
1078         v[1]=t2;
1079
1080         t1=   t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
1081         t2=   t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
1082         v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
1083         v[0]=t1; 
1084         v[1]=t2;
1085 }
1086
1087 void QuatConj(float *q)
1088 {
1089         q[1] = -q[1];
1090         q[2] = -q[2];
1091         q[3] = -q[3];
1092 }
1093
1094 float QuatDot(float *q1, float *q2)
1095 {
1096         return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
1097 }
1098
1099 void QuatInv(float *q)
1100 {
1101         float f = QuatDot(q, q);
1102
1103         if (f == 0.0f)
1104                 return;
1105
1106         QuatConj(q);
1107         QuatMulf(q, 1.0f/f);
1108 }
1109
1110 /* simple mult */
1111 void QuatMulf(float *q, float f)
1112 {
1113         q[0] *= f;
1114         q[1] *= f;
1115         q[2] *= f;
1116         q[3] *= f;
1117 }
1118
1119 void QuatSub(float *q, float *q1, float *q2)
1120 {
1121         q2[0]= -q2[0];
1122         QuatMul(q, q1, q2);
1123         q2[0]= -q2[0];
1124 }
1125
1126 /* angular mult factor */
1127 void QuatMulFac(float *q, float fac)
1128 {
1129         float angle= fac*saacos(q[0]);  /* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */
1130         
1131         float co= (float)cos(angle);
1132         float si= (float)sin(angle);
1133         q[0]= co;
1134         Normalize(q+1);
1135         q[1]*= si;
1136         q[2]*= si;
1137         q[3]*= si;
1138         
1139 }
1140
1141 void QuatToMat3( float *q, float m[][3])
1142 {
1143         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1144
1145         q0= M_SQRT2 * q[0];
1146         q1= M_SQRT2 * q[1];
1147         q2= M_SQRT2 * q[2];
1148         q3= M_SQRT2 * q[3];
1149
1150         qda= q0*q1;
1151         qdb= q0*q2;
1152         qdc= q0*q3;
1153         qaa= q1*q1;
1154         qab= q1*q2;
1155         qac= q1*q3;
1156         qbb= q2*q2;
1157         qbc= q2*q3;
1158         qcc= q3*q3;
1159
1160         m[0][0]= (float)(1.0-qbb-qcc);
1161         m[0][1]= (float)(qdc+qab);
1162         m[0][2]= (float)(-qdb+qac);
1163
1164         m[1][0]= (float)(-qdc+qab);
1165         m[1][1]= (float)(1.0-qaa-qcc);
1166         m[1][2]= (float)(qda+qbc);
1167
1168         m[2][0]= (float)(qdb+qac);
1169         m[2][1]= (float)(-qda+qbc);
1170         m[2][2]= (float)(1.0-qaa-qbb);
1171 }
1172
1173
1174 void QuatToMat4( float *q, float m[][4])
1175 {
1176         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1177
1178         q0= M_SQRT2 * q[0];
1179         q1= M_SQRT2 * q[1];
1180         q2= M_SQRT2 * q[2];
1181         q3= M_SQRT2 * q[3];
1182
1183         qda= q0*q1;
1184         qdb= q0*q2;
1185         qdc= q0*q3;
1186         qaa= q1*q1;
1187         qab= q1*q2;
1188         qac= q1*q3;
1189         qbb= q2*q2;
1190         qbc= q2*q3;
1191         qcc= q3*q3;
1192
1193         m[0][0]= (float)(1.0-qbb-qcc);
1194         m[0][1]= (float)(qdc+qab);
1195         m[0][2]= (float)(-qdb+qac);
1196         m[0][3]= 0.0f;
1197
1198         m[1][0]= (float)(-qdc+qab);
1199         m[1][1]= (float)(1.0-qaa-qcc);
1200         m[1][2]= (float)(qda+qbc);
1201         m[1][3]= 0.0f;
1202
1203         m[2][0]= (float)(qdb+qac);
1204         m[2][1]= (float)(-qda+qbc);
1205         m[2][2]= (float)(1.0-qaa-qbb);
1206         m[2][3]= 0.0f;
1207         
1208         m[3][0]= m[3][1]= m[3][2]= 0.0f;
1209         m[3][3]= 1.0f;
1210 }
1211
1212 void Mat3ToQuat(float wmat[][3], float *q)
1213 {
1214         double tr, s;
1215         float mat[3][3];
1216
1217         /* work on a copy */
1218         Mat3CpyMat3(mat, wmat);
1219         Mat3Ortho(mat);                 /* this is needed AND a NormalQuat in the end */
1220         
1221         tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
1222         
1223         if(tr>FLT_EPSILON) {
1224                 s= sqrt( tr);
1225                 q[0]= (float)s;
1226                 s= 1.0/(4.0*s);
1227                 q[1]= (float)((mat[1][2]-mat[2][1])*s);
1228                 q[2]= (float)((mat[2][0]-mat[0][2])*s);
1229                 q[3]= (float)((mat[0][1]-mat[1][0])*s);
1230         }
1231         else {
1232                 if(mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
1233                         s= 2.0*sqrtf(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
1234                         q[1]= (float)(0.25*s);
1235
1236                         s= 1.0/s;
1237                         q[0]= (float)((mat[2][1] - mat[1][2])*s);
1238                         q[2]= (float)((mat[1][0] + mat[0][1])*s);
1239                         q[3]= (float)((mat[2][0] + mat[0][2])*s);
1240                 }
1241                 else if(mat[1][1] > mat[2][2]) {
1242                         s= 2.0*sqrtf(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
1243                         q[2]= (float)(0.25*s);
1244
1245                         s= 1.0/s;
1246                         q[0]= (float)((mat[2][0] - mat[0][2])*s);
1247                         q[1]= (float)((mat[1][0] + mat[0][1])*s);
1248                         q[3]= (float)((mat[2][1] + mat[1][2])*s);
1249                 }
1250                 else {
1251                         s= 2.0*sqrtf(1.0 + mat[2][2] - mat[0][0] - mat[1][1]);
1252                         q[3]= (float)(0.25*s);
1253
1254                         s= 1.0/s;
1255                         q[0]= (float)((mat[1][0] - mat[0][1])*s);
1256                         q[1]= (float)((mat[2][0] + mat[0][2])*s);
1257                         q[2]= (float)((mat[2][1] + mat[1][2])*s);
1258                 }
1259         }
1260         NormalQuat(q);
1261 }
1262
1263 void Mat3ToQuat_is_ok( float wmat[][3], float *q)
1264 {
1265         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
1266
1267         /* work on a copy */
1268         Mat3CpyMat3(mat, wmat);
1269         Mat3Ortho(mat);
1270         
1271         /* rotate z-axis of matrix to z-axis */
1272
1273         nor[0] = mat[2][1];             /* cross product with (0,0,1) */
1274         nor[1] =  -mat[2][0];
1275         nor[2] = 0.0;
1276         Normalize(nor);
1277         
1278         co= mat[2][2];
1279         angle= 0.5f*saacos(co);
1280         
1281         co= (float)cos(angle);
1282         si= (float)sin(angle);
1283         q1[0]= co;
1284         q1[1]= -nor[0]*si;              /* negative here, but why? */
1285         q1[2]= -nor[1]*si;
1286         q1[3]= -nor[2]*si;
1287
1288         /* rotate back x-axis from mat, using inverse q1 */
1289         QuatToMat3(q1, matr);
1290         Mat3Inv(matn, matr);
1291         Mat3MulVecfl(matn, mat[0]);
1292         
1293         /* and align x-axes */
1294         angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1295         
1296         co= (float)cos(angle);
1297         si= (float)sin(angle);
1298         q2[0]= co;
1299         q2[1]= 0.0f;
1300         q2[2]= 0.0f;
1301         q2[3]= si;
1302         
1303         QuatMul(q, q1, q2);
1304 }
1305
1306
1307 void Mat4ToQuat( float m[][4], float *q)
1308 {
1309         float mat[3][3];
1310         
1311         Mat3CpyMat4(mat, m);
1312         Mat3ToQuat(mat, q);
1313         
1314 }
1315
1316 void QuatOne(float *q)
1317 {
1318         q[0]= q[2]= q[3]= 0.0;
1319         q[1]= 1.0;
1320 }
1321
1322 void NormalQuat(float *q)
1323 {
1324         float len;
1325         
1326         len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
1327         if(len!=0.0) {
1328                 q[0]/= len;
1329                 q[1]/= len;
1330                 q[2]/= len;
1331                 q[3]/= len;
1332         } else {
1333                 q[1]= 1.0f;
1334                 q[0]= q[2]= q[3]= 0.0f;                 
1335         }
1336 }
1337
1338 void vectoquat(float *vec, short axis, short upflag, float *q)
1339 {
1340         float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
1341         
1342         /* first rotate to axis */
1343         if(axis>2) {    
1344                 x2= vec[0] ; y2= vec[1] ; z2= vec[2];
1345                 axis-= 3;
1346         }
1347         else {
1348                 x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
1349         }
1350         
1351         q[0]=1.0; 
1352         q[1]=q[2]=q[3]= 0.0;
1353
1354         len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
1355         if(len1 == 0.0) return;
1356
1357         /* nasty! I need a good routine for this...
1358          * problem is a rotation of an Y axis to the negative Y-axis for example.
1359          */
1360
1361         if(axis==0) {   /* x-axis */
1362                 nor[0]= 0.0;
1363                 nor[1]= -z2;
1364                 nor[2]= y2;
1365
1366                 if(fabs(y2)+fabs(z2)<0.0001)
1367                         nor[1]= 1.0;
1368
1369                 co= x2;
1370         }
1371         else if(axis==1) {      /* y-axis */
1372                 nor[0]= z2;
1373                 nor[1]= 0.0;
1374                 nor[2]= -x2;
1375                 
1376                 if(fabs(x2)+fabs(z2)<0.0001)
1377                         nor[2]= 1.0;
1378                 
1379                 co= y2;
1380         }
1381         else {                  /* z-axis */
1382                 nor[0]= -y2;
1383                 nor[1]= x2;
1384                 nor[2]= 0.0;
1385
1386                 if(fabs(x2)+fabs(y2)<0.0001)
1387                         nor[0]= 1.0;
1388
1389                 co= z2;
1390         }
1391         co/= len1;
1392
1393         Normalize(nor);
1394         
1395         angle= 0.5f*saacos(co);
1396         si= (float)sin(angle);
1397         q[0]= (float)cos(angle);
1398         q[1]= nor[0]*si;
1399         q[2]= nor[1]*si;
1400         q[3]= nor[2]*si;
1401         
1402         if(axis!=upflag) {
1403                 QuatToMat3(q, mat);
1404
1405                 fp= mat[2];
1406                 if(axis==0) {
1407                         if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1]));
1408                         else angle= (float)(-0.5*atan2(fp[1], fp[2]));
1409                 }
1410                 else if(axis==1) {
1411                         if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0]));
1412                         else angle= (float)(0.5*atan2(fp[0], fp[2]));
1413                 }
1414                 else {
1415                         if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0]));
1416                         else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
1417                 }
1418                                 
1419                 co= (float)cos(angle);
1420                 si= (float)(sin(angle)/len1);
1421                 q2[0]= co;
1422                 q2[1]= x2*si;
1423                 q2[2]= y2*si;
1424                 q2[3]= z2*si;
1425                         
1426                 QuatMul(q,q2,q);
1427         }
1428 }
1429
1430 void VecUpMat3old( float *vec, float mat[][3], short axis)
1431 {
1432         float inp, up[3];
1433         short cox = 0, coy = 0, coz = 0;
1434         
1435         /* using different up's is not useful, infact there is no real 'up'!
1436          */
1437
1438         up[0]= 0.0;
1439         up[1]= 0.0;
1440         up[2]= 1.0;
1441
1442         if(axis==0) {
1443                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1444         }
1445         if(axis==1) {
1446                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1447         }
1448         if(axis==2) {
1449                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1450         }
1451         if(axis==3) {
1452                 cox= 0; coy= 2; coz= 1;         /*  */
1453         }
1454         if(axis==4) {
1455                 cox= 1; coy= 0; coz= 2;         /*  */
1456         }
1457         if(axis==5) {
1458                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1459         }
1460
1461         mat[coz][0]= vec[0];
1462         mat[coz][1]= vec[1];
1463         mat[coz][2]= vec[2];
1464         Normalize((float *)mat[coz]);
1465         
1466         inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
1467         mat[coy][0]= up[0] - inp*mat[coz][0];
1468         mat[coy][1]= up[1] - inp*mat[coz][1];
1469         mat[coy][2]= up[2] - inp*mat[coz][2];
1470
1471         Normalize((float *)mat[coy]);
1472         
1473         Crossf(mat[cox], mat[coy], mat[coz]);
1474         
1475 }
1476
1477 void VecUpMat3(float *vec, float mat[][3], short axis)
1478 {
1479         float inp;
1480         short cox = 0, coy = 0, coz = 0;
1481
1482         /* using different up's is not useful, infact there is no real 'up'!
1483         */
1484
1485         if(axis==0) {
1486                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1487         }
1488         if(axis==1) {
1489                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1490         }
1491         if(axis==2) {
1492                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1493         }
1494         if(axis==3) {
1495                 cox= 0; coy= 1; coz= 2;         /* Y op -Z tr */
1496                 vec[0]= -vec[0];
1497                 vec[1]= -vec[1];
1498                 vec[2]= -vec[2];
1499         }
1500         if(axis==4) {
1501                 cox= 1; coy= 0; coz= 2;         /*  */
1502         }
1503         if(axis==5) {
1504                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1505         }
1506
1507         mat[coz][0]= vec[0];
1508         mat[coz][1]= vec[1];
1509         mat[coz][2]= vec[2];
1510         Normalize((float *)mat[coz]);
1511         
1512         inp= mat[coz][2];
1513         mat[coy][0]= - inp*mat[coz][0];
1514         mat[coy][1]= - inp*mat[coz][1];
1515         mat[coy][2]= 1.0f - inp*mat[coz][2];
1516
1517         Normalize((float *)mat[coy]);
1518         
1519         Crossf(mat[cox], mat[coy], mat[coz]);
1520         
1521 }
1522
1523 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
1524 void QuatInterpolW(float *, float *, float *, float );
1525
1526 void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
1527 {
1528         float omega, cosom, sinom, sc1, sc2;
1529
1530         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1531         
1532         /* rotate around shortest angle */
1533         if ((1.0 + cosom) > 0.0001) {
1534                 
1535                 if ((1.0 - cosom) > 0.0001) {
1536                         omega = acos(cosom);
1537                         sinom = sin(omega);
1538                         sc1 = sin((1.0 - t) * omega) / sinom;
1539                         sc2 = sin(t * omega) / sinom;
1540                 } 
1541                 else {
1542                         sc1 = 1.0 - t;
1543                         sc2 = t;
1544                 }
1545                 result[0] = sc1*quat1[0] + sc2*quat2[0];
1546                 result[1] = sc1*quat1[1] + sc2*quat2[1];
1547                 result[2] = sc1*quat1[2] + sc2*quat2[2];
1548                 result[3] = sc1*quat1[3] + sc2*quat2[3];
1549         } 
1550         else {
1551                 result[0] = quat2[3];
1552                 result[1] = -quat2[2];
1553                 result[2] = quat2[1];
1554                 result[3] = -quat2[0];
1555                 
1556                 sc1 = sin((1.0 - t)*M_PI_2);
1557                 sc2 = sin(t*M_PI_2);
1558
1559                 result[0] = sc1*quat1[0] + sc2*result[0];
1560                 result[1] = sc1*quat1[1] + sc2*result[1];
1561                 result[2] = sc1*quat1[2] + sc2*result[2];
1562                 result[3] = sc1*quat1[3] + sc2*result[3];
1563         }
1564 }
1565
1566 void QuatInterpol(float *result, float *quat1, float *quat2, float t)
1567 {
1568         float quat[4], omega, cosom, sinom, sc1, sc2;
1569
1570         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1571         
1572         /* rotate around shortest angle */
1573         if (cosom < 0.0) {
1574                 cosom = -cosom;
1575                 quat[0]= -quat1[0];
1576                 quat[1]= -quat1[1];
1577                 quat[2]= -quat1[2];
1578                 quat[3]= -quat1[3];
1579         } 
1580         else {
1581                 quat[0]= quat1[0];
1582                 quat[1]= quat1[1];
1583                 quat[2]= quat1[2];
1584                 quat[3]= quat1[3];
1585         }
1586         
1587         if ((1.0 - cosom) > 0.0001) {
1588                 omega = acos(cosom);
1589                 sinom = sin(omega);
1590                 sc1 = sin((1 - t) * omega) / sinom;
1591                 sc2 = sin(t * omega) / sinom;
1592         } else {
1593                 sc1= 1.0 - t;
1594                 sc2= t;
1595         }
1596         
1597         result[0] = sc1 * quat[0] + sc2 * quat2[0];
1598         result[1] = sc1 * quat[1] + sc2 * quat2[1];
1599         result[2] = sc1 * quat[2] + sc2 * quat2[2];
1600         result[3] = sc1 * quat[3] + sc2 * quat2[3];
1601 }
1602
1603 void QuatAdd(float *result, float *quat1, float *quat2, float t)
1604 {
1605         result[0]= quat1[0] + t*quat2[0];
1606         result[1]= quat1[1] + t*quat2[1];
1607         result[2]= quat1[2] + t*quat2[2];
1608         result[3]= quat1[3] + t*quat2[3];
1609 }
1610
1611 void QuatCopy(float *q1, float *q2)
1612 {
1613         q1[0]= q2[0];
1614         q1[1]= q2[1];
1615         q1[2]= q2[2];
1616         q1[3]= q2[3];
1617 }
1618
1619 /* **************** DUAL QUATERNIONS ************** */
1620
1621 /*
1622    Conversion routines between (regular quaternion, translation) and
1623    dual quaternion.
1624
1625    Version 1.0.0, February 7th, 2007
1626
1627    Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights 
1628    Reserved
1629
1630    This software is provided 'as-is', without any express or implied
1631    warranty.  In no event will the author(s) be held liable for any damages
1632    arising from the use of this software.
1633
1634    Permission is granted to anyone to use this software for any purpose,
1635    including commercial applications, and to alter it and redistribute it
1636    freely, subject to the following restrictions:
1637
1638    1. The origin of this software must not be misrepresented; you must not
1639       claim that you wrote the original software. If you use this software
1640       in a product, an acknowledgment in the product documentation would be
1641       appreciated but is not required.
1642    2. Altered source versions must be plainly marked as such, and must not be
1643       misrepresented as being the original software.
1644    3. This notice may not be removed or altered from any source distribution.
1645
1646    Author: Ladislav Kavan, kavanl@cs.tcd.ie
1647
1648    Changes for Blender:
1649    - renaming, style changes and optimizations
1650    - added support for scaling
1651 */
1652
1653 void Mat4ToDQuat(float basemat[][4], float mat[][4], DualQuat *dq)
1654 {
1655         float *t, *q, dscale[3], scale[3], basequat[4];
1656         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1657         float R[4][4], S[4][4];
1658
1659         /* split scaling and rotation, there is probably a faster way to do
1660            this, it's done like this now to correctly get negative scaling */
1661         Mat4MulMat4(baseRS, basemat, mat);
1662         Mat4ToSize(baseRS, scale);
1663
1664         VecCopyf(dscale, scale);
1665         dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f;
1666
1667         if((Det4x4(mat) < 0.0f) || VecLength(dscale) > 1e-4) {
1668                 /* extract R and S  */
1669                 Mat4ToQuat(baseRS, basequat);
1670                 QuatToMat4(basequat, baseR);
1671                 VecCopyf(baseR[3], baseRS[3]);
1672
1673                 Mat4Invert(baseinv, basemat);
1674                 Mat4MulMat4(R, baseinv, baseR);
1675
1676                 Mat4Invert(baseRinv, baseR);
1677                 Mat4MulMat4(S, baseRS, baseRinv);
1678
1679                 /* set scaling part */
1680                 Mat4MulSerie(dq->scale, basemat, S, baseinv, 0, 0, 0, 0, 0);
1681                 dq->scale_weight= 1.0f;
1682         }
1683         else {
1684                 /* matrix does not contain scaling */
1685                 Mat4CpyMat4(R, mat);
1686                 dq->scale_weight= 0.0f;
1687         }
1688
1689         /* non-dual part */
1690         Mat4ToQuat(R, dq->quat);
1691
1692         /* dual part */
1693         t= R[3];
1694         q= dq->quat;
1695         dq->trans[0]= -0.5f*( t[0]*q[1] + t[1]*q[2] + t[2]*q[3]);
1696         dq->trans[1]=  0.5f*( t[0]*q[0] + t[1]*q[3] - t[2]*q[2]);
1697         dq->trans[2]=  0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]);
1698         dq->trans[3]=  0.5f*( t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
1699 }
1700
1701 void DQuatToMat4(DualQuat *dq, float mat[][4])
1702 {
1703         float len, *t, q0[4];
1704         
1705         /* regular quaternion */
1706         QuatCopy(q0, dq->quat);
1707
1708         /* normalize */
1709         len= sqrt(QuatDot(q0, q0)); 
1710         if(len != 0.0f)
1711                 QuatMulf(q0, 1.0f/len);
1712         
1713         /* rotation */
1714         QuatToMat4(q0, mat);
1715
1716         /* translation */
1717         t= dq->trans;
1718         mat[3][0]= 2.0*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]);
1719         mat[3][1]= 2.0*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]);
1720         mat[3][2]= 2.0*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]);
1721
1722         /* note: this does not handle scaling */
1723 }       
1724
1725 void DQuatAddWeighted(DualQuat *dqsum, DualQuat *dq, float weight)
1726 {
1727         int flipped= 0;
1728
1729         /* make sure we interpolate quats in the right direction */
1730         if (QuatDot(dq->quat, dqsum->quat) < 0) {
1731                 flipped= 1;
1732                 weight= -weight;
1733         }
1734
1735         /* interpolate rotation and translation */
1736         dqsum->quat[0] += weight*dq->quat[0];
1737         dqsum->quat[1] += weight*dq->quat[1];
1738         dqsum->quat[2] += weight*dq->quat[2];
1739         dqsum->quat[3] += weight*dq->quat[3];
1740
1741         dqsum->trans[0] += weight*dq->trans[0];
1742         dqsum->trans[1] += weight*dq->trans[1];
1743         dqsum->trans[2] += weight*dq->trans[2];
1744         dqsum->trans[3] += weight*dq->trans[3];
1745
1746         /* interpolate scale - but only if needed */
1747         if (dq->scale_weight) {
1748                 float wmat[4][4];
1749
1750                 if(flipped)     /* we don't want negative weights for scaling */
1751                         weight= -weight;
1752
1753                 Mat4CpyMat4(wmat, dq->scale);
1754                 Mat4MulFloat((float*)wmat, weight);
1755                 Mat4AddMat4(dqsum->scale, dqsum->scale, wmat);
1756                 dqsum->scale_weight += weight;
1757         }
1758 }
1759
1760 void DQuatNormalize(DualQuat *dq, float totweight)
1761 {
1762         float scale= 1.0f/totweight;
1763
1764         QuatMulf(dq->quat, scale);
1765         QuatMulf(dq->trans, scale);
1766         
1767         if(dq->scale_weight) {
1768                 float addweight= totweight - dq->scale_weight;
1769
1770                 if(addweight) {
1771                         dq->scale[0][0] += addweight;
1772                         dq->scale[1][1] += addweight;
1773                         dq->scale[2][2] += addweight;
1774                         dq->scale[3][3] += addweight;
1775                 }
1776
1777                 Mat4MulFloat((float*)dq->scale, scale);
1778                 dq->scale_weight= 1.0f;
1779         }
1780 }
1781
1782 void DQuatMulVecfl(DualQuat *dq, float *co, float mat[][3])
1783 {       
1784         float M[3][3], t[3], scalemat[3][3], len2;
1785         float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3];
1786         float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3];
1787         
1788         /* rotation matrix */
1789         M[0][0]= w*w + x*x - y*y - z*z;
1790         M[1][0]= 2*(x*y - w*z);
1791         M[2][0]= 2*(x*z + w*y);
1792
1793         M[0][1]= 2*(x*y + w*z);
1794         M[1][1]= w*w + y*y - x*x - z*z;
1795         M[2][1]= 2*(y*z - w*x); 
1796
1797         M[0][2]= 2*(x*z - w*y);
1798         M[1][2]= 2*(y*z + w*x);
1799         M[2][2]= w*w + z*z - x*x - y*y;
1800         
1801         len2= QuatDot(dq->quat, dq->quat);
1802         if(len2 > 0.0f)
1803                 len2= 1.0f/len2;
1804         
1805         /* translation */
1806         t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3);
1807         t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2);
1808         t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y);
1809
1810         /* apply scaling */
1811         if(dq->scale_weight)
1812                 Mat4MulVecfl(dq->scale, co);
1813         
1814         /* apply rotation and translation */
1815         Mat3MulVecfl(M, co);
1816         co[0]= (co[0] + t[0])*len2;
1817         co[1]= (co[1] + t[1])*len2;
1818         co[2]= (co[2] + t[2])*len2;
1819
1820         /* compute crazyspace correction mat */
1821         if(mat) {
1822                 if(dq->scale_weight) {
1823                         Mat3CpyMat4(scalemat, dq->scale);
1824                         Mat3MulMat3(mat, M, scalemat);
1825                 }
1826                 else
1827                         Mat3CpyMat3(mat, M);
1828                 Mat3MulFloat((float*)mat, len2);
1829         }
1830 }
1831
1832 void DQuatCpyDQuat(DualQuat *dq1, DualQuat *dq2)
1833 {
1834         memcpy(dq1, dq2, sizeof(DualQuat));
1835 }
1836
1837 /* **************** VIEW / PROJECTION ********************************  */
1838
1839
1840 void i_ortho(
1841         float left, float right,
1842         float bottom, float top,
1843         float nearClip, float farClip,
1844         float matrix[][4]
1845 ){
1846     float Xdelta, Ydelta, Zdelta;
1847  
1848     Xdelta = right - left;
1849     Ydelta = top - bottom;
1850     Zdelta = farClip - nearClip;
1851     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1852                 return;
1853     }
1854     Mat4One(matrix);
1855     matrix[0][0] = 2.0f/Xdelta;
1856     matrix[3][0] = -(right + left)/Xdelta;
1857     matrix[1][1] = 2.0f/Ydelta;
1858     matrix[3][1] = -(top + bottom)/Ydelta;
1859     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1860     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1861 }
1862
1863 void i_window(
1864         float left, float right,
1865         float bottom, float top,
1866         float nearClip, float farClip,
1867         float mat[][4]
1868 ){
1869         float Xdelta, Ydelta, Zdelta;
1870
1871         Xdelta = right - left;
1872         Ydelta = top - bottom;
1873         Zdelta = farClip - nearClip;
1874
1875         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1876                 return;
1877         }
1878         mat[0][0] = nearClip * 2.0f/Xdelta;
1879         mat[1][1] = nearClip * 2.0f/Ydelta;
1880         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1881         mat[2][1] = (top + bottom)/Ydelta;
1882         mat[2][2] = -(farClip + nearClip)/Zdelta;
1883         mat[2][3] = -1.0f;
1884         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1885         mat[0][1] = mat[0][2] = mat[0][3] =
1886             mat[1][0] = mat[1][2] = mat[1][3] =
1887             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1888
1889 }
1890
1891 void i_translate(float Tx, float Ty, float Tz, float mat[][4])
1892 {
1893     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1894     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1895     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1896 }
1897
1898 void i_multmatrix( float icand[][4], float Vm[][4])
1899 {
1900     int row, col;
1901     float temp[4][4];
1902
1903     for(row=0 ; row<4 ; row++) 
1904         for(col=0 ; col<4 ; col++)
1905             temp[row][col] = icand[row][0] * Vm[0][col]
1906                            + icand[row][1] * Vm[1][col]
1907                            + icand[row][2] * Vm[2][col]
1908                            + icand[row][3] * Vm[3][col];
1909         Mat4CpyMat4(Vm, temp);
1910 }
1911
1912 void i_rotate(float angle, char axis, float mat[][4])
1913 {
1914         int col;
1915     float temp[4];
1916     float cosine, sine;
1917
1918     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
1919         temp[col] = 0;
1920
1921     angle = (float)(angle*(3.1415926535/180.0));
1922     cosine = (float)cos(angle);
1923     sine = (float)sin(angle);
1924     switch(axis){
1925     case 'x':    
1926     case 'X':    
1927         for(col=0 ; col<4 ; col++)
1928             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1929         for(col=0 ; col<4 ; col++) {
1930             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1931             mat[1][col] = temp[col];
1932         }
1933         break;
1934
1935     case 'y':
1936     case 'Y':
1937         for(col=0 ; col<4 ; col++)
1938             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1939         for(col=0 ; col<4 ; col++) {
1940             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1941             mat[0][col] = temp[col];
1942         }
1943         break;
1944
1945     case 'z':
1946     case 'Z':
1947         for(col=0 ; col<4 ; col++)
1948             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
1949         for(col=0 ; col<4 ; col++) {
1950             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
1951             mat[0][col] = temp[col];
1952         }
1953         break;
1954     }
1955 }
1956
1957 void i_polarview(float dist, float azimuth, float incidence, float twist, float Vm[][4])
1958 {
1959
1960         Mat4One(Vm);
1961
1962     i_translate(0.0, 0.0, -dist, Vm);
1963     i_rotate(-twist,'z', Vm);   
1964     i_rotate(-incidence,'x', Vm);
1965     i_rotate(-azimuth,'z', Vm);
1966 }
1967
1968 void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float twist, float mat[][4])
1969 {
1970         float sine, cosine, hyp, hyp1, dx, dy, dz;
1971         float mat1[4][4];
1972         
1973         Mat4One(mat);
1974         Mat4One(mat1);
1975
1976         i_rotate(-twist,'z', mat);
1977
1978         dx = px - vx;
1979         dy = py - vy;
1980         dz = pz - vz;
1981         hyp = dx * dx + dz * dz;        /* hyp squared  */
1982         hyp1 = (float)sqrt(dy*dy + hyp);
1983         hyp = (float)sqrt(hyp);         /* the real hyp */
1984         
1985         if (hyp1 != 0.0) {              /* rotate X     */
1986                 sine = -dy / hyp1;
1987                 cosine = hyp /hyp1;
1988         } else {
1989                 sine = 0;
1990                 cosine = 1.0f;
1991         }
1992         mat1[1][1] = cosine;
1993         mat1[1][2] = sine;
1994         mat1[2][1] = -sine;
1995         mat1[2][2] = cosine;
1996         
1997         i_multmatrix(mat1, mat);
1998
1999     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
2000     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
2001         
2002         /* paragraph    */
2003         if (hyp != 0.0f) {                      /* rotate Y     */
2004                 sine = dx / hyp;
2005                 cosine = -dz / hyp;
2006         } else {
2007                 sine = 0;
2008                 cosine = 1.0f;
2009         }
2010         mat1[0][0] = cosine;
2011         mat1[0][2] = -sine;
2012         mat1[2][0] = sine;
2013         mat1[2][2] = cosine;
2014         
2015         i_multmatrix(mat1, mat);
2016         i_translate(-vx,-vy,-vz, mat);  /* translate viewpoint to origin */
2017 }
2018
2019
2020
2021
2022
2023 /* ************************************************  */
2024
2025 void Mat3Ortho(float mat[][3])
2026 {       
2027         Normalize(mat[0]);
2028         Normalize(mat[1]);
2029         Normalize(mat[2]);
2030 }
2031
2032 void Mat4Ortho(float mat[][4])
2033 {
2034         float len;
2035         
2036         len= Normalize(mat[0]);
2037         if(len!=0.0) mat[0][3]/= len;
2038         len= Normalize(mat[1]);
2039         if(len!=0.0) mat[1][3]/= len;
2040         len= Normalize(mat[2]);
2041         if(len!=0.0) mat[2][3]/= len;
2042 }
2043
2044 void VecCopyf(float *v1, float *v2)
2045 {
2046         v1[0]= v2[0];
2047         v1[1]= v2[1];
2048         v1[2]= v2[2];
2049 }
2050
2051 int VecLen( int *v1, int *v2)
2052 {
2053         float x,y,z;
2054
2055         x=(float)(v1[0]-v2[0]);
2056         y=(float)(v1[1]-v2[1]);
2057         z=(float)(v1[2]-v2[2]);
2058         return (int)floor(sqrt(x*x+y*y+z*z));
2059 }
2060
2061 float VecLenf( float *v1, float *v2)
2062 {
2063         float x,y,z;
2064
2065         x=v1[0]-v2[0];
2066         y=v1[1]-v2[1];
2067         z=v1[2]-v2[2];
2068         return (float)sqrt(x*x+y*y+z*z);
2069 }
2070
2071 float VecLength(float *v)
2072 {
2073         return (float) sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
2074 }
2075
2076 void VecAddf(float *v, float *v1, float *v2)
2077 {
2078         v[0]= v1[0]+ v2[0];
2079         v[1]= v1[1]+ v2[1];
2080         v[2]= v1[2]+ v2[2];
2081 }
2082
2083 void VecSubf(float *v, float *v1, float *v2)
2084 {
2085         v[0]= v1[0]- v2[0];
2086         v[1]= v1[1]- v2[1];
2087         v[2]= v1[2]- v2[2];
2088 }
2089
2090 void VecLerpf(float *target, float *a, float *b, float t)
2091 {
2092         float s = 1.0f-t;
2093
2094         target[0]= s*a[0] + t*b[0];
2095         target[1]= s*a[1] + t*b[1];
2096         target[2]= s*a[2] + t*b[2];
2097 }
2098
2099 void VecMidf(float *v, float *v1, float *v2)
2100 {
2101         v[0]= 0.5f*(v1[0]+ v2[0]);
2102         v[1]= 0.5f*(v1[1]+ v2[1]);
2103         v[2]= 0.5f*(v1[2]+ v2[2]);
2104 }
2105
2106 void VecMulf(float *v1, float f)
2107 {
2108
2109         v1[0]*= f;
2110         v1[1]*= f;
2111         v1[2]*= f;
2112 }
2113
2114 void VecOrthoBasisf(float *v, float *v1, float *v2)
2115 {
2116         float f = sqrt(v[0]*v[0] + v[1]*v[1]);
2117
2118         if (f < 1e-35f) {
2119                 // degenerate case
2120                 v1[0] = 0.0f; v1[1] = 1.0f; v1[2] = 0.0f;
2121                 if (v[2] > 0.0f) {
2122                         v2[0] = 1.0f; v2[1] = v2[2] = 0.0f;
2123                 }
2124                 else {
2125                         v2[0] = -1.0f; v2[1] = v2[2] = 0.0f;
2126                 }
2127         }
2128         else  {
2129                 f = 1.0f/f;
2130                 v1[0] = v[1]*f;
2131                 v1[1] = -v[0]*f;
2132                 v1[2] = 0.0f;
2133
2134                 Crossf(v2, v, v1);
2135         }
2136 }
2137
2138 int VecLenCompare(float *v1, float *v2, float limit)
2139 {
2140     float x,y,z;
2141
2142         x=v1[0]-v2[0];
2143         y=v1[1]-v2[1];
2144         z=v1[2]-v2[2];
2145
2146         return ((x*x + y*y + z*z) < (limit*limit));
2147 }
2148
2149 int VecCompare( float *v1, float *v2, float limit)
2150 {
2151         if( fabs(v1[0]-v2[0])<limit )
2152                 if( fabs(v1[1]-v2[1])<limit )
2153                         if( fabs(v1[2]-v2[2])<limit ) return 1;
2154         return 0;
2155 }
2156
2157 int VecEqual(float *v1, float *v2)
2158 {
2159         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
2160 }
2161
2162 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
2163 {
2164         float n1[3],n2[3];
2165
2166         n1[0]= (float)(v1[0]-v2[0]);
2167         n2[0]= (float)(v2[0]-v3[0]);
2168         n1[1]= (float)(v1[1]-v2[1]);
2169         n2[1]= (float)(v2[1]-v3[1]);
2170         n1[2]= (float)(v1[2]-v2[2]);
2171         n2[2]= (float)(v2[2]-v3[2]);
2172         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2173         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2174         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2175         Normalize(n);
2176 }
2177
2178 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
2179 {
2180         float n1[3],n2[3];
2181
2182         n1[0]= (float)(v1[0]-v2[0]);
2183         n2[0]= (float)(v2[0]-v3[0]);
2184         n1[1]= (float)(v1[1]-v2[1]);
2185         n2[1]= (float)(v2[1]-v3[1]);
2186         n1[2]= (float)(v1[2]-v2[2]);
2187         n2[2]= (float)(v2[2]-v3[2]);
2188         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2189         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2190         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2191         Normalize(n);
2192 }
2193
2194 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
2195 {
2196         float n1[3],n2[3];
2197
2198         n1[0]= v1[0]-v2[0];
2199         n2[0]= v2[0]-v3[0];
2200         n1[1]= v1[1]-v2[1];
2201         n2[1]= v2[1]-v3[1];
2202         n1[2]= v1[2]-v2[2];
2203         n2[2]= v2[2]-v3[2];
2204         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2205         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2206         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2207         return Normalize(n);
2208 }
2209
2210 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
2211 {
2212         /* real cross! */
2213         float n1[3],n2[3];
2214
2215         n1[0]= v1[0]-v3[0];
2216         n1[1]= v1[1]-v3[1];
2217         n1[2]= v1[2]-v3[2];
2218
2219         n2[0]= v2[0]-v4[0];
2220         n2[1]= v2[1]-v4[1];
2221         n2[2]= v2[2]-v4[2];
2222
2223         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2224         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2225         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2226
2227         return Normalize(n);
2228 }
2229
2230
2231 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
2232 {
2233
2234         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
2235         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
2236         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
2237 }
2238
2239 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
2240 {
2241
2242         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
2243         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
2244         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
2245 }
2246
2247 float Sqrt3f(float f)
2248 {
2249         if(f==0.0) return 0;
2250         if(f<0) return (float)(-exp(log(-f)/3));
2251         else return (float)(exp(log(f)/3));
2252 }
2253
2254 double Sqrt3d(double d)
2255 {
2256         if(d==0.0) return 0;
2257         if(d<0) return -exp(log(-d)/3);
2258         else return exp(log(d)/3);
2259 }
2260
2261 /* distance v1 to line v2-v3 */
2262 /* using Hesse formula, NO LINE PIECE! */
2263 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
2264         float a[2],deler;
2265
2266         a[0]= v2[1]-v3[1];
2267         a[1]= v3[0]-v2[0];
2268         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
2269         if(deler== 0.0f) return 0;
2270
2271         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
2272
2273 }
2274
2275 /* distance v1 to line-piece v2-v3 */
2276 float PdistVL2Dfl( float *v1, float *v2, float *v3) 
2277 {
2278         float labda, rc[2], pt[2], len;
2279         
2280         rc[0]= v3[0]-v2[0];
2281         rc[1]= v3[1]-v2[1];
2282         len= rc[0]*rc[0]+ rc[1]*rc[1];
2283         if(len==0.0) {
2284                 rc[0]= v1[0]-v2[0];
2285                 rc[1]= v1[1]-v2[1];
2286                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
2287         }
2288         
2289         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
2290         if(labda<=0.0) {
2291                 pt[0]= v2[0];
2292                 pt[1]= v2[1];
2293         }
2294         else if(labda>=1.0) {
2295                 pt[0]= v3[0];
2296                 pt[1]= v3[1];
2297         }
2298         else {
2299                 pt[0]= labda*rc[0]+v2[0];
2300                 pt[1]= labda*rc[1]+v2[1];
2301         }
2302
2303         rc[0]= pt[0]-v1[0];
2304         rc[1]= pt[1]-v1[1];
2305         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
2306 }
2307
2308 float AreaF2Dfl( float *v1, float *v2, float *v3)
2309 {
2310         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
2311 }
2312
2313
2314 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
2315 {
2316         float len, vec1[3], vec2[3], n[3];
2317
2318         VecSubf(vec1, v2, v1);
2319         VecSubf(vec2, v4, v1);
2320         Crossf(n, vec1, vec2);
2321         len= Normalize(n);
2322
2323         VecSubf(vec1, v4, v3);
2324         VecSubf(vec2, v2, v3);
2325         Crossf(n, vec1, vec2);
2326         len+= Normalize(n);
2327
2328         return (len/2.0f);
2329 }
2330
2331 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
2332 {
2333         float len, vec1[3], vec2[3], n[3];
2334
2335         VecSubf(vec1, v3, v2);
2336         VecSubf(vec2, v1, v2);
2337         Crossf(n, vec1, vec2);
2338         len= Normalize(n);
2339
2340         return (len/2.0f);
2341 }
2342
2343 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
2344 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
2345
2346
2347 float AreaPoly3Dfl(int nr, float *verts, float *normal)
2348 {
2349         float x, y, z, area, max;
2350         float *cur, *prev;
2351         int a, px=0, py=1;
2352
2353         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
2354         x= (float)fabs(normal[0]);
2355         y= (float)fabs(normal[1]);
2356         z= (float)fabs(normal[2]);
2357         max = MAX3(x, y, z);
2358         if(max==y) py=2;
2359         else if(max==x) {
2360                 px=1; 
2361                 py= 2;
2362         }
2363
2364         /* The Trapezium Area Rule */
2365         prev= verts+3*(nr-1);
2366         cur= verts;
2367         area= 0;
2368         for(a=0; a<nr; a++) {
2369                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
2370                 prev= cur;
2371                 cur+=3;
2372         }
2373
2374         return (float)fabs(0.5*area/max);
2375 }
2376
2377 /* intersect Line-Line, shorts */
2378 short IsectLL2Ds(short *v1, short *v2, short *v3, short *v4)
2379 {
2380         /* return:
2381         -1: colliniar
2382          0: no intersection of segments
2383          1: exact intersection of segments
2384          2: cross-intersection of segments
2385         */
2386         float div, labda, mu;
2387         
2388         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2389         if(div==0.0) return -1;
2390         
2391         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2392         
2393         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2394         
2395         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2396                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2397                 return 2;
2398         }
2399         return 0;
2400 }
2401
2402 /* intersect Line-Line, floats */
2403 short IsectLL2Df(float *v1, float *v2, float *v3, float *v4)
2404 {
2405         /* return:
2406         -1: colliniar
2407 0: no intersection of segments
2408 1: exact intersection of segments
2409 2: cross-intersection of segments
2410         */
2411         float div, labda, mu;
2412         
2413         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2414         if(div==0.0) return -1;
2415         
2416         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2417         
2418         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2419         
2420         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2421                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2422                 return 2;
2423         }
2424         return 0;
2425 }
2426
2427 /*
2428 -1: colliniar
2429  1: intersection
2430
2431 */
2432 short IsectLLPt2Df(float x0,float y0,float x1,float y1,
2433                                          float x2,float y2,float x3,float y3, float *xi,float *yi)
2434
2435 {
2436         /*
2437         this function computes the intersection of the sent lines
2438         and returns the intersection point, note that the function assumes
2439         the lines intersect. the function can handle vertical as well
2440         as horizontal lines. note the function isn't very clever, it simply
2441         applies the math, but we don't need speed since this is a
2442         pre-processing step
2443         */
2444         float c1,c2, // constants of linear equations
2445         det_inv,  // the inverse of the determinant of the coefficient
2446         m1,m2;    // the slopes of each line
2447         /*
2448         compute slopes, note the cludge for infinity, however, this will
2449         be close enough
2450         */
2451         if ( fabs( x1-x0 ) > 0.000001 )
2452                 m1 = ( y1-y0 ) / ( x1-x0 );
2453         else
2454                 return -1; /*m1 = ( float ) 1e+10;*/   // close enough to infinity
2455
2456         if ( fabs( x3-x2 ) > 0.000001 )
2457                 m2 = ( y3-y2 ) / ( x3-x2 );
2458         else
2459                 return -1; /*m2 = ( float ) 1e+10;*/   // close enough to infinity
2460
2461         if (fabs(m1-m2) < 0.000001)
2462                 return -1; /* paralelle lines */
2463         
2464 // compute constants
2465
2466         c1 = ( y0-m1*x0 );
2467         c2 = ( y2-m2*x2 );
2468
2469 // compute the inverse of the determinate
2470
2471         det_inv = 1.0f / ( -m1 + m2 );
2472
2473 // use Kramers rule to compute xi and yi
2474
2475         *xi= ( ( -c2 + c1 ) *det_inv );
2476         *yi= ( ( m2*c1 - m1*c2 ) *det_inv );
2477         
2478         return 1; 
2479 } // end Intersect_Lines
2480
2481 #define SIDE_OF_LINE(pa,pb,pp)  ((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
2482 #define ISECT_EPSILON 1e-6
2483
2484 /* point in tri */
2485 int IsectPT2Df(float pt[2], float v1[2], float v2[2], float v3[2])
2486 {
2487         if ((SIDE_OF_LINE(v1,v2,pt)>=-ISECT_EPSILON) &&
2488                 (SIDE_OF_LINE(v2,v3,pt)>=-ISECT_EPSILON) &&
2489                 (SIDE_OF_LINE(v3,v1,pt)>=-ISECT_EPSILON))
2490                 return 1;
2491         else {
2492                 return 0;
2493         }
2494 }
2495 /* point in quad - only convex quads */
2496 int IsectPQ2Df(float pt[2], float v1[2], float v2[2], float v3[2], float v4[2])
2497 {
2498         if ((SIDE_OF_LINE(v1,v2,pt)>=-ISECT_EPSILON) &&
2499                 (SIDE_OF_LINE(v2,v3,pt)>=-ISECT_EPSILON) &&
2500                 (SIDE_OF_LINE(v3,v4,pt)>=-ISECT_EPSILON) &&
2501                 (SIDE_OF_LINE(v4,v1,pt)>=-ISECT_EPSILON))
2502                 return 1;
2503         else
2504                 return 0;
2505 }
2506
2507
2508
2509                         /* copied from Geometry.c - todo - move to arithb.c or some other generic place we can reuse */
2510 #define SIDE_OF_LINE(pa,pb,pp)  ((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
2511 #define POINT_IN_TRI(p0,p1,p2,p3)       ((SIDE_OF_LINE(p1,p2,p0)>=0) && (SIDE_OF_LINE(p2,p3,p0)>=0) && (SIDE_OF_LINE(p3,p1,p0)>=0))
2512
2513 /**
2514  * 
2515  * @param min 
2516  * @param max 
2517  * @param vec 
2518  */
2519 void MinMax3(float *min, float *max, float *vec)
2520 {
2521         if(min[0]>vec[0]) min[0]= vec[0];
2522         if(min[1]>vec[1]) min[1]= vec[1];
2523         if(min[2]>vec[2]) min[2]= vec[2];
2524
2525         if(max[0]<vec[0]) max[0]= vec[0];
2526         if(max[1]<vec[1]) max[1]= vec[1];
2527         if(max[2]<vec[2]) max[2]= vec[2];
2528 }
2529
2530 static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
2531 {
2532         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
2533 }
2534
2535 static int BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
2536 {
2537         float xn, yn, zn, a1, a2, a3, asum;
2538         short i, j;
2539
2540         /* find best projection of face XY, XZ or YZ: barycentric weights of
2541            the 2d projected coords are the same and faster to compute */
2542         xn= fabs(n[0]);
2543         yn= fabs(n[1]);
2544         zn= fabs(n[2]);
2545         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
2546         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
2547         else {i= 1; j= 2;} 
2548
2549         a1= TriSignedArea(v2, v3, co, i, j);
2550         a2= TriSignedArea(v3, v1, co, i, j);
2551         a3= TriSignedArea(v1, v2, co, i, j);
2552
2553         asum= a1 + a2 + a3;
2554
2555         if (fabs(asum) < FLT_EPSILON) {
2556                 /* zero area triangle */
2557                 w[0]= w[1]= w[2]= 1.0f/3.0f;
2558                 return 1;
2559         }
2560
2561         asum= 1.0f/asum;
2562         w[0]= a1*asum;
2563         w[1]= a2*asum;
2564         w[2]= a3*asum;
2565
2566         return 0;
2567 }
2568
2569 void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
2570 {
2571         float w2[3];
2572
2573         w[0]= w[1]= w[2]= w[3]= 0.0f;
2574
2575         /* first check for exact match */
2576         if(VecEqual(co, v1))
2577                 w[0]= 1.0f;
2578         else if(VecEqual(co, v2))
2579                 w[1]= 1.0f;
2580         else if(VecEqual(co, v3))
2581                 w[2]= 1.0f;
2582         else if(v4 && VecEqual(co, v4))
2583                 w[3]= 1.0f;
2584         else {
2585                 /* otherwise compute barycentric interpolation weights */
2586                 float n1[3], n2[3], n[3];
2587                 int degenerate;
2588
2589                 VecSubf(n1, v1, v3);
2590                 if (v4) {
2591                         VecSubf(n2, v2, v4);
2592                 }
2593                 else {
2594                         VecSubf(n2, v2, v3);
2595                 }
2596                 Crossf(n, n1, n2);
2597
2598                 /* OpenGL seems to split this way, so we do too */
2599                 if (v4) {
2600                         degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
2601                         SWAP(float, w[2], w[3]);
2602
2603                         if(degenerate || (w[0] < 0.0f)) {
2604                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2605                                    so we interpolate using the other triangle */
2606                                 degenerate= BarycentricWeights(v2, v3, v4, co, n, w2);
2607
2608                                 if(!degenerate) {
2609                                         w[0]= 0.0f;
2610                                         w[1]= w2[0];
2611                                         w[2]= w2[1];
2612                                         w[3]= w2[2];
2613                                 }
2614                         }
2615                 }
2616                 else
2617                         BarycentricWeights(v1, v2, v3, co, n, w);
2618         }
2619 }
2620
2621 /* Mean value weights - smooth interpolation weights for polygons with
2622  * more than 3 vertices */
2623 static float MeanValueHalfTan(float *v1, float *v2, float *v3)
2624 {
2625         float d2[3], d3[3], cross[3], area, dot, len;
2626
2627         VecSubf(d2, v2, v1);
2628         VecSubf(d3, v3, v1);
2629         Crossf(cross, d2, d3);
2630
2631         area= VecLength(cross);
2632         dot= Inpf(d2, d3);
2633         len= VecLength(d2)*VecLength(d3);
2634
2635         if(area == 0.0f)
2636                 return 0.0f;
2637         else
2638                 return (len - dot)/area;
2639 }
2640
2641 void MeanValueWeights(float v[][3], int n, float *co, float *w)
2642 {
2643         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
2644         int i;
2645
2646         totweight= 0.0f;
2647
2648         for(i=0; i<n; i++) {
2649                 vmid= v[i];
2650                 vprev= (i == 0)? v[n-1]: v[i-1];
2651                 vnext= (i == n-1)? v[0]: v[i+1];
2652
2653                 t1= MeanValueHalfTan(co, vprev, vmid);
2654                 t2= MeanValueHalfTan(co, vmid, vnext);
2655
2656                 len= VecLenf(co, vmid);
2657                 w[i]= (t1+t2)/len;
2658                 totweight += w[i];
2659         }
2660
2661         if(totweight != 0.0f)
2662                 for(i=0; i<n; i++)
2663                         w[i] /= totweight;
2664 }
2665
2666
2667 /* ************ EULER *************** */
2668
2669 void EulToMat3( float *eul, float mat[][3])
2670 {
2671         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2672         
2673         ci = cos(eul[0]); 
2674         cj = cos(eul[1]); 
2675         ch = cos(eul[2]);
2676         si = sin(eul[0]); 
2677         sj = sin(eul[1]); 
2678         sh = sin(eul[2]);
2679         cc = ci*ch; 
2680         cs = ci*sh; 
2681         sc = si*ch; 
2682         ss = si*sh;
2683
2684         mat[0][0] = (float)(cj*ch); 
2685         mat[1][0] = (float)(sj*sc-cs); 
2686         mat[2][0] = (float)(sj*cc+ss);
2687         mat[0][1] = (float)(cj*sh); 
2688         mat[1][1] = (float)(sj*ss+cc); 
2689         mat[2][1] = (float)(sj*cs-sc);
2690         mat[0][2] = (float)-sj;  
2691         mat[1][2] = (float)(cj*si);    
2692         mat[2][2] = (float)(cj*ci);
2693
2694 }
2695
2696 void EulToMat4( float *eul,float mat[][4])
2697 {
2698         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2699         
2700         ci = cos(eul[0]); 
2701         cj = cos(eul[1]); 
2702         ch = cos(eul[2]);
2703         si = sin(eul[0]); 
2704         sj = sin(eul[1]); 
2705         sh = sin(eul[2]);
2706         cc = ci*ch; 
2707         cs = ci*sh; 
2708         sc = si*ch; 
2709         ss = si*sh;
2710
2711         mat[0][0] = (float)(cj*ch); 
2712         mat[1][0] = (float)(sj*sc-cs); 
2713         mat[2][0] = (float)(sj*cc+ss);
2714         mat[0][1] = (float)(cj*sh); 
2715         mat[1][1] = (float)(sj*ss+cc); 
2716         mat[2][1] = (float)(sj*cs-sc);
2717         mat[0][2] = (float)-sj;  
2718         mat[1][2] = (float)(cj*si);    
2719         mat[2][2] = (float)(cj*ci);
2720
2721
2722         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2723         mat[3][3]= 1.0f;
2724 }
2725
2726 /* returns two euler calculation methods, so we can pick the best */
2727 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
2728 {
2729         float cy, quat[4], mat[3][3];
2730         
2731         Mat3ToQuat(tmat, quat);
2732         QuatToMat3(quat, mat);
2733         Mat3CpyMat3(mat, tmat);
2734         Mat3Ortho(mat);
2735         
2736         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2737         
2738         if (cy > 16.0*FLT_EPSILON) {
2739                 
2740                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2741                 eul1[1] = (float)atan2(-mat[0][2], cy);
2742                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2743                 
2744                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2745                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2746                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2747                 
2748         } else {
2749                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
2750                 eul1[1] = (float)atan2(-mat[0][2], cy);
2751                 eul1[2] = 0.0f;
2752                 
2753                 VecCopyf(eul2, eul1);
2754         }
2755 }
2756
2757 void Mat3ToEul(float tmat[][3], float *eul)
2758 {
2759         float eul1[3], eul2[3];
2760         
2761         mat3_to_eul2(tmat, eul1, eul2);
2762                 
2763         /* return best, which is just the one with lowest values it in */
2764         if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2765                 VecCopyf(eul, eul2);
2766         }
2767         else {
2768                 VecCopyf(eul, eul1);
2769         }
2770 }
2771
2772 void Mat4ToEul(float tmat[][4], float *eul)
2773 {
2774         float tempMat[3][3];
2775
2776         Mat3CpyMat4 (tempMat, tmat);
2777         Mat3Ortho(tempMat);
2778         Mat3ToEul(tempMat, eul);
2779 }
2780
2781 void QuatToEul( float *quat, float *eul)
2782 {
2783         float mat[3][3];
2784         
2785         QuatToMat3(quat, mat);
2786         Mat3ToEul(mat, eul);
2787 }
2788
2789
2790 void EulToQuat( float *eul, float *quat)
2791 {
2792     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2793  
2794     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2795     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2796     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2797     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2798         
2799         quat[0] = cj*cc + sj*ss;
2800         quat[1] = cj*sc - sj*cs;
2801         quat[2] = cj*ss + sj*cc;
2802         quat[3] = cj*cs - sj*sc;
2803 }
2804
2805 void VecRotToMat3( float *vec, float phi, float mat[][3])
2806 {
2807         /* rotation of phi radials around vec */
2808         float vx, vx2, vy, vy2, vz, vz2, co, si;
2809         
2810         vx= vec[0];
2811         vy= vec[1];
2812         vz= vec[2];
2813         vx2= vx*vx;
2814         vy2= vy*vy;
2815         vz2= vz*vz;
2816         co= (float)cos(phi);
2817         si= (float)sin(phi);
2818         
2819         mat[0][0]= vx2+co*(1.0f-vx2);
2820         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2821         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2822         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2823         mat[1][1]= vy2+co*(1.0f-vy2);
2824         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2825         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2826         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2827         mat[2][2]= vz2+co*(1.0f-vz2);
2828         
2829 }
2830
2831 void VecRotToMat4( float *vec, float phi, float mat[][4])
2832 {
2833         float tmat[3][3];
2834         
2835         VecRotToMat3(vec, phi, tmat);
2836         Mat4One(mat);
2837         Mat4CpyMat3(mat, tmat);
2838 }
2839
2840 void VecRotToQuat( float *vec, float phi, float *quat)
2841 {
2842         /* rotation of phi radials around vec */
2843         float si;
2844
2845         quat[1]= vec[0];
2846         quat[2]= vec[1];
2847         quat[3]= vec[2];
2848         
2849         if( Normalize(quat+1) == 0.0) {
2850                 QuatOne(quat);
2851         }
2852         else {
2853                 quat[0]= (float)cos( phi/2.0 );
2854                 si= (float)sin( phi/2.0 );
2855                 quat[1] *= si;
2856                 quat[2] *= si;
2857                 quat[3] *= si;
2858         }
2859 }
2860
2861 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
2862    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
2863    this would return the angle at the elbow */
2864 float VecAngle3(float *v1, float *v2, float *v3)
2865 {
2866         float vec1[3], vec2[3];
2867
2868         VecSubf(vec1, v2, v1);
2869         VecSubf(vec2, v2, v3);
2870         Normalize(vec1);
2871         Normalize(vec2);
2872
2873         return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
2874 }
2875
2876 float VecAngle3_2D(float *v1, float *v2, float *v3)
2877 {
2878         float vec1[2], vec2[2];
2879
2880         vec1[0] = v2[0]-v1[0];
2881         vec1[1] = v2[1]-v1[1];
2882         
2883         vec2[0] = v2[0]-v3[0];
2884         vec2[1] = v2[1]-v3[1];
2885         
2886         Normalize2(vec1);
2887         Normalize2(vec2);
2888
2889         return NormalizedVecAngle2_2D(vec1, vec2) * 180.0/M_PI;
2890 }
2891
2892 /* Return the shortest angle in degrees between the 2 vectors */
2893 float VecAngle2(float *v1, float *v2)
2894 {
2895         float vec1[3], vec2[3];
2896
2897         VecCopyf(vec1, v1);
2898         VecCopyf(vec2, v2);
2899         Normalize(vec1);
2900         Normalize(vec2);
2901
2902         return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
2903 }
2904
2905 float NormalizedVecAngle2(float *v1, float *v2)
2906 {
2907         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
2908         if (Inpf(v1, v2) < 0.0f) {
2909                 float vec[3];
2910                 
2911                 vec[0]= -v2[0];
2912                 vec[1]= -v2[1];
2913                 vec[2]= -v2[2];
2914
2915                 return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
2916         }
2917         else
2918                 return 2.0f*saasin(VecLenf(v2, v1)/2.0);
2919 }
2920
2921 float NormalizedVecAngle2_2D(float *v1, float *v2)
2922 {
2923         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
2924         if (Inp2f(v1, v2) < 0.0f) {
2925                 float vec[2];
2926                 
2927                 vec[0]= -v2[0];
2928                 vec[1]= -v2[1];
2929
2930                 return (float)M_PI - 2.0f*saasin(Vec2Lenf(vec, v1)/2.0f);
2931         }
2932         else
2933                 return 2.0f*saasin(Vec2Lenf(v2, v1)/2.0);
2934 }
2935
2936 void euler_rot(float *beul, float ang, char axis)
2937 {
2938         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2939         
2940         eul[0]= eul[1]= eul[2]= 0.0;
2941         if(axis=='x') eul[0]= ang;
2942         else if(axis=='y') eul[1]= ang;
2943         else eul[2]= ang;
2944         
2945         EulToMat3(eul, mat1);
2946         EulToMat3(beul, mat2);
2947         
2948         Mat3MulMat3(totmat, mat2, mat1);
2949         
2950         Mat3ToEul(totmat, beul);
2951         
2952 }
2953
2954 /* exported to transform.c */
2955 void compatible_eul(float *eul, float *oldrot)
2956 {
2957         float dx, dy, dz;
2958         
2959         /* correct differences of about 360 degrees first */
2960         
2961         dx= eul[0] - oldrot[0];
2962         dy= eul[1] - oldrot[1];
2963         dz= eul[2] - oldrot[2];
2964         
2965         while( fabs(dx) > 5.1) {
2966                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2967                 dx= eul[0] - oldrot[0];
2968         }
2969         while( fabs(dy) > 5.1) {
2970                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2971                 dy= eul[1] - oldrot[1];
2972         }
2973         while( fabs(dz) > 5.1 ) {
2974                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2975                 dz= eul[2] - oldrot[2];
2976         }
2977         
2978         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
2979         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
2980                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
2981         }
2982         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
2983                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
2984         }
2985         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
2986                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
2987         }
2988         
2989         /* the method below was there from ancient days... but why! probably because the code sucks :)
2990                 */
2991 #if 0   
2992         /* calc again */
2993         dx= eul[0] - oldrot[0];
2994         dy= eul[1] - oldrot[1];
2995         dz= eul[2] - oldrot[2];
2996         
2997         /* special case, tested for x-z  */
2998         
2999         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
3000                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3001                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
3002                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3003                 
3004         }
3005         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
3006                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3007                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3008                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
3009         }
3010         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
3011                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
3012                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3013                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3014         }
3015 #endif  
3016 }
3017
3018 /* uses 2 methods to retrieve eulers, and picks the closest */
3019 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
3020 {
3021         float eul1[3], eul2[3];
3022         float d1, d2;
3023         
3024         mat3_to_eul2(mat, eul1, eul2);
3025         
3026         compatible_eul(eul1, oldrot);
3027         compatible_eul(eul2, oldrot);
3028         
3029         d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
3030         d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
3031         
3032         /* return best, which is just the one with lowest difference */
3033         if( d1 > d2) {
3034                 VecCopyf(eul, eul2);
3035         }
3036         else {
3037                 VecCopyf(eul, eul1);
3038         }
3039         
3040 }
3041
3042 /* ******************************************** */
3043
3044 void SizeToMat3( float *size, float mat[][3])
3045 {
3046         mat[0][0]= size[0];
3047         mat[0][1]= 0.0;
3048         mat[0][2]= 0.0;
3049         mat[1][1]= size[1];
3050         mat[1][0]= 0.0;
3051         mat[1][2]= 0.0;
3052         mat[2][2]= size[2];
3053         mat[2][1]= 0.0;
3054         mat[2][0]= 0.0;
3055 }
3056
3057 void SizeToMat4( float *size, float mat[][4])
3058 {
3059         float tmat[3][3];
3060         
3061         SizeToMat3(size, tmat);
3062         Mat4One(mat);
3063         Mat4CpyMat3(mat, tmat);
3064 }
3065
3066 void Mat3ToSize( float mat[][3], float *size)
3067 {
3068         size[0]= VecLength(mat[0]);
3069         size[1]= VecLength(mat[1]);
3070         size[2]= VecLength(mat[2]);
3071 }
3072
3073 void Mat4ToSize( float mat[][4], float *size)
3074 {
3075         size[0]= VecLength(mat[0]);
3076         size[1]= VecLength(mat[1]);
3077         size[2]= VecLength(mat[2]);
3078 }
3079
3080 /* this gets the average scale of a matrix, only use when your scaling
3081  * data that has no idea of scale axis, examples are bone-envelope-radius
3082  * and curve radius */
3083 float Mat3ToScalef(float mat[][3])
3084 {
3085         /* unit length vector */
3086         float unit_vec[3] = {0.577350269189626, 0.577350269189626, 0.577350269189626};
3087         Mat3MulVecfl(mat, unit_vec);
3088         return VecLength(unit_vec);
3089 }
3090
3091 float Mat4ToScalef(float mat[][4])
3092 {
3093         float tmat[3][3];
3094         Mat3CpyMat4(tmat, mat);
3095         return Mat3ToScalef(tmat);
3096 }
3097
3098
3099 /* ************* SPECIALS ******************* */
3100
3101 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
3102 {
3103         /* imaginary x-axis, y-axis triangle is being rotated */
3104         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
3105         
3106         /* move z-axis to face-normal */
3107         CalcNormFloat(v1, v2, v3, vec);
3108
3109         n[0]= vec[1];
3110         n[1]= -vec[0];
3111         n[2]= 0.0;
3112         Normalize(n);
3113         
3114         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
3115         
3116         angle= -0.5f*saacos(vec[2]);
3117         co= (float)cos(angle);
3118         si= (float)sin(angle);
3119         q1[0]= co;
3120         q1[1]= n[0]*si;
3121         q1[2]= n[1]*si;
3122         q1[3]= 0.0f;
3123         
3124         /* rotate back line v1-v2 */
3125         QuatToMat3(q1, mat);
3126         Mat3Inv(imat, mat);
3127         VecSubf(vec, v2, v1);
3128         Mat3MulVecfl(imat, vec);
3129
3130         /* what angle has this line with x-axis? */
3131         vec[2]= 0.0;
3132         Normalize(vec);
3133
3134         angle= (float)(0.5*atan2(vec[1], vec[0]));
3135         co= (float)cos(angle);
3136         si= (float)sin(angle);
3137         q2[0]= co;
3138         q2[1]= 0.0f;
3139         q2[2]= 0.0f;
3140         q2[3]= si;
3141         
3142         QuatMul(quat, q1, q2);
3143 }
3144
3145 void MinMaxRGB(short c[])
3146 {
3147         if(c[0]>255) c[0]=255;
3148         else if(c[0]<0) c[0]=0;
3149         if(c[1]>255) c[1]=255;
3150         else if(c[1]<0) c[1]=0;
3151         if(c[2]>255) c[2]=255;
3152         else if(c[2]<0) c[2]=0;
3153 }
3154
3155 float Vec2Lenf(float *v1, float *v2)
3156 {
3157         float x, y;
3158
3159         x = v1[0]-v2[0];
3160         y = v1[1]-v2[1];
3161         return (float)sqrt(x*x+y*y);
3162 }
3163
3164 float Vec2Length(float *v)
3165 {
3166         return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
3167 }
3168
3169 void Vec2Mulf(float *v1, float f)
3170 {
3171         v1[0]*= f;
3172         v1[1]*= f;
3173 }
3174
3175 void Vec2Addf(float *v, float *v1, float *v2)
3176 {
3177         v[0]= v1[0]+ v2[0];
3178         v[1]= v1[1]+ v2[1];
3179 }
3180
3181 void Vec2Subf(float *v, float *v1, float *v2)
3182 {
3183         v[0]= v1[0]- v2[0];
3184         v[1]= v1[1]- v2[1];
3185 }
3186
3187 void Vec2Copyf(float *v1, float *v2)
3188 {
3189         v1[0]= v2[0];
3190         v1[1]= v2[1];
3191 }
3192
3193 float Inp2f(float *v1, float *v2)
3194 {
3195         return v1[0]*v2[0]+v1[1]*v2[1];
3196 }
3197
3198 float Normalize2(float *n)
3199 {
3200         float d;
3201         
3202         d= n[0]*n[0]+n[1]*n[1];
3203
3204         if(d>1.0e-35F) {
3205                 d= (float)sqrt(d);
3206
3207                 n[0]/=d; 
3208                 n[1]/=d; 
3209         } else {
3210                 n[0]=n[1]= 0.0;
3211                 d= 0.0;
3212         }
3213         return d;
3214 }
3215
3216 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
3217 {
3218         int i;
3219         float f, p, q, t;
3220
3221         h *= 360.0f;
3222         
3223         if(s==0.0) {
3224                 *r = v;
3225                 *g = v;
3226                 *b = v;
3227         }
3228         else {
3229                 if(h==360) h = 0;
3230                 
3231                 h /= 60;
3232                 i = (int)floor(h);
3233                 f = h - i;
3234                 p = v*(1.0f-s);
3235                 q = v*(1.0f-(s*f));
3236                 t = v*(1.0f-(s*(1.0f-f)));
3237                 
3238                 switch (i) {
3239                 case 0 :
3240                         *r = v;
3241                         *g = t;
3242                         *b = p;
3243                         break;
3244                 case 1 :
3245                         *r = q;
3246                         *g = v;
3247                         *b = p;
3248                         break;
3249                 case 2 :
3250                         *r = p;
3251                         *g = v;
3252                         *b = t;
3253                         break;
3254                 case 3 :
3255                         *r = p;
3256                         *g = q;
3257                         *b = v;
3258                         break;
3259                 case 4 :
3260                         *r = t;
3261                         *g = p;
3262                         *b = v;
3263                         break;
3264                 case 5 :
3265                         *r = v;
3266                         *g = p;
3267                         *b = q;
3268                         break;
3269                 }
3270         }
3271 }
3272
3273 void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
3274 {
3275         float y, u, v;
3276         y= 0.299*r + 0.587*g + 0.114*b;
3277         u=-0.147*r - 0.289*g + 0.436*b;
3278         v= 0.615*r - 0.515*g - 0.100*b;
3279         
3280         *ly=y;
3281         *lu=u;
3282         *lv=v;
3283 }
3284
3285 void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
3286 {
3287         float r, g, b;
3288         r=y+1.140*v;
3289         g=y-0.394*u - 0.581*v;
3290         b=y+2.032*u;
3291         
3292         *lr=r;
3293         *lg=g;
3294         *lb=b;
3295 }
3296
3297 void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
3298 {
3299         float sr,sg, sb;
3300         float y, cr, cb;
3301         
3302         sr=255.0*r;
3303         sg=255.0*g;
3304         sb=255.0*b;
3305         
3306         
3307         y=(0.257*sr)+(0.504*sg)+(0.098*sb)+16.0;
3308         cb=(-0.148*sr)-(0.291*sg)+(0.439*sb)+128.0;
3309         cr=(0.439*sr)-(0.368*sg)-(0.071*sb)+128.0;
3310         
3311         *ly=y;
3312         *lcb=cb;
3313         *lcr=cr;
3314 }
3315
3316 void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
3317 {
3318         float r,g,b;
3319         
3320         r=1.164*(y-16)+1.596*(cr-128);
3321         g=1.164*(y-16)-0.813*(cr-128)-0.392*(cb-128);
3322         b=1.164*(y-16)+2.017*(cb-128);
3323         
3324         *lr=r/255.0;
3325         *lg=g/255.0;
3326         *lb=b/255.0;
3327 }
3328
3329 void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
3330 {
3331         unsigned int ri, gi, bi;
3332         
3333         if (hexcol[0] == '#') hexcol++;
3334         
3335         if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
3336                 *r = ri / 255.0;
3337                 *g = gi / 255.0;                
3338                 *b = bi / 255.0;
3339         }
3340 }
3341
3342 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
3343 {
3344         float h, s, v;
3345         float cmax, cmin, cdelta;
3346         float rc, gc, bc;
3347
3348         cmax = r;
3349         cmin = r;
3350         cmax = (g>cmax ? g:cmax);
3351         cmin = (g<cmin ? g:cmin);
3352         cmax = (b>cmax ? b:cmax);
3353         cmin = (b<cmin ? b:cmin);
3354
3355         v = cmax;               /* value */
3356         if (cmax!=0.0)
3357                 s = (cmax - cmin)/cmax;
3358         else {
3359                 s = 0.0;
3360                 h = 0.0;
3361         }
3362         if (s == 0.0)
3363                 h = -1.0;
3364         else {
3365                 cdelta = cmax-cmin;
3366                 rc = (cmax-r)/cdelta;
3367                 gc = (cmax-g)/cdelta;
3368                 bc = (cmax-b)/cdelta;
3369                 if (r==cmax)
3370                         h = bc-gc;
3371                 else
3372                         if (g==cmax)
3373                                 h = 2.0f+rc-bc;
3374                         else
3375                                 h = 4.0f+gc-rc;
3376                 h = h*60.0f;
3377                 if (h<0.0f)
3378                         h += 360.0f;
3379         }
3380         
3381         *ls = s;
3382         *lh = h/360.0f;
3383         if( *lh < 0.0) *lh= 0.0;
3384         *lv = v;
3385 }
3386
3387
3388 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
3389    for that reason it is sensitive for endianness... with this function it works correctly
3390 */
3391
3392 unsigned int hsv_to_cpack(float h, float s, float v)
3393 {
3394         short r, g, b;
3395         float rf, gf, bf;
3396         unsigned int col;
3397         
3398         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
3399         
3400         r= (short)(rf*255.0f);
3401         g= (short)(gf*255.0f);
3402         b= (short)(bf*255.0f);
3403         
3404         col= ( r + (g*256) + (b*256*256) );
3405         return col;
3406 }
3407
3408
3409 unsigned int rgb_to_cpack(float r, float g, float b)
3410 {
3411         int ir, ig, ib;
3412         
3413         ir= (int)floor(255.0*r);
3414         if(ir<0) ir= 0; else if(ir>255) ir= 255;
3415         ig= (int)floor(255.0*g);
3416         if(ig<0) ig= 0; else if(ig>255) ig= 255;
3417         ib= (int)floor(255.0*b);
3418         if(ib<0) ib= 0; else if(ib>255) ib= 255;
3419         
3420         return (ir+ (ig*256) + (ib*256*256));
3421 }
3422
3423 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
3424 {
3425         
3426         *r= (float)((col)&0xFF);
3427         *r /= 255.0f;
3428
3429         *g= (float)(((col)>>8)&0xFF);
3430         *g /= 255.0f;
3431
3432         *b= (float)(((col)>>16)&0xFF);
3433         *b /= 255.0f;
3434 }
3435
3436
3437 /* *************** PROJECTIONS ******************* */
3438
3439 void tubemap(float x, float y, float z, float *u, float *v)
3440 {
3441         float len;
3442         
3443         *v = (z + 1.0) / 2.0;
3444         
3445         len= sqrt(x*x+y*y);
3446         if(len>0) {
3447                 *u = (1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0;
3448         } else {
3449                 *v = *u = 0.0f; /* to avoid un-initialized variables */
3450         }
3451 }
3452
3453 /* ------------------------------------------------------------------------- */
3454
3455 void spheremap(float x, float y, float z, float *u, float *v)
3456 {
3457         float len;
3458         
3459         len= sqrt(x*x+y*y+z*z);
3460         if(len>0.0) {
3461                 
3462                 if(x==0.0 && y==0.0) *u= 0.0;   /* othwise domain error */
3463                 else *u = (1.0 - atan2(x,y)/M_PI )/2.0;
3464                 
3465                 z/=len;
3466                 *v = 1.0- saacos(z)/M_PI;
3467         } else {
3468                 *v = *u = 0.0f; /* to avoid un-initialized variables */
3469         }
3470 }
3471
3472 /* ------------------------------------------------------------------------- */
3473
3474 /* *****************  m1 = m2 *****************  */
3475 void cpy_m3_m3(float m1[][3], float m2[][3]) 
3476 {       
3477         memcpy(m1[0], m2[0], 9*sizeof(float));
3478 }
3479
3480 /* *****************  m1 = m2 *****************  */
3481 void cpy_m4_m4(float m1[][4], float m2[][4]) 
3482 {       
3483         memcpy(m1[0], m2[0], 16*sizeof(float));
3484 }
3485
3486 /* ***************** identity matrix *****************  */
3487 void ident_m4(float m[][4])
3488 {
3489         
3490         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
3491         m[0][1]= m[0][2]= m[0][3]= 0.0;
3492         m[1][0]= m[1][2]= m[1][3]= 0.0;
3493         m[2][0]= m[2][1]= m[2][3]= 0.0;
3494         m[3][0]= m[3][1]= m[3][2]= 0.0;
3495 }
3496
3497
3498 /* *****************  m1 = m2 (pre) * m3 (post) ***************** */
3499 void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
3500 {
3501         float m[3][3];
3502         
3503         m[0][0]= m2[0][0]*m3[0][0] + m2[1][0]*m3[0][1] + m2[2][0]*m3[0][2]; 
3504         m[0][1]= m2[0][1]*m3[0][0] + m2[1][1]*m3[0][1] + m2[2][1]*m3[0][2]; 
3505         m[0][2]= m2[0][2]*m3[0][0] + m2[1][2]*m3[0][1] + m2[2][2]*m3[0][2]; 
3506
3507         m[1][0]= m2[0][0]*m3[1][0] + m2[1][0]*m3[1][1] + m2[2][0]*m3[1][2]; 
3508         m[1][1]= m2[0][1]*m3[1][0] + m2[1][1]*m3[1][1] + m2[2][1]*m3[1][2]; 
3509         m[1][2]= m2[0][2]*m3[1][0] + m2[1][2]*m3[1][1] + m2[2][2]*m3[1][2]; 
3510
3511         m[2][0]= m2[0][0]*m3[2][0] + m2[1][0]*m3[2][1] + m2[2][0]*m3[2][2]; 
3512         m[2][1]= m2[0][1]*m3[2][0] + m2[1][1]*m3[2][1] + m2[2][1]*m3[2][2]; 
3513         m[2][2]= m2[0][2]*m3[2][0] + m2[1][2]*m3[2][1] + m2[2][2]*m3[2][2]; 
3514