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