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