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