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