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