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