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