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