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