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