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