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