Use xrange() rather than range() for loop iterations.
[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 Mat3BlendMat3(float out[][3], float dst[][3], float src[][3], float srcweight)
763 {
764         float squat[4], dquat[4], fquat[4];
765         float ssize[3], dsize[3], fsize[4];
766         float rmat[3][3], smat[3][3];
767         
768         Mat3ToQuat(dst, dquat);
769         Mat3ToSize(dst, dsize);
770
771         Mat3ToQuat(src, squat);
772         Mat3ToSize(src, ssize);
773         
774         /* do blending */
775         QuatInterpol(fquat, dquat, squat, srcweight);
776         VecLerpf(fsize, dsize, ssize, srcweight);
777
778         /* compose new matrix */
779         QuatToMat3(fquat, rmat);
780         SizeToMat3(fsize, smat);
781         Mat3MulMat3(out, rmat, smat);
782 }
783
784 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
785 {
786         float squat[4], dquat[4], fquat[4];
787         float ssize[3], dsize[3], fsize[4];
788         float sloc[3], dloc[3], floc[3];
789         
790         Mat4ToQuat(dst, dquat);
791         Mat4ToSize(dst, dsize);
792         VecCopyf(dloc, dst[3]);
793
794         Mat4ToQuat(src, squat);
795         Mat4ToSize(src, ssize);
796         VecCopyf(sloc, src[3]);
797         
798         /* do blending */
799         VecLerpf(floc, dloc, sloc, srcweight);
800         QuatInterpol(fquat, dquat, squat, srcweight);
801         VecLerpf(fsize, dsize, ssize, srcweight);
802
803         /* compose new matrix */
804         LocQuatSizeToMat4(out, floc, fquat, fsize);
805 }
806
807 void Mat4Clr(float *m)
808 {
809         memset(m, 0, 4*4*sizeof(float));
810 }
811
812 void Mat3Clr(float *m)
813 {
814         memset(m, 0, 3*3*sizeof(float));
815 }
816
817 void Mat4One(float m[][4])
818 {
819
820         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
821         m[0][1]= m[0][2]= m[0][3]= 0.0;
822         m[1][0]= m[1][2]= m[1][3]= 0.0;
823         m[2][0]= m[2][1]= m[2][3]= 0.0;
824         m[3][0]= m[3][1]= m[3][2]= 0.0;
825 }
826
827 void Mat3One(float m[][3])
828 {
829
830         m[0][0]= m[1][1]= m[2][2]= 1.0;
831         m[0][1]= m[0][2]= 0.0;
832         m[1][0]= m[1][2]= 0.0;
833         m[2][0]= m[2][1]= 0.0;
834 }
835
836 void Mat4MulVec( float mat[][4], int *vec)
837 {
838         int x,y;
839
840         x=vec[0]; 
841         y=vec[1];
842         vec[0]=(int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]);
843         vec[1]=(int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]);
844         vec[2]=(int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]);
845 }
846
847 void Mat4MulVecfl( 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] + mat[3][0];
854         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
855         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
856 }
857
858 void VecMat4MulVecfl(float *in, float mat[][4], float *vec)
859 {
860         float x,y;
861
862         x=vec[0]; 
863         y=vec[1];
864         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
865         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
866         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
867 }
868
869 void Mat4Mul3Vecfl( float mat[][4], float *vec)
870 {
871         float x,y;
872
873         x= vec[0]; 
874         y= vec[1];
875         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
876         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
877         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
878 }
879
880 void Mat4MulVec3Project(float mat[][4], float *vec)
881 {
882         float w;
883
884         w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
885         Mat4MulVecfl(mat, vec);
886
887         vec[0] /= w;
888         vec[1] /= w;
889         vec[2] /= w;
890 }
891
892 void Mat4MulVec4fl( float mat[][4], float *vec)
893 {
894         float x,y,z;
895
896         x=vec[0]; 
897         y=vec[1]; 
898         z= vec[2];
899         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
900         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
901         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
902         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
903 }
904
905 void Mat3MulVec( float mat[][3], int *vec)
906 {
907         int x,y;
908
909         x=vec[0]; 
910         y=vec[1];
911         vec[0]= (int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]);
912         vec[1]= (int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]);
913         vec[2]= (int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]);
914 }
915
916 void Mat3MulVecfl( 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[1][0] + mat[2][0]*vec[2];
923         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
924         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
925 }
926
927 void Mat3MulVecd( float mat[][3], double *vec)
928 {
929         double x,y;
930
931         x=vec[0]; 
932         y=vec[1];
933         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
934         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
935         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
936 }
937
938 void Mat3TransMulVecfl( float mat[][3], float *vec)
939 {
940         float x,y;
941
942         x=vec[0]; 
943         y=vec[1];
944         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
945         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
946         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
947 }
948
949 void Mat3MulFloat(float *m, float f)
950 {
951         int i;
952
953         for(i=0;i<9;i++) m[i]*=f;
954 }
955
956 void Mat4MulFloat(float *m, float f)
957 {
958         int i;
959
960         for(i=0;i<16;i++) m[i]*=f;      /* count to 12: without vector component */
961 }
962
963
964 void Mat4MulFloat3(float *m, float f)           /* only scale component */
965 {
966         int i,j;
967
968         for(i=0; i<3; i++) {
969                 for(j=0; j<3; j++) {
970                         
971                         m[4*i+j] *= f;
972                 }
973         }
974 }
975
976 void Mat3AddMat3(float m1[][3], float m2[][3], float m3[][3])
977 {
978         int i, j;
979
980         for(i=0;i<3;i++)
981                 for(j=0;j<3;j++)
982                         m1[i][j]= m2[i][j] + m3[i][j];
983 }
984
985 void Mat4AddMat4(float m1[][4], float m2[][4], float m3[][4])
986 {
987         int i, j;
988
989         for(i=0;i<4;i++)
990                 for(j=0;j<4;j++)
991                         m1[i][j]= m2[i][j] + m3[i][j];
992 }
993
994 void VecStar(float mat[][3], float *vec)
995 {
996
997         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
998         mat[0][1]= -vec[2];     
999         mat[0][2]= vec[1];
1000         mat[1][0]= vec[2];      
1001         mat[1][2]= -vec[0];
1002         mat[2][0]= -vec[1];     
1003         mat[2][1]= vec[0];
1004         
1005 }
1006 #ifdef TEST_ACTIVE
1007 short EenheidsMat(float mat[][3])
1008 {
1009
1010         if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0)
1011                 if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0)
1012                         if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0)
1013                                 return 1;
1014         return 0;
1015 }
1016 #endif
1017
1018 int FloatCompare( float *v1,  float *v2, float limit)
1019 {
1020
1021         if( fabs(v1[0]-v2[0])<limit ) {
1022                 if( fabs(v1[1]-v2[1])<limit ) {
1023                         if( fabs(v1[2]-v2[2])<limit ) return 1;
1024                 }
1025         }
1026         return 0;
1027 }
1028
1029 int FloatCompare4( float *v1,  float *v2, float limit)
1030 {
1031
1032         if( fabs(v1[0]-v2[0])<limit ) {
1033                 if( fabs(v1[1]-v2[1])<limit ) {
1034                         if( fabs(v1[2]-v2[2])<limit ) {
1035                                 if( fabs(v1[3]-v2[3])<limit ) return 1;
1036                         }
1037                 }
1038         }
1039         return 0;
1040 }
1041
1042 float FloatLerpf( float target, float origin, float fac)
1043 {
1044         return (fac*target) + (1.0f-fac)*origin;
1045 }
1046
1047 void printvecf( char *str,  float v[3])
1048 {
1049         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
1050
1051 }
1052
1053 void printquat( char *str,  float q[4])
1054 {
1055         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
1056
1057 }
1058
1059 void printvec4f( char *str,  float v[4])
1060 {
1061         printf("%s\n", str);
1062         printf("%f %f %f %f\n",v[0],v[1],v[2], v[3]);
1063         printf("\n");
1064
1065 }
1066
1067 void printmatrix4( char *str,  float m[][4])
1068 {
1069         printf("%s\n", str);
1070         printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
1071         printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
1072         printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
1073         printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
1074         printf("\n");
1075
1076 }
1077
1078 void printmatrix3( char *str,  float m[][3])
1079 {
1080         printf("%s\n", str);
1081         printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
1082         printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
1083         printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
1084         printf("\n");
1085
1086 }
1087
1088 /* **************** QUATERNIONS ********** */
1089
1090
1091 void QuatMul(float *q, float *q1, float *q2)
1092 {
1093         float t0,t1,t2;
1094
1095         t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
1096         t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
1097         t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
1098         q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
1099         q[0]=t0; 
1100         q[1]=t1; 
1101         q[2]=t2;
1102 }
1103
1104 /* Assumes a unit quaternion */
1105 void QuatMulVecf(float *q, float *v)
1106 {
1107         float t0, t1, t2;
1108
1109         t0=  -q[1]*v[0]-q[2]*v[1]-q[3]*v[2];
1110         t1=   q[0]*v[0]+q[2]*v[2]-q[3]*v[1];
1111         t2=   q[0]*v[1]+q[3]*v[0]-q[1]*v[2];
1112         v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0];
1113         v[0]=t1; 
1114         v[1]=t2;
1115
1116         t1=   t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
1117         t2=   t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
1118         v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
1119         v[0]=t1; 
1120         v[1]=t2;
1121 }
1122
1123 void QuatConj(float *q)
1124 {
1125         q[1] = -q[1];
1126         q[2] = -q[2];
1127         q[3] = -q[3];
1128 }
1129
1130 float QuatDot(float *q1, float *q2)
1131 {
1132         return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
1133 }
1134
1135 void QuatInv(float *q)
1136 {
1137         float f = QuatDot(q, q);
1138
1139         if (f == 0.0f)
1140                 return;
1141
1142         QuatConj(q);
1143         QuatMulf(q, 1.0f/f);
1144 }
1145
1146 /* simple mult */
1147 void QuatMulf(float *q, float f)
1148 {
1149         q[0] *= f;
1150         q[1] *= f;
1151         q[2] *= f;
1152         q[3] *= f;
1153 }
1154
1155 void QuatSub(float *q, float *q1, float *q2)
1156 {
1157         q2[0]= -q2[0];
1158         QuatMul(q, q1, q2);
1159         q2[0]= -q2[0];
1160 }
1161
1162 /* angular mult factor */
1163 void QuatMulFac(float *q, float fac)
1164 {
1165         float angle= fac*saacos(q[0]);  /* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */
1166         
1167         float co= (float)cos(angle);
1168         float si= (float)sin(angle);
1169         q[0]= co;
1170         Normalize(q+1);
1171         q[1]*= si;
1172         q[2]*= si;
1173         q[3]*= si;
1174         
1175 }
1176
1177 void QuatToMat3( float *q, float m[][3])
1178 {
1179         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1180
1181         q0= M_SQRT2 * q[0];
1182         q1= M_SQRT2 * q[1];
1183         q2= M_SQRT2 * q[2];
1184         q3= M_SQRT2 * q[3];
1185
1186         qda= q0*q1;
1187         qdb= q0*q2;
1188         qdc= q0*q3;
1189         qaa= q1*q1;
1190         qab= q1*q2;
1191         qac= q1*q3;
1192         qbb= q2*q2;
1193         qbc= q2*q3;
1194         qcc= q3*q3;
1195
1196         m[0][0]= (float)(1.0-qbb-qcc);
1197         m[0][1]= (float)(qdc+qab);
1198         m[0][2]= (float)(-qdb+qac);
1199
1200         m[1][0]= (float)(-qdc+qab);
1201         m[1][1]= (float)(1.0-qaa-qcc);
1202         m[1][2]= (float)(qda+qbc);
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 }
1208
1209
1210 void QuatToMat4( float *q, float m[][4])
1211 {
1212         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1213
1214         q0= M_SQRT2 * q[0];
1215         q1= M_SQRT2 * q[1];
1216         q2= M_SQRT2 * q[2];
1217         q3= M_SQRT2 * q[3];
1218
1219         qda= q0*q1;
1220         qdb= q0*q2;
1221         qdc= q0*q3;
1222         qaa= q1*q1;
1223         qab= q1*q2;
1224         qac= q1*q3;
1225         qbb= q2*q2;
1226         qbc= q2*q3;
1227         qcc= q3*q3;
1228
1229         m[0][0]= (float)(1.0-qbb-qcc);
1230         m[0][1]= (float)(qdc+qab);
1231         m[0][2]= (float)(-qdb+qac);
1232         m[0][3]= 0.0f;
1233
1234         m[1][0]= (float)(-qdc+qab);
1235         m[1][1]= (float)(1.0-qaa-qcc);
1236         m[1][2]= (float)(qda+qbc);
1237         m[1][3]= 0.0f;
1238
1239         m[2][0]= (float)(qdb+qac);
1240         m[2][1]= (float)(-qda+qbc);
1241         m[2][2]= (float)(1.0-qaa-qbb);
1242         m[2][3]= 0.0f;
1243         
1244         m[3][0]= m[3][1]= m[3][2]= 0.0f;
1245         m[3][3]= 1.0f;
1246 }
1247
1248 void Mat3ToQuat(float wmat[][3], float *q)
1249 {
1250         double tr, s;
1251         float mat[3][3];
1252
1253         /* work on a copy */
1254         Mat3CpyMat3(mat, wmat);
1255         Mat3Ortho(mat);                 /* this is needed AND a NormalQuat in the end */
1256         
1257         tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
1258         
1259         if(tr>FLT_EPSILON) {
1260                 s= sqrt( tr);
1261                 q[0]= (float)s;
1262                 s= 1.0/(4.0*s);
1263                 q[1]= (float)((mat[1][2]-mat[2][1])*s);
1264                 q[2]= (float)((mat[2][0]-mat[0][2])*s);
1265                 q[3]= (float)((mat[0][1]-mat[1][0])*s);
1266         }
1267         else {
1268                 if(mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
1269                         s= 2.0*sqrtf(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
1270                         q[1]= (float)(0.25*s);
1271
1272                         s= 1.0/s;
1273                         q[0]= (float)((mat[2][1] - mat[1][2])*s);
1274                         q[2]= (float)((mat[1][0] + mat[0][1])*s);
1275                         q[3]= (float)((mat[2][0] + mat[0][2])*s);
1276                 }
1277                 else if(mat[1][1] > mat[2][2]) {
1278                         s= 2.0*sqrtf(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
1279                         q[2]= (float)(0.25*s);
1280
1281                         s= 1.0/s;
1282                         q[0]= (float)((mat[2][0] - mat[0][2])*s);
1283                         q[1]= (float)((mat[1][0] + mat[0][1])*s);
1284                         q[3]= (float)((mat[2][1] + mat[1][2])*s);
1285                 }
1286                 else {
1287                         s= 2.0*sqrtf(1.0 + mat[2][2] - mat[0][0] - mat[1][1]);
1288                         q[3]= (float)(0.25*s);
1289
1290                         s= 1.0/s;
1291                         q[0]= (float)((mat[1][0] - mat[0][1])*s);
1292                         q[1]= (float)((mat[2][0] + mat[0][2])*s);
1293                         q[2]= (float)((mat[2][1] + mat[1][2])*s);
1294                 }
1295         }
1296         NormalQuat(q);
1297 }
1298
1299 void Mat3ToQuat_is_ok( float wmat[][3], float *q)
1300 {
1301         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
1302
1303         /* work on a copy */
1304         Mat3CpyMat3(mat, wmat);
1305         Mat3Ortho(mat);
1306         
1307         /* rotate z-axis of matrix to z-axis */
1308
1309         nor[0] = mat[2][1];             /* cross product with (0,0,1) */
1310         nor[1] =  -mat[2][0];
1311         nor[2] = 0.0;
1312         Normalize(nor);
1313         
1314         co= mat[2][2];
1315         angle= 0.5f*saacos(co);
1316         
1317         co= (float)cos(angle);
1318         si= (float)sin(angle);
1319         q1[0]= co;
1320         q1[1]= -nor[0]*si;              /* negative here, but why? */
1321         q1[2]= -nor[1]*si;
1322         q1[3]= -nor[2]*si;
1323
1324         /* rotate back x-axis from mat, using inverse q1 */
1325         QuatToMat3(q1, matr);
1326         Mat3Inv(matn, matr);
1327         Mat3MulVecfl(matn, mat[0]);
1328         
1329         /* and align x-axes */
1330         angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1331         
1332         co= (float)cos(angle);
1333         si= (float)sin(angle);
1334         q2[0]= co;
1335         q2[1]= 0.0f;
1336         q2[2]= 0.0f;
1337         q2[3]= si;
1338         
1339         QuatMul(q, q1, q2);
1340 }
1341
1342
1343 void Mat4ToQuat( float m[][4], float *q)
1344 {
1345         float mat[3][3];
1346         
1347         Mat3CpyMat4(mat, m);
1348         Mat3ToQuat(mat, q);
1349         
1350 }
1351
1352 void QuatOne(float *q)
1353 {
1354         q[0]= q[2]= q[3]= 0.0;
1355         q[1]= 1.0;
1356 }
1357
1358 void NormalQuat(float *q)
1359 {
1360         float len;
1361         
1362         len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
1363         if(len!=0.0) {
1364                 q[0]/= len;
1365                 q[1]/= len;
1366                 q[2]/= len;
1367                 q[3]/= len;
1368         } else {
1369                 q[1]= 1.0f;
1370                 q[0]= q[2]= q[3]= 0.0f;                 
1371         }
1372 }
1373
1374 void AxisAngleToQuat(float *q, float *axis, float angle)
1375 {
1376         float nor[3];
1377         float si;
1378         
1379         VecCopyf(nor, axis);
1380         Normalize(nor);
1381         
1382         angle /= 2;
1383         si = (float)sin(angle);
1384         q[0] = (float)cos(angle);
1385         q[1] = nor[0] * si;
1386         q[2] = nor[1] * si;
1387         q[3] = nor[2] * si;     
1388 }
1389
1390 void vectoquat(float *vec, short axis, short upflag, float *q)
1391 {
1392         float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
1393         
1394         /* first rotate to axis */
1395         if(axis>2) {    
1396                 x2= vec[0] ; y2= vec[1] ; z2= vec[2];
1397                 axis-= 3;
1398         }
1399         else {
1400                 x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
1401         }
1402         
1403         q[0]=1.0; 
1404         q[1]=q[2]=q[3]= 0.0;
1405
1406         len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
1407         if(len1 == 0.0) return;
1408
1409         /* nasty! I need a good routine for this...
1410          * problem is a rotation of an Y axis to the negative Y-axis for example.
1411          */
1412
1413         if(axis==0) {   /* x-axis */
1414                 nor[0]= 0.0;
1415                 nor[1]= -z2;
1416                 nor[2]= y2;
1417
1418                 if(fabs(y2)+fabs(z2)<0.0001)
1419                         nor[1]= 1.0;
1420
1421                 co= x2;
1422         }
1423         else if(axis==1) {      /* y-axis */
1424                 nor[0]= z2;
1425                 nor[1]= 0.0;
1426                 nor[2]= -x2;
1427                 
1428                 if(fabs(x2)+fabs(z2)<0.0001)
1429                         nor[2]= 1.0;
1430                 
1431                 co= y2;
1432         }
1433         else {                  /* z-axis */
1434                 nor[0]= -y2;
1435                 nor[1]= x2;
1436                 nor[2]= 0.0;
1437
1438                 if(fabs(x2)+fabs(y2)<0.0001)
1439                         nor[0]= 1.0;
1440
1441                 co= z2;
1442         }
1443         co/= len1;
1444
1445         Normalize(nor);
1446         
1447         angle= 0.5f*saacos(co);
1448         si= (float)sin(angle);
1449         q[0]= (float)cos(angle);
1450         q[1]= nor[0]*si;
1451         q[2]= nor[1]*si;
1452         q[3]= nor[2]*si;
1453         
1454         if(axis!=upflag) {
1455                 QuatToMat3(q, mat);
1456
1457                 fp= mat[2];
1458                 if(axis==0) {
1459                         if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1]));
1460                         else angle= (float)(-0.5*atan2(fp[1], fp[2]));
1461                 }
1462                 else if(axis==1) {
1463                         if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0]));
1464                         else angle= (float)(0.5*atan2(fp[0], fp[2]));
1465                 }
1466                 else {
1467                         if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0]));
1468                         else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
1469                 }
1470                                 
1471                 co= (float)cos(angle);
1472                 si= (float)(sin(angle)/len1);
1473                 q2[0]= co;
1474                 q2[1]= x2*si;
1475                 q2[2]= y2*si;
1476                 q2[3]= z2*si;
1477                         
1478                 QuatMul(q,q2,q);
1479         }
1480 }
1481
1482 void VecUpMat3old( float *vec, float mat[][3], short axis)
1483 {
1484         float inp, up[3];
1485         short cox = 0, coy = 0, coz = 0;
1486         
1487         /* using different up's is not useful, infact there is no real 'up'!
1488          */
1489
1490         up[0]= 0.0;
1491         up[1]= 0.0;
1492         up[2]= 1.0;
1493
1494         if(axis==0) {
1495                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1496         }
1497         if(axis==1) {
1498                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1499         }
1500         if(axis==2) {
1501                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1502         }
1503         if(axis==3) {
1504                 cox= 0; coy= 2; coz= 1;         /*  */
1505         }
1506         if(axis==4) {
1507                 cox= 1; coy= 0; coz= 2;         /*  */
1508         }
1509         if(axis==5) {
1510                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1511         }
1512
1513         mat[coz][0]= vec[0];
1514         mat[coz][1]= vec[1];
1515         mat[coz][2]= vec[2];
1516         Normalize((float *)mat[coz]);
1517         
1518         inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
1519         mat[coy][0]= up[0] - inp*mat[coz][0];
1520         mat[coy][1]= up[1] - inp*mat[coz][1];
1521         mat[coy][2]= up[2] - inp*mat[coz][2];
1522
1523         Normalize((float *)mat[coy]);
1524         
1525         Crossf(mat[cox], mat[coy], mat[coz]);
1526         
1527 }
1528
1529 void VecUpMat3(float *vec, float mat[][3], short axis)
1530 {
1531         float inp;
1532         short cox = 0, coy = 0, coz = 0;
1533
1534         /* using different up's is not useful, infact there is no real 'up'!
1535         */
1536
1537         if(axis==0) {
1538                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1539         }
1540         if(axis==1) {
1541                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1542         }
1543         if(axis==2) {
1544                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1545         }
1546         if(axis==3) {
1547                 cox= 0; coy= 1; coz= 2;         /* Y op -Z tr */
1548                 vec[0]= -vec[0];
1549                 vec[1]= -vec[1];
1550                 vec[2]= -vec[2];
1551         }
1552         if(axis==4) {
1553                 cox= 1; coy= 0; coz= 2;         /*  */
1554         }
1555         if(axis==5) {
1556                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1557         }
1558
1559         mat[coz][0]= vec[0];
1560         mat[coz][1]= vec[1];
1561         mat[coz][2]= vec[2];
1562         Normalize((float *)mat[coz]);
1563         
1564         inp= mat[coz][2];
1565         mat[coy][0]= - inp*mat[coz][0];
1566         mat[coy][1]= - inp*mat[coz][1];
1567         mat[coy][2]= 1.0f - inp*mat[coz][2];
1568
1569         Normalize((float *)mat[coy]);
1570         
1571         Crossf(mat[cox], mat[coy], mat[coz]);
1572         
1573 }
1574
1575 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
1576 void QuatInterpolW(float *, float *, float *, float );
1577
1578 void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
1579 {
1580         float omega, cosom, sinom, sc1, sc2;
1581
1582         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1583         
1584         /* rotate around shortest angle */
1585         if ((1.0 + cosom) > 0.0001) {
1586                 
1587                 if ((1.0 - cosom) > 0.0001) {
1588                         omega = acos(cosom);
1589                         sinom = sin(omega);
1590                         sc1 = sin((1.0 - t) * omega) / sinom;
1591                         sc2 = sin(t * omega) / sinom;
1592                 } 
1593                 else {
1594                         sc1 = 1.0 - t;
1595                         sc2 = t;
1596                 }
1597                 result[0] = sc1*quat1[0] + sc2*quat2[0];
1598                 result[1] = sc1*quat1[1] + sc2*quat2[1];
1599                 result[2] = sc1*quat1[2] + sc2*quat2[2];
1600                 result[3] = sc1*quat1[3] + sc2*quat2[3];
1601         } 
1602         else {
1603                 result[0] = quat2[3];
1604                 result[1] = -quat2[2];
1605                 result[2] = quat2[1];
1606                 result[3] = -quat2[0];
1607                 
1608                 sc1 = sin((1.0 - t)*M_PI_2);
1609                 sc2 = sin(t*M_PI_2);
1610
1611                 result[0] = sc1*quat1[0] + sc2*result[0];
1612                 result[1] = sc1*quat1[1] + sc2*result[1];
1613                 result[2] = sc1*quat1[2] + sc2*result[2];
1614                 result[3] = sc1*quat1[3] + sc2*result[3];
1615         }
1616 }
1617
1618 void QuatInterpol(float *result, float *quat1, float *quat2, float t)
1619 {
1620         float quat[4], omega, cosom, sinom, sc1, sc2;
1621
1622         cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
1623         
1624         /* rotate around shortest angle */
1625         if (cosom < 0.0) {
1626                 cosom = -cosom;
1627                 quat[0]= -quat1[0];
1628                 quat[1]= -quat1[1];
1629                 quat[2]= -quat1[2];
1630                 quat[3]= -quat1[3];
1631         } 
1632         else {
1633                 quat[0]= quat1[0];
1634                 quat[1]= quat1[1];
1635                 quat[2]= quat1[2];
1636                 quat[3]= quat1[3];
1637         }
1638         
1639         if ((1.0 - cosom) > 0.0001) {
1640                 omega = acos(cosom);
1641                 sinom = sin(omega);
1642                 sc1 = sin((1 - t) * omega) / sinom;
1643                 sc2 = sin(t * omega) / sinom;
1644         } else {
1645                 sc1= 1.0 - t;
1646                 sc2= t;
1647         }
1648         
1649         result[0] = sc1 * quat[0] + sc2 * quat2[0];
1650         result[1] = sc1 * quat[1] + sc2 * quat2[1];
1651         result[2] = sc1 * quat[2] + sc2 * quat2[2];
1652         result[3] = sc1 * quat[3] + sc2 * quat2[3];
1653 }
1654
1655 void QuatAdd(float *result, float *quat1, float *quat2, float t)
1656 {
1657         result[0]= quat1[0] + t*quat2[0];
1658         result[1]= quat1[1] + t*quat2[1];
1659         result[2]= quat1[2] + t*quat2[2];
1660         result[3]= quat1[3] + t*quat2[3];
1661 }
1662
1663 void QuatCopy(float *q1, float *q2)
1664 {
1665         q1[0]= q2[0];
1666         q1[1]= q2[1];
1667         q1[2]= q2[2];
1668         q1[3]= q2[3];
1669 }
1670
1671 /* **************** DUAL QUATERNIONS ************** */
1672
1673 /*
1674    Conversion routines between (regular quaternion, translation) and
1675    dual quaternion.
1676
1677    Version 1.0.0, February 7th, 2007
1678
1679    Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights 
1680    Reserved
1681
1682    This software is provided 'as-is', without any express or implied
1683    warranty.  In no event will the author(s) be held liable for any damages
1684    arising from the use of this software.
1685
1686    Permission is granted to anyone to use this software for any purpose,
1687    including commercial applications, and to alter it and redistribute it
1688    freely, subject to the following restrictions:
1689
1690    1. The origin of this software must not be misrepresented; you must not
1691       claim that you wrote the original software. If you use this software
1692       in a product, an acknowledgment in the product documentation would be
1693       appreciated but is not required.
1694    2. Altered source versions must be plainly marked as such, and must not be
1695       misrepresented as being the original software.
1696    3. This notice may not be removed or altered from any source distribution.
1697
1698    Author: Ladislav Kavan, kavanl@cs.tcd.ie
1699
1700    Changes for Blender:
1701    - renaming, style changes and optimizations
1702    - added support for scaling
1703 */
1704
1705 void Mat4ToDQuat(float basemat[][4], float mat[][4], DualQuat *dq)
1706 {
1707         float *t, *q, dscale[3], scale[3], basequat[4];
1708         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1709         float R[4][4], S[4][4];
1710
1711         /* split scaling and rotation, there is probably a faster way to do
1712            this, it's done like this now to correctly get negative scaling */
1713         Mat4MulMat4(baseRS, basemat, mat);
1714         Mat4ToSize(baseRS, scale);
1715
1716         VecCopyf(dscale, scale);
1717         dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f;
1718
1719         if((Det4x4(mat) < 0.0f) || VecLength(dscale) > 1e-4) {
1720                 /* extract R and S  */
1721                 Mat4ToQuat(baseRS, basequat);
1722                 QuatToMat4(basequat, baseR);
1723                 VecCopyf(baseR[3], baseRS[3]);
1724
1725                 Mat4Invert(baseinv, basemat);
1726                 Mat4MulMat4(R, baseinv, baseR);
1727
1728                 Mat4Invert(baseRinv, baseR);
1729                 Mat4MulMat4(S, baseRS, baseRinv);
1730
1731                 /* set scaling part */
1732                 Mat4MulSerie(dq->scale, basemat, S, baseinv, 0, 0, 0, 0, 0);
1733                 dq->scale_weight= 1.0f;
1734         }
1735         else {
1736                 /* matrix does not contain scaling */
1737                 Mat4CpyMat4(R, mat);
1738                 dq->scale_weight= 0.0f;
1739         }
1740
1741         /* non-dual part */
1742         Mat4ToQuat(R, dq->quat);
1743
1744         /* dual part */
1745         t= R[3];
1746         q= dq->quat;
1747         dq->trans[0]= -0.5f*( t[0]*q[1] + t[1]*q[2] + t[2]*q[3]);
1748         dq->trans[1]=  0.5f*( t[0]*q[0] + t[1]*q[3] - t[2]*q[2]);
1749         dq->trans[2]=  0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]);
1750         dq->trans[3]=  0.5f*( t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
1751 }
1752
1753 void DQuatToMat4(DualQuat *dq, float mat[][4])
1754 {
1755         float len, *t, q0[4];
1756         
1757         /* regular quaternion */
1758         QuatCopy(q0, dq->quat);
1759
1760         /* normalize */
1761         len= sqrt(QuatDot(q0, q0)); 
1762         if(len != 0.0f)
1763                 QuatMulf(q0, 1.0f/len);
1764         
1765         /* rotation */
1766         QuatToMat4(q0, mat);
1767
1768         /* translation */
1769         t= dq->trans;
1770         mat[3][0]= 2.0*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]);
1771         mat[3][1]= 2.0*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]);
1772         mat[3][2]= 2.0*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]);
1773
1774         /* note: this does not handle scaling */
1775 }       
1776
1777 void DQuatAddWeighted(DualQuat *dqsum, DualQuat *dq, float weight)
1778 {
1779         int flipped= 0;
1780
1781         /* make sure we interpolate quats in the right direction */
1782         if (QuatDot(dq->quat, dqsum->quat) < 0) {
1783                 flipped= 1;
1784                 weight= -weight;
1785         }
1786
1787         /* interpolate rotation and translation */
1788         dqsum->quat[0] += weight*dq->quat[0];
1789         dqsum->quat[1] += weight*dq->quat[1];
1790         dqsum->quat[2] += weight*dq->quat[2];
1791         dqsum->quat[3] += weight*dq->quat[3];
1792
1793         dqsum->trans[0] += weight*dq->trans[0];
1794         dqsum->trans[1] += weight*dq->trans[1];
1795         dqsum->trans[2] += weight*dq->trans[2];
1796         dqsum->trans[3] += weight*dq->trans[3];
1797
1798         /* interpolate scale - but only if needed */
1799         if (dq->scale_weight) {
1800                 float wmat[4][4];
1801
1802                 if(flipped)     /* we don't want negative weights for scaling */
1803                         weight= -weight;
1804
1805                 Mat4CpyMat4(wmat, dq->scale);
1806                 Mat4MulFloat((float*)wmat, weight);
1807                 Mat4AddMat4(dqsum->scale, dqsum->scale, wmat);
1808                 dqsum->scale_weight += weight;
1809         }
1810 }
1811
1812 void DQuatNormalize(DualQuat *dq, float totweight)
1813 {
1814         float scale= 1.0f/totweight;
1815
1816         QuatMulf(dq->quat, scale);
1817         QuatMulf(dq->trans, scale);
1818         
1819         if(dq->scale_weight) {
1820                 float addweight= totweight - dq->scale_weight;
1821
1822                 if(addweight) {
1823                         dq->scale[0][0] += addweight;
1824                         dq->scale[1][1] += addweight;
1825                         dq->scale[2][2] += addweight;
1826                         dq->scale[3][3] += addweight;
1827                 }
1828
1829                 Mat4MulFloat((float*)dq->scale, scale);
1830                 dq->scale_weight= 1.0f;
1831         }
1832 }
1833
1834 void DQuatMulVecfl(DualQuat *dq, float *co, float mat[][3])
1835 {       
1836         float M[3][3], t[3], scalemat[3][3], len2;
1837         float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3];
1838         float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3];
1839         
1840         /* rotation matrix */
1841         M[0][0]= w*w + x*x - y*y - z*z;
1842         M[1][0]= 2*(x*y - w*z);
1843         M[2][0]= 2*(x*z + w*y);
1844
1845         M[0][1]= 2*(x*y + w*z);
1846         M[1][1]= w*w + y*y - x*x - z*z;
1847         M[2][1]= 2*(y*z - w*x); 
1848
1849         M[0][2]= 2*(x*z - w*y);
1850         M[1][2]= 2*(y*z + w*x);
1851         M[2][2]= w*w + z*z - x*x - y*y;
1852         
1853         len2= QuatDot(dq->quat, dq->quat);
1854         if(len2 > 0.0f)
1855                 len2= 1.0f/len2;
1856         
1857         /* translation */
1858         t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3);
1859         t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2);
1860         t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y);
1861
1862         /* apply scaling */
1863         if(dq->scale_weight)
1864                 Mat4MulVecfl(dq->scale, co);
1865         
1866         /* apply rotation and translation */
1867         Mat3MulVecfl(M, co);
1868         co[0]= (co[0] + t[0])*len2;
1869         co[1]= (co[1] + t[1])*len2;
1870         co[2]= (co[2] + t[2])*len2;
1871
1872         /* compute crazyspace correction mat */
1873         if(mat) {
1874                 if(dq->scale_weight) {
1875                         Mat3CpyMat4(scalemat, dq->scale);
1876                         Mat3MulMat3(mat, M, scalemat);
1877                 }
1878                 else
1879                         Mat3CpyMat3(mat, M);
1880                 Mat3MulFloat((float*)mat, len2);
1881         }
1882 }
1883
1884 void DQuatCpyDQuat(DualQuat *dq1, DualQuat *dq2)
1885 {
1886         memcpy(dq1, dq2, sizeof(DualQuat));
1887 }
1888
1889 /* **************** VIEW / PROJECTION ********************************  */
1890
1891
1892 void i_ortho(
1893         float left, float right,
1894         float bottom, float top,
1895         float nearClip, float farClip,
1896         float matrix[][4]
1897 ){
1898     float Xdelta, Ydelta, Zdelta;
1899  
1900     Xdelta = right - left;
1901     Ydelta = top - bottom;
1902     Zdelta = farClip - nearClip;
1903     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1904                 return;
1905     }
1906     Mat4One(matrix);
1907     matrix[0][0] = 2.0f/Xdelta;
1908     matrix[3][0] = -(right + left)/Xdelta;
1909     matrix[1][1] = 2.0f/Ydelta;
1910     matrix[3][1] = -(top + bottom)/Ydelta;
1911     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1912     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1913 }
1914
1915 void i_window(
1916         float left, float right,
1917         float bottom, float top,
1918         float nearClip, float farClip,
1919         float mat[][4]
1920 ){
1921         float Xdelta, Ydelta, Zdelta;
1922
1923         Xdelta = right - left;
1924         Ydelta = top - bottom;
1925         Zdelta = farClip - nearClip;
1926
1927         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1928                 return;
1929         }
1930         mat[0][0] = nearClip * 2.0f/Xdelta;
1931         mat[1][1] = nearClip * 2.0f/Ydelta;
1932         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1933         mat[2][1] = (top + bottom)/Ydelta;
1934         mat[2][2] = -(farClip + nearClip)/Zdelta;
1935         mat[2][3] = -1.0f;
1936         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1937         mat[0][1] = mat[0][2] = mat[0][3] =
1938             mat[1][0] = mat[1][2] = mat[1][3] =
1939             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1940
1941 }
1942
1943 void i_translate(float Tx, float Ty, float Tz, float mat[][4])
1944 {
1945     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1946     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1947     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1948 }
1949
1950 void i_multmatrix( float icand[][4], float Vm[][4])
1951 {
1952     int row, col;
1953     float temp[4][4];
1954
1955     for(row=0 ; row<4 ; row++) 
1956         for(col=0 ; col<4 ; col++)
1957             temp[row][col] = icand[row][0] * Vm[0][col]
1958                            + icand[row][1] * Vm[1][col]
1959                            + icand[row][2] * Vm[2][col]
1960                            + icand[row][3] * Vm[3][col];
1961         Mat4CpyMat4(Vm, temp);
1962 }
1963
1964 void i_rotate(float angle, char axis, float mat[][4])
1965 {
1966         int col;
1967     float temp[4];
1968     float cosine, sine;
1969
1970     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
1971         temp[col] = 0;
1972
1973     angle = (float)(angle*(3.1415926535/180.0));
1974     cosine = (float)cos(angle);
1975     sine = (float)sin(angle);
1976     switch(axis){
1977     case 'x':    
1978     case 'X':    
1979         for(col=0 ; col<4 ; col++)
1980             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1981         for(col=0 ; col<4 ; col++) {
1982             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1983             mat[1][col] = temp[col];
1984         }
1985         break;
1986
1987     case 'y':
1988     case 'Y':
1989         for(col=0 ; col<4 ; col++)
1990             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1991         for(col=0 ; col<4 ; col++) {
1992             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1993             mat[0][col] = temp[col];
1994         }
1995         break;
1996
1997     case 'z':
1998     case 'Z':
1999         for(col=0 ; col<4 ; col++)
2000             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
2001         for(col=0 ; col<4 ; col++) {
2002             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
2003             mat[0][col] = temp[col];
2004         }
2005         break;
2006     }
2007 }
2008
2009 void i_polarview(float dist, float azimuth, float incidence, float twist, float Vm[][4])
2010 {
2011
2012         Mat4One(Vm);
2013
2014     i_translate(0.0, 0.0, -dist, Vm);
2015     i_rotate(-twist,'z', Vm);   
2016     i_rotate(-incidence,'x', Vm);
2017     i_rotate(-azimuth,'z', Vm);
2018 }
2019
2020 void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float twist, float mat[][4])
2021 {
2022         float sine, cosine, hyp, hyp1, dx, dy, dz;
2023         float mat1[4][4];
2024         
2025         Mat4One(mat);
2026         Mat4One(mat1);
2027
2028         i_rotate(-twist,'z', mat);
2029
2030         dx = px - vx;
2031         dy = py - vy;
2032         dz = pz - vz;
2033         hyp = dx * dx + dz * dz;        /* hyp squared  */
2034         hyp1 = (float)sqrt(dy*dy + hyp);
2035         hyp = (float)sqrt(hyp);         /* the real hyp */
2036         
2037         if (hyp1 != 0.0) {              /* rotate X     */
2038                 sine = -dy / hyp1;
2039                 cosine = hyp /hyp1;
2040         } else {
2041                 sine = 0;
2042                 cosine = 1.0f;
2043         }
2044         mat1[1][1] = cosine;
2045         mat1[1][2] = sine;
2046         mat1[2][1] = -sine;
2047         mat1[2][2] = cosine;
2048         
2049         i_multmatrix(mat1, mat);
2050
2051     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
2052     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
2053         
2054         /* paragraph    */
2055         if (hyp != 0.0f) {                      /* rotate Y     */
2056                 sine = dx / hyp;
2057                 cosine = -dz / hyp;
2058         } else {
2059                 sine = 0;
2060                 cosine = 1.0f;
2061         }
2062         mat1[0][0] = cosine;
2063         mat1[0][2] = -sine;
2064         mat1[2][0] = sine;
2065         mat1[2][2] = cosine;
2066         
2067         i_multmatrix(mat1, mat);
2068         i_translate(-vx,-vy,-vz, mat);  /* translate viewpoint to origin */
2069 }
2070
2071
2072
2073
2074
2075 /* ************************************************  */
2076
2077 void Mat3Ortho(float mat[][3])
2078 {       
2079         Normalize(mat[0]);
2080         Normalize(mat[1]);
2081         Normalize(mat[2]);
2082 }
2083
2084 void Mat4Ortho(float mat[][4])
2085 {
2086         float len;
2087         
2088         len= Normalize(mat[0]);
2089         if(len!=0.0) mat[0][3]/= len;
2090         len= Normalize(mat[1]);
2091         if(len!=0.0) mat[1][3]/= len;
2092         len= Normalize(mat[2]);
2093         if(len!=0.0) mat[2][3]/= len;
2094 }
2095
2096 void VecCopyf(float *v1, float *v2)
2097 {
2098         v1[0]= v2[0];
2099         v1[1]= v2[1];
2100         v1[2]= v2[2];
2101 }
2102
2103 int VecLen( int *v1, int *v2)
2104 {
2105         float x,y,z;
2106
2107         x=(float)(v1[0]-v2[0]);
2108         y=(float)(v1[1]-v2[1]);
2109         z=(float)(v1[2]-v2[2]);
2110         return (int)floor(sqrt(x*x+y*y+z*z));
2111 }
2112
2113 float VecLenf( float *v1, float *v2)
2114 {
2115         float x,y,z;
2116
2117         x=v1[0]-v2[0];
2118         y=v1[1]-v2[1];
2119         z=v1[2]-v2[2];
2120         return (float)sqrt(x*x+y*y+z*z);
2121 }
2122
2123 float VecLength(float *v)
2124 {
2125         return (float) sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
2126 }
2127
2128 void VecAddf(float *v, float *v1, float *v2)
2129 {
2130         v[0]= v1[0]+ v2[0];
2131         v[1]= v1[1]+ v2[1];
2132         v[2]= v1[2]+ v2[2];
2133 }
2134
2135 void VecSubf(float *v, float *v1, float *v2)
2136 {
2137         v[0]= v1[0]- v2[0];
2138         v[1]= v1[1]- v2[1];
2139         v[2]= v1[2]- v2[2];
2140 }
2141
2142 void VecLerpf(float *target, float *a, float *b, float t)
2143 {
2144         float s = 1.0f-t;
2145
2146         target[0]= s*a[0] + t*b[0];
2147         target[1]= s*a[1] + t*b[1];
2148         target[2]= s*a[2] + t*b[2];
2149 }
2150
2151 void Vec2Lerpf(float *target, float *a, float *b, float t)
2152 {
2153         float s = 1.0f-t;
2154
2155         target[0]= s*a[0] + t*b[0];
2156         target[1]= s*a[1] + t*b[1];
2157 }
2158
2159 void VecMidf(float *v, float *v1, float *v2)
2160 {
2161         v[0]= 0.5f*(v1[0]+ v2[0]);
2162         v[1]= 0.5f*(v1[1]+ v2[1]);
2163         v[2]= 0.5f*(v1[2]+ v2[2]);
2164 }
2165
2166 void VecMulf(float *v1, float f)
2167 {
2168
2169         v1[0]*= f;
2170         v1[1]*= f;
2171         v1[2]*= f;
2172 }
2173
2174 void VecOrthoBasisf(float *v, float *v1, float *v2)
2175 {
2176         float f = sqrt(v[0]*v[0] + v[1]*v[1]);
2177
2178         if (f < 1e-35f) {
2179                 // degenerate case
2180                 v1[0] = 0.0f; v1[1] = 1.0f; v1[2] = 0.0f;
2181                 if (v[2] > 0.0f) {
2182                         v2[0] = 1.0f; v2[1] = v2[2] = 0.0f;
2183                 }
2184                 else {
2185                         v2[0] = -1.0f; v2[1] = v2[2] = 0.0f;
2186                 }
2187         }
2188         else  {
2189                 f = 1.0f/f;
2190                 v1[0] = v[1]*f;
2191                 v1[1] = -v[0]*f;
2192                 v1[2] = 0.0f;
2193
2194                 Crossf(v2, v, v1);
2195         }
2196 }
2197
2198 int VecLenCompare(float *v1, float *v2, float limit)
2199 {
2200     float x,y,z;
2201
2202         x=v1[0]-v2[0];
2203         y=v1[1]-v2[1];
2204         z=v1[2]-v2[2];
2205
2206         return ((x*x + y*y + z*z) < (limit*limit));
2207 }
2208
2209 int VecCompare( float *v1, float *v2, float limit)
2210 {
2211         if( fabs(v1[0]-v2[0])<limit )
2212                 if( fabs(v1[1]-v2[1])<limit )
2213                         if( fabs(v1[2]-v2[2])<limit ) return 1;
2214         return 0;
2215 }
2216
2217 int VecEqual(float *v1, float *v2)
2218 {
2219         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
2220 }
2221
2222 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
2223 {
2224         float n1[3],n2[3];
2225
2226         n1[0]= (float)(v1[0]-v2[0]);
2227         n2[0]= (float)(v2[0]-v3[0]);
2228         n1[1]= (float)(v1[1]-v2[1]);
2229         n2[1]= (float)(v2[1]-v3[1]);
2230         n1[2]= (float)(v1[2]-v2[2]);
2231         n2[2]= (float)(v2[2]-v3[2]);
2232         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2233         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2234         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2235         Normalize(n);
2236 }
2237
2238 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
2239 {
2240         float n1[3],n2[3];
2241
2242         n1[0]= (float)(v1[0]-v2[0]);
2243         n2[0]= (float)(v2[0]-v3[0]);
2244         n1[1]= (float)(v1[1]-v2[1]);
2245         n2[1]= (float)(v2[1]-v3[1]);
2246         n1[2]= (float)(v1[2]-v2[2]);
2247         n2[2]= (float)(v2[2]-v3[2]);
2248         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2249         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2250         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2251         Normalize(n);
2252 }
2253
2254 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
2255 {
2256         float n1[3],n2[3];
2257
2258         n1[0]= v1[0]-v2[0];
2259         n2[0]= v2[0]-v3[0];
2260         n1[1]= v1[1]-v2[1];
2261         n2[1]= v2[1]-v3[1];
2262         n1[2]= v1[2]-v2[2];
2263         n2[2]= v2[2]-v3[2];
2264         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2265         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2266         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2267         return Normalize(n);
2268 }
2269
2270 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
2271 {
2272         /* real cross! */
2273         float n1[3],n2[3];
2274
2275         n1[0]= v1[0]-v3[0];
2276         n1[1]= v1[1]-v3[1];
2277         n1[2]= v1[2]-v3[2];
2278
2279         n2[0]= v2[0]-v4[0];
2280         n2[1]= v2[1]-v4[1];
2281         n2[2]= v2[2]-v4[2];
2282
2283         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2284         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2285         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2286
2287         return Normalize(n);
2288 }
2289
2290
2291 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
2292 {
2293
2294         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
2295         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
2296         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
2297 }
2298
2299 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
2300 {
2301
2302         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
2303         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
2304         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
2305 }
2306
2307 float Sqrt3f(float f)
2308 {
2309         if(f==0.0) return 0;
2310         if(f<0) return (float)(-exp(log(-f)/3));
2311         else return (float)(exp(log(f)/3));
2312 }
2313
2314 double Sqrt3d(double d)
2315 {
2316         if(d==0.0) return 0;
2317         if(d<0) return -exp(log(-d)/3);
2318         else return exp(log(d)/3);
2319 }
2320
2321 void NormalShortToFloat(float *out, short *in)
2322 {
2323         out[0] = in[0] / 32767.0;
2324         out[1] = in[1] / 32767.0;
2325         out[2] = in[2] / 32767.0;
2326 }
2327
2328 void NormalFloatToShort(short *out, float *in)
2329 {
2330         out[0] = (short)(in[0] * 32767.0);
2331         out[1] = (short)(in[1] * 32767.0);
2332         out[2] = (short)(in[2] * 32767.0);
2333 }
2334
2335 /* distance v1 to line v2-v3 */
2336 /* using Hesse formula, NO LINE PIECE! */
2337 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
2338         float a[2],deler;
2339
2340         a[0]= v2[1]-v3[1];
2341         a[1]= v3[0]-v2[0];
2342         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
2343         if(deler== 0.0f) return 0;
2344
2345         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
2346
2347 }
2348
2349 /* distance v1 to line-piece v2-v3 */
2350 float PdistVL2Dfl( float *v1, float *v2, float *v3) 
2351 {
2352         float labda, rc[2], pt[2], len;
2353         
2354         rc[0]= v3[0]-v2[0];
2355         rc[1]= v3[1]-v2[1];
2356         len= rc[0]*rc[0]+ rc[1]*rc[1];
2357         if(len==0.0) {
2358                 rc[0]= v1[0]-v2[0];
2359                 rc[1]= v1[1]-v2[1];
2360                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
2361         }
2362         
2363         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
2364         if(labda<=0.0) {
2365                 pt[0]= v2[0];
2366                 pt[1]= v2[1];
2367         }
2368         else if(labda>=1.0) {
2369                 pt[0]= v3[0];
2370                 pt[1]= v3[1];
2371         }
2372         else {
2373                 pt[0]= labda*rc[0]+v2[0];
2374                 pt[1]= labda*rc[1]+v2[1];
2375         }
2376
2377         rc[0]= pt[0]-v1[0];
2378         rc[1]= pt[1]-v1[1];
2379         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
2380 }
2381
2382 float AreaF2Dfl( float *v1, float *v2, float *v3)
2383 {
2384         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
2385 }
2386
2387
2388 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
2389 {
2390         float len, vec1[3], vec2[3], n[3];
2391
2392         VecSubf(vec1, v2, v1);
2393         VecSubf(vec2, v4, v1);
2394         Crossf(n, vec1, vec2);
2395         len= Normalize(n);
2396
2397         VecSubf(vec1, v4, v3);
2398         VecSubf(vec2, v2, v3);
2399         Crossf(n, vec1, vec2);
2400         len+= Normalize(n);
2401
2402         return (len/2.0f);
2403 }
2404
2405 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
2406 {
2407         float len, vec1[3], vec2[3], n[3];
2408
2409         VecSubf(vec1, v3, v2);
2410         VecSubf(vec2, v1, v2);
2411         Crossf(n, vec1, vec2);
2412         len= Normalize(n);
2413
2414         return (len/2.0f);
2415 }
2416
2417 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
2418 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
2419
2420
2421 float AreaPoly3Dfl(int nr, float *verts, float *normal)
2422 {
2423         float x, y, z, area, max;
2424         float *cur, *prev;
2425         int a, px=0, py=1;
2426
2427         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
2428         x= (float)fabs(normal[0]);
2429         y= (float)fabs(normal[1]);
2430         z= (float)fabs(normal[2]);
2431         max = MAX3(x, y, z);
2432         if(max==y) py=2;
2433         else if(max==x) {
2434                 px=1; 
2435                 py= 2;
2436         }
2437
2438         /* The Trapezium Area Rule */
2439         prev= verts+3*(nr-1);
2440         cur= verts;
2441         area= 0;
2442         for(a=0; a<nr; a++) {
2443                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
2444                 prev= cur;
2445                 cur+=3;
2446         }
2447
2448         return (float)fabs(0.5*area/max);
2449 }
2450
2451 /* intersect Line-Line, shorts */
2452 short IsectLL2Ds(short *v1, short *v2, short *v3, short *v4)
2453 {
2454         /* return:
2455         -1: colliniar
2456          0: no intersection of segments
2457          1: exact intersection of segments
2458          2: cross-intersection of segments
2459         */
2460         float div, labda, mu;
2461         
2462         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2463         if(div==0.0) return -1;
2464         
2465         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2466         
2467         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2468         
2469         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2470                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2471                 return 2;
2472         }
2473         return 0;
2474 }
2475
2476 /* intersect Line-Line, floats */
2477 short IsectLL2Df(float *v1, float *v2, float *v3, float *v4)
2478 {
2479         /* return:
2480         -1: colliniar
2481 0: no intersection of segments
2482 1: exact intersection of segments
2483 2: cross-intersection of segments
2484         */
2485         float div, labda, mu;
2486         
2487         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2488         if(div==0.0) return -1;
2489         
2490         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2491         
2492         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2493         
2494         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2495                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2496                 return 2;
2497         }
2498         return 0;
2499 }
2500
2501 /*
2502 -1: colliniar
2503  1: intersection
2504
2505 */
2506 static short IsectLLPt2Df(float x0,float y0,float x1,float y1,
2507                                          float x2,float y2,float x3,float y3, float *xi,float *yi)
2508
2509 {
2510         /*
2511         this function computes the intersection of the sent lines
2512         and returns the intersection point, note that the function assumes
2513         the lines intersect. the function can handle vertical as well
2514         as horizontal lines. note the function isn't very clever, it simply
2515         applies the math, but we don't need speed since this is a
2516         pre-processing step
2517         */
2518         float c1,c2, // constants of linear equations
2519         det_inv,  // the inverse of the determinant of the coefficient
2520         m1,m2;    // the slopes of each line
2521         /*
2522         compute slopes, note the cludge for infinity, however, this will
2523         be close enough
2524         */
2525         if ( fabs( x1-x0 ) > 0.000001 )
2526                 m1 = ( y1-y0 ) / ( x1-x0 );
2527         else
2528                 return -1; /*m1 = ( float ) 1e+10;*/   // close enough to infinity
2529
2530         if ( fabs( x3-x2 ) > 0.000001 )
2531                 m2 = ( y3-y2 ) / ( x3-x2 );
2532         else
2533                 return -1; /*m2 = ( float ) 1e+10;*/   // close enough to infinity
2534
2535         if (fabs(m1-m2) < 0.000001)
2536                 return -1; /* paralelle lines */
2537         
2538 // compute constants
2539
2540         c1 = ( y0-m1*x0 );
2541         c2 = ( y2-m2*x2 );
2542
2543 // compute the inverse of the determinate
2544
2545         det_inv = 1.0f / ( -m1 + m2 );
2546
2547 // use Kramers rule to compute xi and yi
2548
2549         *xi= ( ( -c2 + c1 ) *det_inv );
2550         *yi= ( ( m2*c1 - m1*c2 ) *det_inv );
2551         
2552         return 1; 
2553 } // end Intersect_Lines
2554
2555 #define SIDE_OF_LINE(pa,pb,pp)  ((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
2556 /* point in tri */
2557 int IsectPT2Df(float pt[2], float v1[2], float v2[2], float v3[2])
2558 {
2559         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
2560                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
2561                         if (SIDE_OF_LINE(v3,v1,pt)>=0.0) {
2562                                 return 1;
2563                         }
2564                 }
2565         } else {
2566                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0) ) {
2567                         if (! (SIDE_OF_LINE(v3,v1,pt)>=0.0)) {
2568                                 return -1;
2569                         }
2570                 }
2571         }
2572         
2573         return 0;
2574 }
2575 /* point in quad - only convex quads */
2576 int IsectPQ2Df(float pt[2], float v1[2], float v2[2], float v3[2], float v4[2])
2577 {
2578         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
2579                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
2580                         if (SIDE_OF_LINE(v3,v4,pt)>=0.0) {
2581                                 if (SIDE_OF_LINE(v4,v1,pt)>=0.0) {
2582                                         return 1;
2583                                 }
2584                         }
2585                 }
2586         } else {
2587                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0) ) {
2588                         if (! (SIDE_OF_LINE(v3,v4,pt)>=0.0)) {
2589                                 if (! (SIDE_OF_LINE(v4,v1,pt)>=0.0)) {
2590                                         return -1;
2591                                 }
2592                         }
2593                 }
2594         }
2595         
2596         return 0;
2597 }
2598
2599
2600 /**
2601  * 
2602  * @param min 
2603  * @param max 
2604  * @param vec 
2605  */
2606 void MinMax3(float *min, float *max, float *vec)
2607 {
2608         if(min[0]>vec[0]) min[0]= vec[0];
2609         if(min[1]>vec[1]) min[1]= vec[1];
2610         if(min[2]>vec[2]) min[2]= vec[2];
2611
2612         if(max[0]<vec[0]) max[0]= vec[0];
2613         if(max[1]<vec[1]) max[1]= vec[1];
2614         if(max[2]<vec[2]) max[2]= vec[2];
2615 }
2616
2617 static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
2618 {
2619         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
2620 }
2621
2622 static int BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
2623 {
2624         float xn, yn, zn, a1, a2, a3, asum;
2625         short i, j;
2626
2627         /* find best projection of face XY, XZ or YZ: barycentric weights of
2628            the 2d projected coords are the same and faster to compute */
2629         xn= fabs(n[0]);
2630         yn= fabs(n[1]);
2631         zn= fabs(n[2]);
2632         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
2633         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
2634         else {i= 1; j= 2;} 
2635
2636         a1= TriSignedArea(v2, v3, co, i, j);
2637         a2= TriSignedArea(v3, v1, co, i, j);
2638         a3= TriSignedArea(v1, v2, co, i, j);
2639
2640         asum= a1 + a2 + a3;
2641
2642         if (fabs(asum) < FLT_EPSILON) {
2643                 /* zero area triangle */
2644                 w[0]= w[1]= w[2]= 1.0f/3.0f;
2645                 return 1;
2646         }
2647
2648         asum= 1.0f/asum;
2649         w[0]= a1*asum;
2650         w[1]= a2*asum;
2651         w[2]= a3*asum;
2652
2653         return 0;
2654 }
2655
2656 void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
2657 {
2658         float w2[3];
2659
2660         w[0]= w[1]= w[2]= w[3]= 0.0f;
2661
2662         /* first check for exact match */
2663         if(VecEqual(co, v1))
2664                 w[0]= 1.0f;
2665         else if(VecEqual(co, v2))
2666                 w[1]= 1.0f;
2667         else if(VecEqual(co, v3))
2668                 w[2]= 1.0f;
2669         else if(v4 && VecEqual(co, v4))
2670                 w[3]= 1.0f;
2671         else {
2672                 /* otherwise compute barycentric interpolation weights */
2673                 float n1[3], n2[3], n[3];
2674                 int degenerate;
2675
2676                 VecSubf(n1, v1, v3);
2677                 if (v4) {
2678                         VecSubf(n2, v2, v4);
2679                 }
2680                 else {
2681                         VecSubf(n2, v2, v3);
2682                 }
2683                 Crossf(n, n1, n2);
2684
2685                 /* OpenGL seems to split this way, so we do too */
2686                 if (v4) {
2687                         degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
2688                         SWAP(float, w[2], w[3]);
2689
2690                         if(degenerate || (w[0] < 0.0f)) {
2691                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2692                                    so we interpolate using the other triangle */
2693                                 degenerate= BarycentricWeights(v2, v3, v4, co, n, w2);
2694
2695                                 if(!degenerate) {
2696                                         w[0]= 0.0f;
2697                                         w[1]= w2[0];
2698                                         w[2]= w2[1];
2699                                         w[3]= w2[2];
2700                                 }
2701                         }
2702                 }
2703                 else
2704                         BarycentricWeights(v1, v2, v3, co, n, w);
2705         }
2706 }
2707
2708 /* Mean value weights - smooth interpolation weights for polygons with
2709  * more than 3 vertices */
2710 static float MeanValueHalfTan(float *v1, float *v2, float *v3)
2711 {
2712         float d2[3], d3[3], cross[3], area, dot, len;
2713
2714         VecSubf(d2, v2, v1);
2715         VecSubf(d3, v3, v1);
2716         Crossf(cross, d2, d3);
2717
2718         area= VecLength(cross);
2719         dot= Inpf(d2, d3);
2720         len= VecLength(d2)*VecLength(d3);
2721
2722         if(area == 0.0f)
2723                 return 0.0f;
2724         else
2725                 return (len - dot)/area;
2726 }
2727
2728 void MeanValueWeights(float v[][3], int n, float *co, float *w)
2729 {
2730         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
2731         int i;
2732
2733         totweight= 0.0f;
2734
2735         for(i=0; i<n; i++) {
2736                 vmid= v[i];
2737                 vprev= (i == 0)? v[n-1]: v[i-1];
2738                 vnext= (i == n-1)? v[0]: v[i+1];
2739
2740                 t1= MeanValueHalfTan(co, vprev, vmid);
2741                 t2= MeanValueHalfTan(co, vmid, vnext);
2742
2743                 len= VecLenf(co, vmid);
2744                 w[i]= (t1+t2)/len;
2745                 totweight += w[i];
2746         }
2747
2748         if(totweight != 0.0f)
2749                 for(i=0; i<n; i++)
2750                         w[i] /= totweight;
2751 }
2752
2753
2754 /* ************ EULER *************** */
2755
2756 void EulToMat3( float *eul, float mat[][3])
2757 {
2758         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2759         
2760         ci = cos(eul[0]); 
2761         cj = cos(eul[1]); 
2762         ch = cos(eul[2]);
2763         si = sin(eul[0]); 
2764         sj = sin(eul[1]); 
2765         sh = sin(eul[2]);
2766         cc = ci*ch; 
2767         cs = ci*sh; 
2768         sc = si*ch; 
2769         ss = si*sh;
2770
2771         mat[0][0] = (float)(cj*ch); 
2772         mat[1][0] = (float)(sj*sc-cs); 
2773         mat[2][0] = (float)(sj*cc+ss);
2774         mat[0][1] = (float)(cj*sh); 
2775         mat[1][1] = (float)(sj*ss+cc); 
2776         mat[2][1] = (float)(sj*cs-sc);
2777         mat[0][2] = (float)-sj;  
2778         mat[1][2] = (float)(cj*si);    
2779         mat[2][2] = (float)(cj*ci);
2780
2781 }
2782
2783 void EulToMat4( float *eul,float mat[][4])
2784 {
2785         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2786         
2787         ci = cos(eul[0]); 
2788         cj = cos(eul[1]); 
2789         ch = cos(eul[2]);
2790         si = sin(eul[0]); 
2791         sj = sin(eul[1]); 
2792         sh = sin(eul[2]);
2793         cc = ci*ch; 
2794         cs = ci*sh; 
2795         sc = si*ch; 
2796         ss = si*sh;
2797
2798         mat[0][0] = (float)(cj*ch); 
2799         mat[1][0] = (float)(sj*sc-cs); 
2800         mat[2][0] = (float)(sj*cc+ss);
2801         mat[0][1] = (float)(cj*sh); 
2802         mat[1][1] = (float)(sj*ss+cc); 
2803         mat[2][1] = (float)(sj*cs-sc);
2804         mat[0][2] = (float)-sj;  
2805         mat[1][2] = (float)(cj*si);    
2806         mat[2][2] = (float)(cj*ci);
2807
2808
2809         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2810         mat[3][3]= 1.0f;
2811 }
2812
2813 /* returns two euler calculation methods, so we can pick the best */
2814 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
2815 {
2816         float cy, quat[4], mat[3][3];
2817         
2818         Mat3ToQuat(tmat, quat);
2819         QuatToMat3(quat, mat);
2820         Mat3CpyMat3(mat, tmat);
2821         Mat3Ortho(mat);
2822         
2823         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2824         
2825         if (cy > 16.0*FLT_EPSILON) {
2826                 
2827                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2828                 eul1[1] = (float)atan2(-mat[0][2], cy);
2829                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2830                 
2831                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2832                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2833                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2834                 
2835         } else {
2836                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
2837                 eul1[1] = (float)atan2(-mat[0][2], cy);
2838                 eul1[2] = 0.0f;
2839                 
2840                 VecCopyf(eul2, eul1);
2841         }
2842 }
2843
2844 void Mat3ToEul(float tmat[][3], float *eul)
2845 {
2846         float eul1[3], eul2[3];
2847         
2848         mat3_to_eul2(tmat, eul1, eul2);
2849                 
2850         /* return best, which is just the one with lowest values it in */
2851         if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2852                 VecCopyf(eul, eul2);
2853         }
2854         else {
2855                 VecCopyf(eul, eul1);
2856         }
2857 }
2858
2859 void Mat4ToEul(float tmat[][4], float *eul)
2860 {
2861         float tempMat[3][3];
2862
2863         Mat3CpyMat4 (tempMat, tmat);
2864         Mat3Ortho(tempMat);
2865         Mat3ToEul(tempMat, eul);
2866 }
2867
2868 void QuatToEul( float *quat, float *eul)
2869 {
2870         float mat[3][3];
2871         
2872         QuatToMat3(quat, mat);
2873         Mat3ToEul(mat, eul);
2874 }
2875
2876
2877 void EulToQuat( float *eul, float *quat)
2878 {
2879     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2880  
2881     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2882     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2883     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2884     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2885         
2886         quat[0] = cj*cc + sj*ss;
2887         quat[1] = cj*sc - sj*cs;
2888         quat[2] = cj*ss + sj*cc;
2889         quat[3] = cj*cs - sj*sc;
2890 }
2891
2892 void VecRotToMat3( float *vec, float phi, float mat[][3])
2893 {
2894         /* rotation of phi radials around vec */
2895         float vx, vx2, vy, vy2, vz, vz2, co, si;
2896         
2897         vx= vec[0];
2898         vy= vec[1];
2899         vz= vec[2];
2900         vx2= vx*vx;
2901         vy2= vy*vy;
2902         vz2= vz*vz;
2903         co= (float)cos(phi);
2904         si= (float)sin(phi);
2905         
2906         mat[0][0]= vx2+co*(1.0f-vx2);
2907         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2908         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2909         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2910         mat[1][1]= vy2+co*(1.0f-vy2);
2911         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2912         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2913         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2914         mat[2][2]= vz2+co*(1.0f-vz2);
2915         
2916 }
2917
2918 void VecRotToMat4( float *vec, float phi, float mat[][4])
2919 {
2920         float tmat[3][3];
2921         
2922         VecRotToMat3(vec, phi, tmat);
2923         Mat4One(mat);
2924         Mat4CpyMat3(mat, tmat);
2925 }
2926
2927 void VecRotToQuat( float *vec, float phi, float *quat)
2928 {
2929         /* rotation of phi radials around vec */
2930         float si;
2931
2932         quat[1]= vec[0];
2933         quat[2]= vec[1];
2934         quat[3]= vec[2];
2935         
2936         if( Normalize(quat+1) == 0.0) {
2937                 QuatOne(quat);
2938         }
2939         else {
2940                 quat[0]= (float)cos( phi/2.0 );
2941                 si= (float)sin( phi/2.0 );
2942                 quat[1] *= si;
2943                 quat[2] *= si;
2944                 quat[3] *= si;
2945         }
2946 }
2947
2948 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
2949    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
2950    this would return the angle at the elbow */
2951 float VecAngle3(float *v1, float *v2, float *v3)
2952 {
2953         float vec1[3], vec2[3];
2954
2955         VecSubf(vec1, v2, v1);
2956         VecSubf(vec2, v2, v3);
2957         Normalize(vec1);
2958         Normalize(vec2);
2959
2960         return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
2961 }
2962
2963 float VecAngle3_2D(float *v1, float *v2, float *v3)
2964 {
2965         float vec1[2], vec2[2];
2966
2967         vec1[0] = v2[0]-v1[0];
2968         vec1[1] = v2[1]-v1[1];
2969         
2970         vec2[0] = v2[0]-v3[0];
2971         vec2[1] = v2[1]-v3[1];
2972         
2973         Normalize2(vec1);
2974         Normalize2(vec2);
2975
2976         return NormalizedVecAngle2_2D(vec1, vec2) * 180.0/M_PI;
2977 }
2978
2979 /* Return the shortest angle in degrees between the 2 vectors */
2980 float VecAngle2(float *v1, float *v2)
2981 {
2982         float vec1[3], vec2[3];
2983
2984         VecCopyf(vec1, v1);
2985         VecCopyf(vec2, v2);
2986         Normalize(vec1);
2987         Normalize(vec2);
2988
2989         return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
2990 }
2991
2992 float NormalizedVecAngle2(float *v1, float *v2)
2993 {
2994         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
2995         if (Inpf(v1, v2) < 0.0f) {
2996                 float vec[3];
2997                 
2998                 vec[0]= -v2[0];
2999                 vec[1]= -v2[1];
3000                 vec[2]= -v2[2];
3001
3002                 return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
3003         }
3004         else
3005                 return 2.0f*saasin(VecLenf(v2, v1)/2.0);
3006 }
3007
3008 float NormalizedVecAngle2_2D(float *v1, float *v2)
3009 {
3010         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
3011         if (Inp2f(v1, v2) < 0.0f) {
3012                 float vec[2];
3013                 
3014                 vec[0]= -v2[0];
3015                 vec[1]= -v2[1];
3016
3017                 return (float)M_PI - 2.0f*saasin(Vec2Lenf(vec, v1)/2.0f);
3018         }
3019         else
3020                 return 2.0f*saasin(Vec2Lenf(v2, v1)/2.0);
3021 }
3022
3023 void euler_rot(float *beul, float ang, char axis)
3024 {
3025         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
3026         
3027         eul[0]= eul[1]= eul[2]= 0.0;
3028         if(axis=='x') eul[0]= ang;
3029         else if(axis=='y') eul[1]= ang;
3030         else eul[2]= ang;
3031         
3032         EulToMat3(eul, mat1);
3033         EulToMat3(beul, mat2);
3034         
3035         Mat3MulMat3(totmat, mat2, mat1);
3036         
3037         Mat3ToEul(totmat, beul);
3038         
3039 }
3040
3041 /* exported to transform.c */
3042 void compatible_eul(float *eul, float *oldrot)
3043 {
3044         float dx, dy, dz;
3045         
3046         /* correct differences of about 360 degrees first */
3047         
3048         dx= eul[0] - oldrot[0];
3049         dy= eul[1] - oldrot[1];
3050         dz= eul[2] - oldrot[2];
3051         
3052         while( fabs(dx) > 5.1) {
3053                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
3054                 dx= eul[0] - oldrot[0];
3055         }
3056         while( fabs(dy) > 5.1) {
3057                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
3058                 dy= eul[1] - oldrot[1];
3059         }
3060         while( fabs(dz) > 5.1 ) {
3061                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
3062                 dz= eul[2] - oldrot[2];
3063         }
3064         
3065         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
3066         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
3067                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
3068         }
3069         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
3070                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
3071         }
3072         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
3073                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
3074         }
3075         
3076         /* the method below was there from ancient days... but why! probably because the code sucks :)
3077                 */
3078 #if 0   
3079         /* calc again */
3080         dx= eul[0] - oldrot[0];
3081         dy= eul[1] - oldrot[1];
3082         dz= eul[2] - oldrot[2];
3083         
3084         /* special case, tested for x-z  */
3085         
3086         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
3087                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3088                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
3089                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3090                 
3091         }
3092         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
3093                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3094                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3095                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
3096         }
3097         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
3098                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
3099                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3100                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3101         }
3102 #endif  
3103 }
3104
3105 /* uses 2 methods to retrieve eulers, and picks the closest */
3106 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
3107 {
3108         float eul1[3], eul2[3];
3109         float d1, d2;
3110         
3111         mat3_to_eul2(mat, eul1, eul2);
3112         
3113         compatible_eul(eul1, oldrot);
3114         compatible_eul(eul2, oldrot);
3115         
3116         d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
3117         d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
3118         
3119         /* return best, which is just the one with lowest difference */
3120         if( d1 > d2) {
3121                 VecCopyf(eul, eul2);
3122         }
3123         else {
3124                 VecCopyf(eul, eul1);
3125         }
3126         
3127 }
3128
3129 /* ******************************************** */
3130
3131 void SizeToMat3( float *size, float mat[][3])
3132 {
3133         mat[0][0]= size[0];
3134         mat[0][1]= 0.0;
3135         mat[0][2]= 0.0;
3136         mat[1][1]= size[1];
3137         mat[1][0]= 0.0;
3138         mat[1][2]= 0.0;
3139         mat[2][2]= size[2];
3140         mat[2][1]= 0.0;
3141         mat[2][0]= 0.0;
3142 }
3143
3144 void SizeToMat4( float *size, float mat[][4])
3145 {
3146         float tmat[3][3];
3147         
3148         SizeToMat3(size, tmat);
3149         Mat4One(mat);
3150         Mat4CpyMat3(mat, tmat);
3151 }
3152
3153 void Mat3ToSize( float mat[][3], float *size)
3154 {
3155         size[0]= VecLength(mat[0]);
3156         size[1]= VecLength(mat[1]);
3157         size[2]= VecLength(mat[2]);
3158 }
3159
3160 void Mat4ToSize( float mat[][4], float *size)
3161 {
3162         size[0]= VecLength(mat[0]);
3163         size[1]= VecLength(mat[1]);
3164         size[2]= VecLength(mat[2]);
3165 }
3166
3167 /* this gets the average scale of a matrix, only use when your scaling
3168  * data that has no idea of scale axis, examples are bone-envelope-radius
3169  * and curve radius */
3170 float Mat3ToScalef(float mat[][3])
3171 {
3172         /* unit length vector */
3173         float unit_vec[3] = {0.577350269189626, 0.577350269189626, 0.577350269189626};
3174         Mat3MulVecfl(mat, unit_vec);
3175         return VecLength(unit_vec);
3176 }
3177
3178 float Mat4ToScalef(float mat[][4])
3179 {
3180         float tmat[3][3];
3181         Mat3CpyMat4(tmat, mat);
3182         return Mat3ToScalef(tmat);
3183 }
3184
3185
3186 /* ************* SPECIALS ******************* */
3187
3188 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
3189 {
3190         /* imaginary x-axis, y-axis triangle is being rotated */
3191         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
3192         
3193         /* move z-axis to face-normal */
3194         CalcNormFloat(v1, v2, v3, vec);
3195
3196         n[0]= vec[1];
3197         n[1]= -vec[0];
3198         n[2]= 0.0;
3199         Normalize(n);
3200         
3201         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
3202         
3203         angle= -0.5f*saacos(vec[2]);
3204         co= (float)cos(angle);
3205         si= (float)sin(angle);
3206         q1[0]= co;
3207         q1[1]= n[0]*si;
3208         q1[2]= n[1]*si;
3209         q1[3]= 0.0f;
3210         
3211         /* rotate back line v1-v2 */
3212         QuatToMat3(q1, mat);
3213         Mat3Inv(imat, mat);
3214         VecSubf(vec, v2, v1);
3215         Mat3MulVecfl(imat, vec);
3216
3217         /* what angle has this line with x-axis? */
3218         vec[2]= 0.0;
3219         Normalize(vec);
3220
3221         angle= (float)(0.5*atan2(vec[1], vec[0]));
3222         co= (float)cos(angle);
3223         si= (float)sin(angle);
3224         q2[0]= co;
3225         q2[1]= 0.0f;
3226         q2[2]= 0.0f;
3227         q2[3]= si;
3228         
3229         QuatMul(quat, q1, q2);
3230 }
3231
3232 void MinMaxRGB(short c[])
3233 {
3234         if(c[0]>255) c[0]=255;
3235         else if(c[0]<0) c[0]=0;
3236         if(c[1]>255) c[1]=255;
3237         else if(c[1]<0) c[1]=0;
3238         if(c[2]>255) c[2]=255;
3239         else if(c[2]<0) c[2]=0;
3240 }
3241
3242 float Vec2Lenf(float *v1, float *v2)
3243 {
3244         float x, y;
3245
3246         x = v1[0]-v2[0];
3247         y = v1[1]-v2[1];
3248         return (float)sqrt(x*x+y*y);
3249 }
3250
3251 float Vec2Length(float *v)
3252 {
3253         return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
3254 }
3255
3256 void Vec2Mulf(float *v1, float f)
3257 {
3258         v1[0]*= f;
3259         v1[1]*= f;
3260 }
3261
3262 void Vec2Addf(float *v, float *v1, float *v2)
3263 {
3264         v[0]= v1[0]+ v2[0];
3265         v[1]= v1[1]+ v2[1];
3266 }
3267
3268 void Vec2Subf(float *v, float *v1, float *v2)
3269 {
3270         v[0]= v1[0]- v2[0];
3271         v[1]= v1[1]- v2[1];
3272 }
3273
3274 void Vec2Copyf(float *v1, float *v2)
3275 {
3276         v1[0]= v2[0];
3277         v1[1]= v2[1];
3278 }
3279
3280 float Inp2f(float *v1, float *v2)
3281 {
3282         return v1[0]*v2[0]+v1[1]*v2[1];
3283 }
3284
3285 float Normalize2(float *n)
3286 {
3287         float d;
3288         
3289         d= n[0]*n[0]+n[1]*n[1];
3290
3291         if(d>1.0e-35F) {
3292                 d= (float)sqrt(d);
3293
3294                 n[0]/=d; 
3295                 n[1]/=d; 
3296         } else {
3297                 n[0]=n[1]= 0.0;
3298                 d= 0.0;
3299         }
3300         return d;
3301 }
3302
3303 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
3304 {
3305         int i;
3306         float f, p, q, t;
3307
3308         h *= 360.0f;
3309         
3310         if(s==0.0) {
3311                 *r = v;
3312                 *g = v;
3313                 *b = v;
3314         }
3315         else {
3316                 if(h==360) h = 0;
3317                 
3318                 h /= 60;
3319                 i = (int)floor(h);
3320                 f = h - i;
3321                 p = v*(1.0f-s);
3322                 q = v*(1.0f-(s*f));
3323                 t = v*(1.0f-(s*(1.0f-f)));
3324                 
3325                 switch (i) {
3326                 case 0 :
3327                         *r = v;
3328                         *g = t;
3329                         *b = p;
3330                         break;
3331                 case 1 :
3332                         *r = q;
3333                         *g = v;
3334                         *b = p;
3335                         break;
3336                 case 2 :
3337                         *r = p;
3338                         *g = v;
3339                         *b = t;
3340                         break;
3341                 case 3 :
3342                         *r = p;
3343                         *g = q;
3344                         *b = v;
3345                         break;
3346                 case 4 :
3347                         *r = t;
3348                         *g = p;
3349                         *b = v;
3350                         break;
3351                 case 5 :
3352                         *r = v;
3353                         *g = p;
3354                         *b = q;
3355                         break;
3356                 }
3357         }
3358 }
3359
3360 void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
3361 {
3362         float y, u, v;
3363         y= 0.299*r + 0.587*g + 0.114*b;
3364         u=-0.147*r - 0.289*g + 0.436*b;
3365         v= 0.615*r - 0.515*g - 0.100*b;
3366         
3367         *ly=y;
3368         *lu=u;
3369         *lv=v;
3370 }
3371
3372 void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
3373 {
3374         float r, g, b;
3375         r=y+1.140*v;
3376         g=y-0.394*u - 0.581*v;
3377         b=y+2.032*u;
3378         
3379         *lr=r;
3380         *lg=g;
3381         *lb=b;
3382 }
3383
3384 void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
3385 {
3386         float sr,sg, sb;
3387         float y, cr, cb;
3388         
3389         sr=255.0*r;
3390         sg=255.0*g;
3391         sb=255.0*b;
3392         
3393         
3394         y=(0.257*sr)+(0.504*sg)+(0.098*sb)+16.0;
3395         cb=(-0.148*sr)-(0.291*sg)+(0.439*sb)+128.0;
3396         cr=(0.439*sr)-(0.368*sg)-(0.071*sb)+128.0;
3397         
3398         *ly=y;
3399         *lcb=cb;
3400         *lcr=cr;
3401 }
3402
3403 void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
3404 {
3405         float r,g,b;
3406         
3407         r=1.164*(y-16)+1.596*(cr-128);
3408         g=1.164*(y-16)-0.813*(cr-128)-0.392*(cb-128);
3409         b=1.164*(y-16)+2.017*(cb-128);
3410         
3411         *lr=r/255.0;
3412         *lg=g/255.0;
3413         *lb=b/255.0;
3414 }
3415
3416 void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
3417 {
3418         unsigned int ri, gi, bi;
3419         
3420         if (hexcol[0] == '#') hexcol++;
3421         
3422         if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
3423                 *r = ri / 255.0;
3424                 *g = gi / 255.0;                
3425                 *b = bi / 255.0;
3426         }
3427 }
3428
3429 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
3430 {
3431         float h, s, v;
3432         float cmax, cmin, cdelta;
3433         float rc, gc, bc;
3434
3435         cmax = r;
3436         cmin = r;
3437         cmax = (g>cmax ? g:cmax);
3438         cmin = (g<cmin ? g:cmin);
3439         cmax = (b>cmax ? b:cmax);
3440         cmin = (b<cmin ? b:cmin);
3441
3442         v = cmax;               /* value */
3443         if (cmax!=0.0)
3444                 s = (cmax - cmin)/cmax;
3445         else {
3446                 s = 0.0;
3447                 h = 0.0;
3448         }
3449         if (s == 0.0)
3450                 h = -1.0;
3451         else {
3452                 cdelta = cmax-cmin;
3453                 rc = (cmax-r)/cdelta;
3454                 gc = (cmax-g)/cdelta;
3455                 bc = (cmax-b)/cdelta;
3456                 if (r==cmax)
3457                         h = bc-gc;
3458                 else
3459                         if (g==cmax)
3460                                 h = 2.0f+rc-bc;
3461                         else
3462                                 h = 4.0f+gc-rc;
3463                 h = h*60.0f;
3464                 if (h<0.0f)
3465                         h += 360.0f;
3466         }
3467         
3468         *ls = s;
3469         *lh = h/360.0f;
3470         if( *lh < 0.0) *lh= 0.0;
3471         *lv = v;
3472 }
3473
3474 /*http://brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html */
3475
3476 void xyz_to_rgb(float xc, float yc, float zc, float *r, float *g, float *b, int colorspace)
3477 {
3478         switch (colorspace) { 
3479         case BLI_CS_SMPTE:
3480                 *r = (3.50570   * xc) + (-1.73964       * yc) + (-0.544011      * zc);
3481                 *g = (-1.06906  * xc) + (1.97781        * yc) + (0.0351720      * zc);
3482                 *b = (0.0563117 * xc) + (-0.196994      * yc) + (1.05005        * zc);
3483                 break;
3484         case BLI_CS_REC709:
3485                 *r = (3.240476  * xc) + (-1.537150      * yc) + (-0.498535      * zc);
3486                 *g = (-0.969256 * xc) + (1.875992 * yc) + (0.041556 * zc);
3487                 *b = (0.055648  * xc) + (-0.204043      * yc) + (1.057311       * zc);
3488                 break;
3489         case BLI_CS_CIE:
3490                 *r = (2.28783848734076f * xc) + (-0.833367677835217f    * yc) + (-0.454470795871421f    * zc);
3491                 *g = (-0.511651380743862f * xc) + (1.42275837632178f * yc) + (0.0888930017552939f * zc);
3492                 *b = (0.00572040983140966f      * xc) + (-0.0159068485104036f   * yc) + (1.0101864083734f       * zc);
3493                 break;
3494         }
3495 }
3496
3497 /*If the requested RGB shade contains a negative weight for
3498   one of the primaries, it lies outside the colour gamut 
3499   accessible from the given triple of primaries.  Desaturate
3500   it by adding white, equal quantities of R, G, and B, enough
3501   to make RGB all positive.  The function returns 1 if the
3502   components were modified, zero otherwise.*/
3503 int constrain_rgb(float *r, float *g, float *b)
3504 {
3505         float w;
3506
3507     /* Amount of white needed is w = - min(0, *r, *g, *b) */
3508     
3509     w = (0 < *r) ? 0 : *r;
3510     w = (w < *g) ? w : *g;
3511     w = (w < *b) ? w : *b;
3512     w = -w;
3513
3514     /* Add just enough white to make r, g, b all positive. */
3515     
3516     if (w > 0) {
3517         *r += w;  *g += w; *b += w;
3518         return 1;                     /* Colour modified to fit RGB gamut */
3519     }
3520
3521     return 0;                         /* Colour within RGB gamut */
3522 }
3523
3524 /*Transform linear RGB values to nonlinear RGB values. Rec.
3525   709 is ITU-R Recommendation BT. 709 (1990) ``Basic
3526   Parameter Values for the HDTV Standard for the Studio and
3527   for International Programme Exchange'', formerly CCIR Rec.
3528   709.*/
3529 static void gamma_correct(float *c)
3530 {
3531         /* Rec. 709 gamma correction. */
3532         float cc = 0.018;
3533         
3534         if (*c < cc) {
3535             *c *= ((1.099 * pow(cc, 0.45)) - 0.099) / cc;
3536         } else {
3537             *c = (1.099 * pow(*c, 0.45)) - 0.099;
3538         }
3539 }
3540
3541 void gamma_correct_rgb(float *r, float *g, float *b)
3542 {
3543     gamma_correct(r);
3544     gamma_correct(g);
3545     gamma_correct(b);
3546 }
3547
3548
3549 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
3550    for that reason it is sensitive for endianness... with this function it works correctly
3551 */
3552
3553 unsigned int hsv_to_cpack(float h, float s, float v)
3554 {
3555         short r, g, b;
3556         float rf, gf, bf;
3557         unsigned int col;
3558         
3559         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
3560         
3561         r= (short)(rf*255.0f);
3562         g= (short)(gf*255.0f);
3563         b= (short)(bf*255.0f);
3564         
3565         col= ( r + (g*256) + (b*256*256) );
3566         return col;
3567 }
3568
3569
3570 unsigned int rgb_to_cpack(float r, float g, float b)
3571 {
3572         int ir, ig, ib;
3573         
3574         ir= (int)floor(255.0*r);
3575         if(ir<0) ir= 0; else if(ir>255) ir= 255;
3576         ig= (int)floor(255.0*g);
3577         if(ig<0) ig= 0; else if(ig>255) ig= 255;
3578         ib= (int)floor(255.0*b);
3579         if(ib<0) ib= 0; else if(ib>255) ib= 255;
3580         
3581         return (ir+ (ig*256) + (ib*256*256));
3582 }
3583
3584 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
3585 {
3586         
3587         *r= (float)((col)&0xFF);
3588         *r /= 255.0f;
3589
3590         *g= (float)(((col)>>8)&0xFF);
3591         *g /= 255.0f;
3592
3593         *b= (float)(((col)>>16)&0xFF);
3594         *b /= 255.0f;
3595 }
3596
3597
3598 /* *************** PROJECTIONS ******************* */
3599
3600 void tubemap(float x, float y, float z, float *u, float *v)
3601 {
3602         float len;
3603         
3604         *v = (z + 1.0) / 2.0;