remove MTC_ functions, (don't merge)
[blender-staging.git] / source / blender / blenlib / intern / arithb.c
1 /* arithb.c
2  *
3  * simple math for blender code
4  *
5  * sort of cleaned up mar-01 nzc
6  *
7  * $Id$
8  *
9  * ***** BEGIN GPL LICENSE BLOCK *****
10  *
11  * This program is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU General Public License
13  * as published by the Free Software Foundation; either version 2
14  * of the License, or (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
24  *
25  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
26  * All rights reserved.
27  *
28  * The Original Code is: all of this file.
29  *
30  * Contributor(s): none yet.
31  *
32  * ***** END GPL LICENSE BLOCK *****
33  */
34
35 /* ************************ FUNKTIES **************************** */
36
37 #include <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 VecLerpf(float *target, float *a, float *b, float t)
2155 {
2156         float s = 1.0f-t;
2157
2158         target[0]= s*a[0] + t*b[0];
2159         target[1]= s*a[1] + t*b[1];
2160         target[2]= s*a[2] + t*b[2];
2161 }
2162
2163 void Vec2Lerpf(float *target, float *a, float *b, float t)
2164 {
2165         float s = 1.0f-t;
2166
2167         target[0]= s*a[0] + t*b[0];
2168         target[1]= s*a[1] + t*b[1];
2169 }
2170
2171 void VecMidf(float *v, float *v1, float *v2)
2172 {
2173         v[0]= 0.5f*(v1[0]+ v2[0]);
2174         v[1]= 0.5f*(v1[1]+ v2[1]);
2175         v[2]= 0.5f*(v1[2]+ v2[2]);
2176 }
2177
2178 void VecMulf(float *v1, float f)
2179 {
2180
2181         v1[0]*= f;
2182         v1[1]*= f;
2183         v1[2]*= f;
2184 }
2185
2186 void VecNegf(float *v1)
2187 {
2188         v1[0] = -v1[0];
2189         v1[1] = -v1[1];
2190         v1[2] = -v1[2];
2191 }
2192
2193 void VecOrthoBasisf(float *v, float *v1, float *v2)
2194 {
2195         float f = sqrt(v[0]*v[0] + v[1]*v[1]);
2196
2197         if (f < 1e-35f) {
2198                 // degenerate case
2199                 v1[0] = 0.0f; v1[1] = 1.0f; v1[2] = 0.0f;
2200                 if (v[2] > 0.0f) {
2201                         v2[0] = 1.0f; v2[1] = v2[2] = 0.0f;
2202                 }
2203                 else {
2204                         v2[0] = -1.0f; v2[1] = v2[2] = 0.0f;
2205                 }
2206         }
2207         else  {
2208                 f = 1.0f/f;
2209                 v1[0] = v[1]*f;
2210                 v1[1] = -v[0]*f;
2211                 v1[2] = 0.0f;
2212
2213                 Crossf(v2, v, v1);
2214         }
2215 }
2216
2217 int VecLenCompare(float *v1, float *v2, float limit)
2218 {
2219     float x,y,z;
2220
2221         x=v1[0]-v2[0];
2222         y=v1[1]-v2[1];
2223         z=v1[2]-v2[2];
2224
2225         return ((x*x + y*y + z*z) < (limit*limit));
2226 }
2227
2228 int VecCompare( float *v1, float *v2, float limit)
2229 {
2230         if( fabs(v1[0]-v2[0])<limit )
2231                 if( fabs(v1[1]-v2[1])<limit )
2232                         if( fabs(v1[2]-v2[2])<limit ) return 1;
2233         return 0;
2234 }
2235
2236 int VecEqual(float *v1, float *v2)
2237 {
2238         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
2239 }
2240
2241 int VecIsNull(float *v)
2242 {
2243         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
2244 }
2245
2246 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
2247 {
2248         float n1[3],n2[3];
2249
2250         n1[0]= (float)(v1[0]-v2[0]);
2251         n2[0]= (float)(v2[0]-v3[0]);
2252         n1[1]= (float)(v1[1]-v2[1]);
2253         n2[1]= (float)(v2[1]-v3[1]);
2254         n1[2]= (float)(v1[2]-v2[2]);
2255         n2[2]= (float)(v2[2]-v3[2]);
2256         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2257         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2258         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2259         Normalize(n);
2260 }
2261
2262 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
2263 {
2264         float n1[3],n2[3];
2265
2266         n1[0]= (float)(v1[0]-v2[0]);
2267         n2[0]= (float)(v2[0]-v3[0]);
2268         n1[1]= (float)(v1[1]-v2[1]);
2269         n2[1]= (float)(v2[1]-v3[1]);
2270         n1[2]= (float)(v1[2]-v2[2]);
2271         n2[2]= (float)(v2[2]-v3[2]);
2272         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2273         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2274         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2275         Normalize(n);
2276 }
2277
2278 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
2279 {
2280         float n1[3],n2[3];
2281
2282         n1[0]= v1[0]-v2[0];
2283         n2[0]= v2[0]-v3[0];
2284         n1[1]= v1[1]-v2[1];
2285         n2[1]= v2[1]-v3[1];
2286         n1[2]= v1[2]-v2[2];
2287         n2[2]= v2[2]-v3[2];
2288         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2289         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2290         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2291         return Normalize(n);
2292 }
2293
2294 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
2295 {
2296         /* real cross! */
2297         float n1[3],n2[3];
2298
2299         n1[0]= v1[0]-v3[0];
2300         n1[1]= v1[1]-v3[1];
2301         n1[2]= v1[2]-v3[2];
2302
2303         n2[0]= v2[0]-v4[0];
2304         n2[1]= v2[1]-v4[1];
2305         n2[2]= v2[2]-v4[2];
2306
2307         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
2308         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
2309         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
2310
2311         return Normalize(n);
2312 }
2313
2314
2315 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
2316 {
2317
2318         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
2319         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
2320         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
2321 }
2322
2323 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
2324 {
2325
2326         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
2327         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
2328         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
2329 }
2330
2331 float Sqrt3f(float f)
2332 {
2333         if(f==0.0) return 0;
2334         if(f<0) return (float)(-exp(log(-f)/3));
2335         else return (float)(exp(log(f)/3));
2336 }
2337
2338 double Sqrt3d(double d)
2339 {
2340         if(d==0.0) return 0;
2341         if(d<0) return -exp(log(-d)/3);
2342         else return exp(log(d)/3);
2343 }
2344
2345 void NormalShortToFloat(float *out, short *in)
2346 {
2347         out[0] = in[0] / 32767.0;
2348         out[1] = in[1] / 32767.0;
2349         out[2] = in[2] / 32767.0;
2350 }
2351
2352 void NormalFloatToShort(short *out, float *in)
2353 {
2354         out[0] = (short)(in[0] * 32767.0);
2355         out[1] = (short)(in[1] * 32767.0);
2356         out[2] = (short)(in[2] * 32767.0);
2357 }
2358
2359 /* distance v1 to line v2-v3 */
2360 /* using Hesse formula, NO LINE PIECE! */
2361 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
2362         float a[2],deler;
2363
2364         a[0]= v2[1]-v3[1];
2365         a[1]= v3[0]-v2[0];
2366         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
2367         if(deler== 0.0f) return 0;
2368
2369         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
2370
2371 }
2372
2373 /* distance v1 to line-piece v2-v3 */
2374 float PdistVL2Dfl( float *v1, float *v2, float *v3) 
2375 {
2376         float labda, rc[2], pt[2], len;
2377         
2378         rc[0]= v3[0]-v2[0];
2379         rc[1]= v3[1]-v2[1];
2380         len= rc[0]*rc[0]+ rc[1]*rc[1];
2381         if(len==0.0) {
2382                 rc[0]= v1[0]-v2[0];
2383                 rc[1]= v1[1]-v2[1];
2384                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
2385         }
2386         
2387         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
2388         if(labda<=0.0) {
2389                 pt[0]= v2[0];
2390                 pt[1]= v2[1];
2391         }
2392         else if(labda>=1.0) {
2393                 pt[0]= v3[0];
2394                 pt[1]= v3[1];
2395         }
2396         else {
2397                 pt[0]= labda*rc[0]+v2[0];
2398                 pt[1]= labda*rc[1]+v2[1];
2399         }
2400
2401         rc[0]= pt[0]-v1[0];
2402         rc[1]= pt[1]-v1[1];
2403         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
2404 }
2405
2406 float AreaF2Dfl( float *v1, float *v2, float *v3)
2407 {
2408         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
2409 }
2410
2411
2412 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
2413 {
2414         float len, vec1[3], vec2[3], n[3];
2415
2416         VecSubf(vec1, v2, v1);
2417         VecSubf(vec2, v4, v1);
2418         Crossf(n, vec1, vec2);
2419         len= Normalize(n);
2420
2421         VecSubf(vec1, v4, v3);
2422         VecSubf(vec2, v2, v3);
2423         Crossf(n, vec1, vec2);
2424         len+= Normalize(n);
2425
2426         return (len/2.0f);
2427 }
2428
2429 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
2430 {
2431         float len, vec1[3], vec2[3], n[3];
2432
2433         VecSubf(vec1, v3, v2);
2434         VecSubf(vec2, v1, v2);
2435         Crossf(n, vec1, vec2);
2436         len= Normalize(n);
2437
2438         return (len/2.0f);
2439 }
2440
2441 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
2442 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
2443
2444
2445 float AreaPoly3Dfl(int nr, float *verts, float *normal)
2446 {
2447         float x, y, z, area, max;
2448         float *cur, *prev;
2449         int a, px=0, py=1;
2450
2451         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
2452         x= (float)fabs(normal[0]);
2453         y= (float)fabs(normal[1]);
2454         z= (float)fabs(normal[2]);
2455         max = MAX3(x, y, z);
2456         if(max==y) py=2;
2457         else if(max==x) {
2458                 px=1; 
2459                 py= 2;
2460         }
2461
2462         /* The Trapezium Area Rule */
2463         prev= verts+3*(nr-1);
2464         cur= verts;
2465         area= 0;
2466         for(a=0; a<nr; a++) {
2467                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
2468                 prev= cur;
2469                 cur+=3;
2470         }
2471
2472         return (float)fabs(0.5*area/max);
2473 }
2474
2475 /* intersect Line-Line, shorts */
2476 short IsectLL2Ds(short *v1, short *v2, short *v3, short *v4)
2477 {
2478         /* return:
2479         -1: colliniar
2480          0: no intersection of segments
2481          1: exact intersection of segments
2482          2: cross-intersection of segments
2483         */
2484         float div, labda, mu;
2485         
2486         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2487         if(div==0.0) return -1;
2488         
2489         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2490         
2491         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2492         
2493         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2494                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2495                 return 2;
2496         }
2497         return 0;
2498 }
2499
2500 /* intersect Line-Line, floats */
2501 short IsectLL2Df(float *v1, float *v2, float *v3, float *v4)
2502 {
2503         /* return:
2504         -1: colliniar
2505 0: no intersection of segments
2506 1: exact intersection of segments
2507 2: cross-intersection of segments
2508         */
2509         float div, labda, mu;
2510         
2511         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
2512         if(div==0.0) return -1;
2513         
2514         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
2515         
2516         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
2517         
2518         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
2519                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
2520                 return 2;
2521         }
2522         return 0;
2523 }
2524
2525 /*
2526 -1: colliniar
2527  1: intersection
2528
2529 */
2530 static short IsectLLPt2Df(float x0,float y0,float x1,float y1,
2531                                          float x2,float y2,float x3,float y3, float *xi,float *yi)
2532
2533 {
2534         /*
2535         this function computes the intersection of the sent lines
2536         and returns the intersection point, note that the function assumes
2537         the lines intersect. the function can handle vertical as well
2538         as horizontal lines. note the function isn't very clever, it simply
2539         applies the math, but we don't need speed since this is a
2540         pre-processing step
2541         */
2542         float c1,c2, // constants of linear equations
2543         det_inv,  // the inverse of the determinant of the coefficient
2544         m1,m2;    // the slopes of each line
2545         /*
2546         compute slopes, note the cludge for infinity, however, this will
2547         be close enough
2548         */
2549         if ( fabs( x1-x0 ) > 0.000001 )
2550                 m1 = ( y1-y0 ) / ( x1-x0 );
2551         else
2552                 return -1; /*m1 = ( float ) 1e+10;*/   // close enough to infinity
2553
2554         if ( fabs( x3-x2 ) > 0.000001 )
2555                 m2 = ( y3-y2 ) / ( x3-x2 );
2556         else
2557                 return -1; /*m2 = ( float ) 1e+10;*/   // close enough to infinity
2558
2559         if (fabs(m1-m2) < 0.000001)
2560                 return -1; /* paralelle lines */
2561         
2562 // compute constants
2563
2564         c1 = ( y0-m1*x0 );
2565         c2 = ( y2-m2*x2 );
2566
2567 // compute the inverse of the determinate
2568
2569         det_inv = 1.0f / ( -m1 + m2 );
2570
2571 // use Kramers rule to compute xi and yi
2572
2573         *xi= ( ( -c2 + c1 ) *det_inv );
2574         *yi= ( ( m2*c1 - m1*c2 ) *det_inv );
2575         
2576         return 1; 
2577 } // end Intersect_Lines
2578
2579 #define SIDE_OF_LINE(pa,pb,pp)  ((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
2580 /* point in tri */
2581 int IsectPT2Df(float pt[2], float v1[2], float v2[2], float v3[2])
2582 {
2583         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
2584                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
2585                         if (SIDE_OF_LINE(v3,v1,pt)>=0.0) {
2586                                 return 1;
2587                         }
2588                 }
2589         } else {
2590                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0) ) {
2591                         if (! (SIDE_OF_LINE(v3,v1,pt)>=0.0)) {
2592                                 return -1;
2593                         }
2594                 }
2595         }
2596         
2597         return 0;
2598 }
2599 /* point in quad - only convex quads */
2600 int IsectPQ2Df(float pt[2], float v1[2], float v2[2], float v3[2], float v4[2])
2601 {
2602         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
2603                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
2604                         if (SIDE_OF_LINE(v3,v4,pt)>=0.0) {
2605                                 if (SIDE_OF_LINE(v4,v1,pt)>=0.0) {
2606                                         return 1;
2607                                 }
2608                         }
2609                 }
2610         } else {
2611                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0) ) {
2612                         if (! (SIDE_OF_LINE(v3,v4,pt)>=0.0)) {
2613                                 if (! (SIDE_OF_LINE(v4,v1,pt)>=0.0)) {
2614                                         return -1;
2615                                 }
2616                         }
2617                 }
2618         }
2619         
2620         return 0;
2621 }
2622
2623
2624 /**
2625  * 
2626  * @param min 
2627  * @param max 
2628  * @param vec 
2629  */
2630 void MinMax3(float *min, float *max, float *vec)
2631 {
2632         if(min[0]>vec[0]) min[0]= vec[0];
2633         if(min[1]>vec[1]) min[1]= vec[1];
2634         if(min[2]>vec[2]) min[2]= vec[2];
2635
2636         if(max[0]<vec[0]) max[0]= vec[0];
2637         if(max[1]<vec[1]) max[1]= vec[1];
2638         if(max[2]<vec[2]) max[2]= vec[2];
2639 }
2640
2641 static float TriSignedArea(float *v1, float *v2, float *v3, int i, int j)
2642 {
2643         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
2644 }
2645
2646 static int BarycentricWeights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
2647 {
2648         float xn, yn, zn, a1, a2, a3, asum;
2649         short i, j;
2650
2651         /* find best projection of face XY, XZ or YZ: barycentric weights of
2652            the 2d projected coords are the same and faster to compute */
2653         xn= fabs(n[0]);
2654         yn= fabs(n[1]);
2655         zn= fabs(n[2]);
2656         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
2657         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
2658         else {i= 1; j= 2;} 
2659
2660         a1= TriSignedArea(v2, v3, co, i, j);
2661         a2= TriSignedArea(v3, v1, co, i, j);
2662         a3= TriSignedArea(v1, v2, co, i, j);
2663
2664         asum= a1 + a2 + a3;
2665
2666         if (fabs(asum) < FLT_EPSILON) {
2667                 /* zero area triangle */
2668                 w[0]= w[1]= w[2]= 1.0f/3.0f;
2669                 return 1;
2670         }
2671
2672         asum= 1.0f/asum;
2673         w[0]= a1*asum;
2674         w[1]= a2*asum;
2675         w[2]= a3*asum;
2676
2677         return 0;
2678 }
2679
2680 void InterpWeightsQ3Dfl(float *v1, float *v2, float *v3, float *v4, float *co, float *w)
2681 {
2682         float w2[3];
2683
2684         w[0]= w[1]= w[2]= w[3]= 0.0f;
2685
2686         /* first check for exact match */
2687         if(VecEqual(co, v1))
2688                 w[0]= 1.0f;
2689         else if(VecEqual(co, v2))
2690                 w[1]= 1.0f;
2691         else if(VecEqual(co, v3))
2692                 w[2]= 1.0f;
2693         else if(v4 && VecEqual(co, v4))
2694                 w[3]= 1.0f;
2695         else {
2696                 /* otherwise compute barycentric interpolation weights */
2697                 float n1[3], n2[3], n[3];
2698                 int degenerate;
2699
2700                 VecSubf(n1, v1, v3);
2701                 if (v4) {
2702                         VecSubf(n2, v2, v4);
2703                 }
2704                 else {
2705                         VecSubf(n2, v2, v3);
2706                 }
2707                 Crossf(n, n1, n2);
2708
2709                 /* OpenGL seems to split this way, so we do too */
2710                 if (v4) {
2711                         degenerate= BarycentricWeights(v1, v2, v4, co, n, w);
2712                         SWAP(float, w[2], w[3]);
2713
2714                         if(degenerate || (w[0] < 0.0f)) {
2715                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
2716                                    so we interpolate using the other triangle */
2717                                 degenerate= BarycentricWeights(v2, v3, v4, co, n, w2);
2718
2719                                 if(!degenerate) {
2720                                         w[0]= 0.0f;
2721                                         w[1]= w2[0];
2722                                         w[2]= w2[1];
2723                                         w[3]= w2[2];
2724                                 }
2725                         }
2726                 }
2727                 else
2728                         BarycentricWeights(v1, v2, v3, co, n, w);
2729         }
2730 }
2731
2732 /* Mean value weights - smooth interpolation weights for polygons with
2733  * more than 3 vertices */
2734 static float MeanValueHalfTan(float *v1, float *v2, float *v3)
2735 {
2736         float d2[3], d3[3], cross[3], area, dot, len;
2737
2738         VecSubf(d2, v2, v1);
2739         VecSubf(d3, v3, v1);
2740         Crossf(cross, d2, d3);
2741
2742         area= VecLength(cross);
2743         dot= Inpf(d2, d3);
2744         len= VecLength(d2)*VecLength(d3);
2745
2746         if(area == 0.0f)
2747                 return 0.0f;
2748         else
2749                 return (len - dot)/area;
2750 }
2751
2752 void MeanValueWeights(float v[][3], int n, float *co, float *w)
2753 {
2754         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
2755         int i;
2756
2757         totweight= 0.0f;
2758
2759         for(i=0; i<n; i++) {
2760                 vmid= v[i];
2761                 vprev= (i == 0)? v[n-1]: v[i-1];
2762                 vnext= (i == n-1)? v[0]: v[i+1];
2763
2764                 t1= MeanValueHalfTan(co, vprev, vmid);
2765                 t2= MeanValueHalfTan(co, vmid, vnext);
2766
2767                 len= VecLenf(co, vmid);
2768                 w[i]= (t1+t2)/len;
2769                 totweight += w[i];
2770         }
2771
2772         if(totweight != 0.0f)
2773                 for(i=0; i<n; i++)
2774                         w[i] /= totweight;
2775 }
2776
2777
2778 /* ************ EULER *************** */
2779
2780 void EulToMat3( float *eul, float mat[][3])
2781 {
2782         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2783         
2784         ci = cos(eul[0]); 
2785         cj = cos(eul[1]); 
2786         ch = cos(eul[2]);
2787         si = sin(eul[0]); 
2788         sj = sin(eul[1]); 
2789         sh = sin(eul[2]);
2790         cc = ci*ch; 
2791         cs = ci*sh; 
2792         sc = si*ch; 
2793         ss = si*sh;
2794
2795         mat[0][0] = (float)(cj*ch); 
2796         mat[1][0] = (float)(sj*sc-cs); 
2797         mat[2][0] = (float)(sj*cc+ss);
2798         mat[0][1] = (float)(cj*sh); 
2799         mat[1][1] = (float)(sj*ss+cc); 
2800         mat[2][1] = (float)(sj*cs-sc);
2801         mat[0][2] = (float)-sj;  
2802         mat[1][2] = (float)(cj*si);    
2803         mat[2][2] = (float)(cj*ci);
2804
2805 }
2806
2807 void EulToMat4( float *eul,float mat[][4])
2808 {
2809         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2810         
2811         ci = cos(eul[0]); 
2812         cj = cos(eul[1]); 
2813         ch = cos(eul[2]);
2814         si = sin(eul[0]); 
2815         sj = sin(eul[1]); 
2816         sh = sin(eul[2]);
2817         cc = ci*ch; 
2818         cs = ci*sh; 
2819         sc = si*ch; 
2820         ss = si*sh;
2821
2822         mat[0][0] = (float)(cj*ch); 
2823         mat[1][0] = (float)(sj*sc-cs); 
2824         mat[2][0] = (float)(sj*cc+ss);
2825         mat[0][1] = (float)(cj*sh); 
2826         mat[1][1] = (float)(sj*ss+cc); 
2827         mat[2][1] = (float)(sj*cs-sc);
2828         mat[0][2] = (float)-sj;  
2829         mat[1][2] = (float)(cj*si);    
2830         mat[2][2] = (float)(cj*ci);
2831
2832
2833         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
2834         mat[3][3]= 1.0f;
2835 }
2836
2837 /* returns two euler calculation methods, so we can pick the best */
2838 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
2839 {
2840         float cy, quat[4], mat[3][3];
2841         
2842         Mat3ToQuat(tmat, quat);
2843         QuatToMat3(quat, mat);
2844         Mat3CpyMat3(mat, tmat);
2845         Mat3Ortho(mat);
2846         
2847         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
2848         
2849         if (cy > 16.0*FLT_EPSILON) {
2850                 
2851                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
2852                 eul1[1] = (float)atan2(-mat[0][2], cy);
2853                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
2854                 
2855                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
2856                 eul2[1] = (float)atan2(-mat[0][2], -cy);
2857                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
2858                 
2859         } else {
2860                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
2861                 eul1[1] = (float)atan2(-mat[0][2], cy);
2862                 eul1[2] = 0.0f;
2863                 
2864                 VecCopyf(eul2, eul1);
2865         }
2866 }
2867
2868 void Mat3ToEul(float tmat[][3], float *eul)
2869 {
2870         float eul1[3], eul2[3];
2871         
2872         mat3_to_eul2(tmat, eul1, eul2);
2873                 
2874         /* return best, which is just the one with lowest values it in */
2875         if( fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
2876                 VecCopyf(eul, eul2);
2877         }
2878         else {
2879                 VecCopyf(eul, eul1);
2880         }
2881 }
2882
2883 void Mat4ToEul(float tmat[][4], float *eul)
2884 {
2885         float tempMat[3][3];
2886
2887         Mat3CpyMat4 (tempMat, tmat);
2888         Mat3Ortho(tempMat);
2889         Mat3ToEul(tempMat, eul);
2890 }
2891
2892 void QuatToEul( float *quat, float *eul)
2893 {
2894         float mat[3][3];
2895         
2896         QuatToMat3(quat, mat);
2897         Mat3ToEul(mat, eul);
2898 }
2899
2900
2901 void EulToQuat( float *eul, float *quat)
2902 {
2903     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2904  
2905     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2906     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2907     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2908     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2909         
2910         quat[0] = cj*cc + sj*ss;
2911         quat[1] = cj*sc - sj*cs;
2912         quat[2] = cj*ss + sj*cc;
2913         quat[3] = cj*cs - sj*sc;
2914 }
2915
2916 void VecRotToMat3( float *vec, float phi, float mat[][3])
2917 {
2918         /* rotation of phi radials around vec */
2919         float vx, vx2, vy, vy2, vz, vz2, co, si;
2920         
2921         vx= vec[0];
2922         vy= vec[1];
2923         vz= vec[2];
2924         vx2= vx*vx;
2925         vy2= vy*vy;
2926         vz2= vz*vz;
2927         co= (float)cos(phi);
2928         si= (float)sin(phi);
2929         
2930         mat[0][0]= vx2+co*(1.0f-vx2);
2931         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2932         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2933         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2934         mat[1][1]= vy2+co*(1.0f-vy2);
2935         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2936         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2937         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2938         mat[2][2]= vz2+co*(1.0f-vz2);
2939         
2940 }
2941
2942 void VecRotToMat4( float *vec, float phi, float mat[][4])
2943 {
2944         float tmat[3][3];
2945         
2946         VecRotToMat3(vec, phi, tmat);
2947         Mat4One(mat);
2948         Mat4CpyMat3(mat, tmat);
2949 }
2950
2951 void VecRotToQuat( float *vec, float phi, float *quat)
2952 {
2953         /* rotation of phi radials around vec */
2954         float si;
2955
2956         quat[1]= vec[0];
2957         quat[2]= vec[1];
2958         quat[3]= vec[2];
2959         
2960         if( Normalize(quat+1) == 0.0) {
2961                 QuatOne(quat);
2962         }
2963         else {
2964                 quat[0]= (float)cos( phi/2.0 );
2965                 si= (float)sin( phi/2.0 );
2966                 quat[1] *= si;
2967                 quat[2] *= si;
2968                 quat[3] *= si;
2969         }
2970 }
2971
2972 /* get a direction from 3 vectors that wont depend
2973  * on the distance between the points */
2974 void Vec3ToTangent(float *v, float *v1, float *v2, float *v3)
2975 {
2976         float d_12[3], d_23[3];
2977         VecSubf(d_12, v2, v1);
2978         VecSubf(d_23, v3, v2);
2979         Normalize(d_12);
2980         Normalize(d_23);
2981         VecAddf(v, d_12, d_23);
2982         Normalize(v);
2983 }
2984
2985 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
2986    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
2987    this would return the angle at the elbow */
2988 float VecAngle3(float *v1, float *v2, float *v3)
2989 {
2990         float vec1[3], vec2[3];
2991
2992         VecSubf(vec1, v2, v1);
2993         VecSubf(vec2, v2, v3);
2994         Normalize(vec1);
2995         Normalize(vec2);
2996
2997         return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
2998 }
2999
3000 float VecAngle3_2D(float *v1, float *v2, float *v3)
3001 {
3002         float vec1[2], vec2[2];
3003
3004         vec1[0] = v2[0]-v1[0];
3005         vec1[1] = v2[1]-v1[1];
3006         
3007         vec2[0] = v2[0]-v3[0];
3008         vec2[1] = v2[1]-v3[1];
3009         
3010         Normalize2(vec1);
3011         Normalize2(vec2);
3012
3013         return NormalizedVecAngle2_2D(vec1, vec2) * 180.0/M_PI;
3014 }
3015
3016 /* Return the shortest angle in degrees between the 2 vectors */
3017 float VecAngle2(float *v1, float *v2)
3018 {
3019         float vec1[3], vec2[3];
3020
3021         VecCopyf(vec1, v1);
3022         VecCopyf(vec2, v2);
3023         Normalize(vec1);
3024         Normalize(vec2);
3025
3026         return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
3027 }
3028
3029 float NormalizedVecAngle2(float *v1, float *v2)
3030 {
3031         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
3032         if (Inpf(v1, v2) < 0.0f) {
3033                 float vec[3];
3034                 
3035                 vec[0]= -v2[0];
3036                 vec[1]= -v2[1];
3037                 vec[2]= -v2[2];
3038
3039                 return (float)M_PI - 2.0f*saasin(VecLenf(vec, v1)/2.0f);
3040         }
3041         else
3042                 return 2.0f*saasin(VecLenf(v2, v1)/2.0);
3043 }
3044
3045 float NormalizedVecAngle2_2D(float *v1, float *v2)
3046 {
3047         /* this is the same as acos(Inpf(v1, v2)), but more accurate */
3048         if (Inp2f(v1, v2) < 0.0f) {
3049                 float vec[2];
3050                 
3051                 vec[0]= -v2[0];
3052                 vec[1]= -v2[1];
3053
3054                 return (float)M_PI - 2.0f*saasin(Vec2Lenf(vec, v1)/2.0f);
3055         }
3056         else
3057                 return 2.0f*saasin(Vec2Lenf(v2, v1)/2.0);
3058 }
3059
3060 void euler_rot(float *beul, float ang, char axis)
3061 {
3062         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
3063         
3064         eul[0]= eul[1]= eul[2]= 0.0;
3065         if(axis=='x') eul[0]= ang;
3066         else if(axis=='y') eul[1]= ang;
3067         else eul[2]= ang;
3068         
3069         EulToMat3(eul, mat1);
3070         EulToMat3(beul, mat2);
3071         
3072         Mat3MulMat3(totmat, mat2, mat1);
3073         
3074         Mat3ToEul(totmat, beul);
3075         
3076 }
3077
3078 /* exported to transform.c */
3079 void compatible_eul(float *eul, float *oldrot)
3080 {
3081         float dx, dy, dz;
3082         
3083         /* correct differences of about 360 degrees first */
3084         
3085         dx= eul[0] - oldrot[0];
3086         dy= eul[1] - oldrot[1];
3087         dz= eul[2] - oldrot[2];
3088         
3089         while( fabs(dx) > 5.1) {
3090                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
3091                 dx= eul[0] - oldrot[0];
3092         }
3093         while( fabs(dy) > 5.1) {
3094                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
3095                 dy= eul[1] - oldrot[1];
3096         }
3097         while( fabs(dz) > 5.1 ) {
3098                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
3099                 dz= eul[2] - oldrot[2];
3100         }
3101         
3102         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
3103         if( fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6 ) {
3104                 if(dx > 0.0) eul[0] -= 2.0*M_PI; else eul[0]+= 2.0*M_PI;
3105         }
3106         if( fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6 ) {
3107                 if(dy > 0.0) eul[1] -= 2.0*M_PI; else eul[1]+= 2.0*M_PI;
3108         }
3109         if( fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6 ) {
3110                 if(dz > 0.0) eul[2] -= 2.0*M_PI; else eul[2]+= 2.0*M_PI;
3111         }
3112         
3113         /* the method below was there from ancient days... but why! probably because the code sucks :)
3114                 */
3115 #if 0   
3116         /* calc again */
3117         dx= eul[0] - oldrot[0];
3118         dy= eul[1] - oldrot[1];
3119         dz= eul[2] - oldrot[2];
3120         
3121         /* special case, tested for x-z  */
3122         
3123         if( (fabs(dx) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dz) > 3.1 ) ) {
3124                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3125                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
3126                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3127                 
3128         }
3129         else if( (fabs(dx) > 3.1 && fabs(dy) > 1.5 ) || ( fabs(dx) > 1.5 && fabs(dy) > 3.1 ) ) {
3130                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
3131                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3132                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
3133         }
3134         else if( (fabs(dy) > 3.1 && fabs(dz) > 1.5 ) || ( fabs(dy) > 1.5 && fabs(dz) > 3.1 ) ) {
3135                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
3136                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
3137                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
3138         }
3139 #endif  
3140 }
3141
3142 /* uses 2 methods to retrieve eulers, and picks the closest */
3143 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
3144 {
3145         float eul1[3], eul2[3];
3146         float d1, d2;
3147         
3148         mat3_to_eul2(mat, eul1, eul2);
3149         
3150         compatible_eul(eul1, oldrot);
3151         compatible_eul(eul2, oldrot);
3152         
3153         d1= fabs(eul1[0]-oldrot[0]) + fabs(eul1[1]-oldrot[1]) + fabs(eul1[2]-oldrot[2]);
3154         d2= fabs(eul2[0]-oldrot[0]) + fabs(eul2[1]-oldrot[1]) + fabs(eul2[2]-oldrot[2]);
3155         
3156         /* return best, which is just the one with lowest difference */
3157         if( d1 > d2) {
3158                 VecCopyf(eul, eul2);
3159         }
3160         else {
3161                 VecCopyf(eul, eul1);
3162         }
3163         
3164 }
3165
3166 /* ******************************************** */
3167
3168 void SizeToMat3( float *size, float mat[][3])
3169 {
3170         mat[0][0]= size[0];
3171         mat[0][1]= 0.0;
3172         mat[0][2]= 0.0;
3173         mat[1][1]= size[1];
3174         mat[1][0]= 0.0;
3175         mat[1][2]= 0.0;
3176         mat[2][2]= size[2];
3177         mat[2][1]= 0.0;
3178         mat[2][0]= 0.0;
3179 }
3180
3181 void SizeToMat4( float *size, float mat[][4])
3182 {
3183         float tmat[3][3];
3184         
3185         SizeToMat3(size, tmat);
3186         Mat4One(mat);
3187         Mat4CpyMat3(mat, tmat);
3188 }
3189
3190 void Mat3ToSize( float mat[][3], float *size)
3191 {
3192         size[0]= VecLength(mat[0]);
3193         size[1]= VecLength(mat[1]);
3194         size[2]= VecLength(mat[2]);
3195 }
3196
3197 void Mat4ToSize( float mat[][4], float *size)
3198 {
3199         size[0]= VecLength(mat[0]);
3200         size[1]= VecLength(mat[1]);
3201         size[2]= VecLength(mat[2]);
3202 }
3203
3204 /* this gets the average scale of a matrix, only use when your scaling
3205  * data that has no idea of scale axis, examples are bone-envelope-radius
3206  * and curve radius */
3207 float Mat3ToScalef(float mat[][3])
3208 {
3209         /* unit length vector */
3210         float unit_vec[3] = {0.577350269189626, 0.577350269189626, 0.577350269189626};
3211         Mat3MulVecfl(mat, unit_vec);
3212         return VecLength(unit_vec);
3213 }
3214
3215 float Mat4ToScalef(float mat[][4])
3216 {
3217         float tmat[3][3];
3218         Mat3CpyMat4(tmat, mat);
3219         return Mat3ToScalef(tmat);
3220 }
3221
3222
3223 /* ************* SPECIALS ******************* */
3224
3225 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
3226 {
3227         /* imaginary x-axis, y-axis triangle is being rotated */
3228         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
3229         
3230         /* move z-axis to face-normal */
3231         CalcNormFloat(v1, v2, v3, vec);
3232
3233         n[0]= vec[1];
3234         n[1]= -vec[0];
3235         n[2]= 0.0;
3236         Normalize(n);
3237         
3238         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
3239         
3240         angle= -0.5f*saacos(vec[2]);
3241         co= (float)cos(angle);
3242         si= (float)sin(angle);
3243         q1[0]= co;
3244         q1[1]= n[0]*si;
3245         q1[2]= n[1]*si;
3246         q1[3]= 0.0f;
3247         
3248         /* rotate back line v1-v2 */
3249         QuatToMat3(q1, mat);
3250         Mat3Inv(imat, mat);
3251         VecSubf(vec, v2, v1);
3252         Mat3MulVecfl(imat, vec);
3253
3254         /* what angle has this line with x-axis? */
3255         vec[2]= 0.0;
3256         Normalize(vec);
3257
3258         angle= (float)(0.5*atan2(vec[1], vec[0]));
3259         co= (float)cos(angle);
3260         si= (float)sin(angle);
3261         q2[0]= co;
3262         q2[1]= 0.0f;
3263         q2[2]= 0.0f;
3264         q2[3]= si;
3265         
3266         QuatMul(quat, q1, q2);
3267 }
3268
3269 void MinMaxRGB(short c[])
3270 {
3271         if(c[0]>255) c[0]=255;
3272         else if(c[0]<0) c[0]=0;
3273         if(c[1]>255) c[1]=255;
3274         else if(c[1]<0) c[1]=0;
3275         if(c[2]>255) c[2]=255;
3276         else if(c[2]<0) c[2]=0;
3277 }
3278
3279 float Vec2Lenf(float *v1, float *v2)
3280 {
3281         float x, y;
3282
3283         x = v1[0]-v2[0];
3284         y = v1[1]-v2[1];
3285         return (float)sqrt(x*x+y*y);
3286 }
3287
3288 float Vec2Length(float *v)
3289 {
3290         return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
3291 }
3292
3293 void Vec2Mulf(float *v1, float f)
3294 {
3295         v1[0]*= f;
3296         v1[1]*= f;
3297 }
3298
3299 void Vec2Addf(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 Vec2Subf(float *v, float *v1, float *v2)
3306 {
3307         v[0]= v1[0]- v2[0];
3308         v[1]= v1[1]- v2[1];
3309 }
3310
3311 void Vec2Copyf(float *v1, float *v2)
3312 {
3313         v1[0]= v2[0];
3314         v1[1]= v2[1];
3315 }
3316
3317 float Inp2f(float *v1, float *v2)
3318 {
3319         return v1[0]*v2[0]+v1[1]*v2[1];
3320 }
3321
3322 float Normalize2(float *n)
3323 {
3324         float d;
3325         
3326         d= n[0]*n[0]+n[1]*n[1];
3327
3328         if(d>1.0e-35F) {
3329                 d= (float)sqrt(d);
3330
3331                 n[0]/=d; 
3332                 n[1]/=d; 
3333         } else {
3334                 n[0]=n[1]= 0.0;
3335                 d= 0.0;
3336         }
3337         return d;
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 */