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