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 sasqrt(float fac)
  86 {
  87         if(fac<=0.0) return 0.0;
  88         return (float)sqrt(fac);
  89 }
  90
  91 float Normalise(float *n)
  92 {
  93         float d;
  94
  95         d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
  96         /* FLT_EPSILON is too large! A larger value causes normalise errors in a scaled down utah teapot */
  97         if(d>0.0000000000001) {
  98                 d= (float)sqrt(d);
  99
 100                 n[0]/=d;
 101                 n[1]/=d;
 102                 n[2]/=d;
 103         } else {
 104                 n[0]=n[1]=n[2]= 0.0;
 105                 d= 0.0;
 106         }
 107         return d;
 108 }
 109
 110 void Crossf(float *c, float *a, float *b)
 111 {
 112         c[0] = a[1] * b[2] - a[2] * b[1];
 113         c[1] = a[2] * b[0] - a[0] * b[2];
 114         c[2] = a[0] * b[1] - a[1] * b[0];
 115 }
 116
 117 float Inpf( float *v1, float *v2)
 118 {
 119         return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
 120 }
 121
 122 /* Project v1 on v2 */
 123 void Projf(float *c, float *v1, float *v2)
 124 {
 125         float mul;
 126         mul = Inpf(v1, v2) / Inpf(v2, v2);
 127
 128         c[0] = mul * v2[0];
 129         c[1] = mul * v2[1];
 130         c[2] = mul * v2[2];
 131 }
 132
 133 void Mat3Transp(float mat[][3])
 134 {
 135         float t;
 136
 137         t = mat[0][1] ;
 138         mat[0][1] = mat[1][0] ;
 139         mat[1][0] = t;
 140         t = mat[0][2] ;
 141         mat[0][2] = mat[2][0] ;
 142         mat[2][0] = t;
 143         t = mat[1][2] ;
 144         mat[1][2] = mat[2][1] ;
 145         mat[2][1] = t;
 146 }
 147
 148 void Mat4Transp(float mat[][4])
 149 {
 150         float t;
 151
 152         t = mat[0][1] ;
 153         mat[0][1] = mat[1][0] ;
 154         mat[1][0] = t;
 155         t = mat[0][2] ;
 156         mat[0][2] = mat[2][0] ;
 157         mat[2][0] = t;
 158         t = mat[0][3] ;
 159         mat[0][3] = mat[3][0] ;
 160         mat[3][0] = t;
 161
 162         t = mat[1][2] ;
 163         mat[1][2] = mat[2][1] ;
 164         mat[2][1] = t;
 165         t = mat[1][3] ;
 166         mat[1][3] = mat[3][1] ;
 167         mat[3][1] = t;
 168
 169         t = mat[2][3] ;
 170         mat[2][3] = mat[3][2] ;
 171         mat[3][2] = t;
 172 }
 173
 174
 175 /*
 176  * invertmat -
 177  *              computes the inverse of mat and puts it in inverse.  Returns
 178  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
 179  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
 180  *
 181  *                                      Mark Segal - 1992
 182  */
 183
 184 int Mat4Invert(float inverse[][4], float mat[][4])
 185 {
 186         int i, j, k;
 187         double temp;
 188         float tempmat[4][4];
 189         float max;
 190         int maxj;
 191
 192         /* Set inverse to identity */
 193         for (i=0; i<4; i++)
 194                 for (j=0; j<4; j++)
 195                         inverse[i][j] = 0;
 196         for (i=0; i<4; i++)
 197                 inverse[i][i] = 1;
 198
 199         /* Copy original matrix so we don't mess it up */
 200         for(i = 0; i < 4; i++)
 201                 for(j = 0; j <4; j++)
 202                         tempmat[i][j] = mat[i][j];
 203
 204         for(i = 0; i < 4; i++) {
 205                 /* Look for row with max pivot */
 206                 max = ABS(tempmat[i][i]);
 207                 maxj = i;
 208                 for(j = i + 1; j < 4; j++) {
 209                         if(ABS(tempmat[j][i]) > max) {
 210                                 max = ABS(tempmat[j][i]);
 211                                 maxj = j;
 212                         }
 213                 }
 214                 /* Swap rows if necessary */
 215                 if (maxj != i) {
 216                         for( k = 0; k < 4; k++) {
 217                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
 218                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
 219                         }
 220                 }
 221
 222                 temp = tempmat[i][i];
 223                 if (temp == 0)
 224                         return 0;  /* No non-zero pivot */
 225                 for(k = 0; k < 4; k++) {
 226                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
 227                         inverse[i][k] = (float)(inverse[i][k]/temp);
 228                 }
 229                 for(j = 0; j < 4; j++) {
 230                         if(j != i) {
 231                                 temp = tempmat[j][i];
 232                                 for(k = 0; k < 4; k++) {
 233                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
 234                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
 235                                 }
 236                         }
 237                 }
 238         }
 239         return 1;
 240 }
 241 #ifdef TEST_ACTIVE
 242 void Mat4InvertSimp(float inverse[][4], float mat[][4])
 243 {
 244         /* only for Matrices that have a rotation */
 245         /* based at GG IV pag 205 */
 246         float scale;
 247
 248         scale= mat[0][0]*mat[0][0] + mat[1][0]*mat[1][0] + mat[2][0]*mat[2][0];
 249         if(scale==0.0) return;
 250
 251         scale= 1.0/scale;
 252
 253         /* transpose and scale */
 254         inverse[0][0]= scale*mat[0][0];
 255         inverse[1][0]= scale*mat[0][1];
 256         inverse[2][0]= scale*mat[0][2];
 257         inverse[0][1]= scale*mat[1][0];
 258         inverse[1][1]= scale*mat[1][1];
 259         inverse[2][1]= scale*mat[1][2];
 260         inverse[0][2]= scale*mat[2][0];
 261         inverse[1][2]= scale*mat[2][1];
 262         inverse[2][2]= scale*mat[2][2];
 263
 264         inverse[3][0]= -(inverse[0][0]*mat[3][0] + inverse[1][0]*mat[3][1] + inverse[2][0]*mat[3][2]);
 265         inverse[3][1]= -(inverse[0][1]*mat[3][0] + inverse[1][1]*mat[3][1] + inverse[2][1]*mat[3][2]);
 266         inverse[3][2]= -(inverse[0][2]*mat[3][0] + inverse[1][2]*mat[3][1] + inverse[2][2]*mat[3][2]);
 267
 268         inverse[0][3]= inverse[1][3]= inverse[2][3]= 0.0;
 269         inverse[3][3]= 1.0;
 270 }
 271 #endif
 272 /*  struct Matrix4; */
 273
 274 #ifdef TEST_ACTIVE
 275 /* this seems to be unused.. */
 276
 277 void Mat4Inv(float *m1, float *m2)
 278 {
 279
 280 /* This gets me into trouble:  */
 281         float mat1[3][3], mat2[3][3];
 282
 283 /*      void Mat3Inv(); */
 284 /*      void Mat3CpyMat4(); */
 285 /*      void Mat4CpyMat3(); */
 286
 287         Mat3CpyMat4((float*)mat2,m2);
 288         Mat3Inv((float*)mat1, (float*) mat2);
 289         Mat4CpyMat3(m1, mat1);
 290
 291 }
 292 #endif
 293
 294
 295 float Det2x2(float a,float b,float c,float d)
 296 {
 297
 298         return a*d - b*c;
 299 }
 300
 301
 302
 303 float Det3x3(float a1, float a2, float a3,
 304                          float b1, float b2, float b3,
 305                          float c1, float c2, float c3 )
 306 {
 307         float ans;
 308
 309         ans = a1 * Det2x2( b2, b3, c2, c3 )
 310             - b1 * Det2x2( a2, a3, c2, c3 )
 311             + c1 * Det2x2( a2, a3, b2, b3 );
 312
 313         return ans;
 314 }
 315
 316 float Det4x4(float m[][4])
 317 {
 318         float ans;
 319         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
 320
 321         a1= m[0][0];
 322         b1= m[0][1];
 323         c1= m[0][2];
 324         d1= m[0][3];
 325
 326         a2= m[1][0];
 327         b2= m[1][1];
 328         c2= m[1][2];
 329         d2= m[1][3];
 330
 331         a3= m[2][0];
 332         b3= m[2][1];
 333         c3= m[2][2];
 334         d3= m[2][3];
 335
 336         a4= m[3][0];
 337         b4= m[3][1];
 338         c4= m[3][2];
 339         d4= m[3][3];
 340
 341         ans = a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
 342             - b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
 343             + c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
 344             - d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
 345
 346         return ans;
 347 }
 348
 349
 350 void Mat4Adj(float out[][4], float in[][4])     /* out = ADJ(in) */
 351 {
 352         float a1, a2, a3, a4, b1, b2, b3, b4;
 353         float c1, c2, c3, c4, d1, d2, d3, d4;
 354
 355         a1= in[0][0];
 356         b1= in[0][1];
 357         c1= in[0][2];
 358         d1= in[0][3];
 359
 360         a2= in[1][0];
 361         b2= in[1][1];
 362         c2= in[1][2];
 363         d2= in[1][3];
 364
 365         a3= in[2][0];
 366         b3= in[2][1];
 367         c3= in[2][2];
 368         d3= in[2][3];
 369
 370         a4= in[3][0];
 371         b4= in[3][1];
 372         c4= in[3][2];
 373         d4= in[3][3];
 374
 375
 376         out[0][0]  =   Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
 377         out[1][0]  = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
 378         out[2][0]  =   Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
 379         out[3][0]  = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
 380
 381         out[0][1]  = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
 382         out[1][1]  =   Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
 383         out[2][1]  = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
 384         out[3][1]  =   Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
 385
 386         out[0][2]  =   Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
 387         out[1][2]  = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
 388         out[2][2]  =   Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
 389         out[3][2]  = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
 390
 391         out[0][3]  = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
 392         out[1][3]  =   Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
 393         out[2][3]  = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
 394         out[3][3]  =   Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
 395 }
 396
 397 void Mat4InvGG(float out[][4], float in[][4])   /* from Graphic Gems I, out= INV(in)  */
 398 {
 399         int i, j;
 400         float det;
 401
 402         /* calculate the adjoint matrix */
 403
 404         Mat4Adj(out,in);
 405
 406         det = Det4x4(out);
 407
 408         if ( fabs( det ) < SMALL_NUMBER) {
 409                 return;
 410         }
 411
 412         /* scale the adjoint matrix to get the inverse */
 413
 414         for (i=0; i<4; i++)
 415                 for(j=0; j<4; j++)
 416                         out[i][j] = out[i][j] / det;
 417
 418         /* the last factor is not always 1. For that reason an extra division should be implemented? */
 419 }
 420
 421
 422 void Mat3Inv(float m1[][3], float m2[][3])
 423 {
 424         short a,b;
 425         float det;
 426
 427         /* calc adjoint */
 428         Mat3Adj(m1,m2);
 429
 430         /* then determinant old matrix! */
 431         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
 432             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
 433             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
 434
 435         if(det==0) det=1;
 436         det= 1/det;
 437         for(a=0;a<3;a++) {
 438                 for(b=0;b<3;b++) {
 439                         m1[a][b]*=det;
 440                 }
 441         }
 442 }
 443
 444 void Mat3Adj(float m1[][3], float m[][3])
 445 {
 446         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
 447         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
 448         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
 449
 450         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
 451         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
 452         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
 453
 454         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
 455         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
 456         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
 457 }
 458
 459 void Mat4MulMat4(float m1[][4], float m2[][4], float m3[][4])
 460 {
 461   /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
 462
 463         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];
 464         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];
 465         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];
 466         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];
 467
 468         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];
 469         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];
 470         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];
 471         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];
 472
 473         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];
 474         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];
 475         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];
 476         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];
 477
 478         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];
 479         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];
 480         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];
 481         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];
 482
 483 }
 484 #ifdef TEST_ACTIVE
 485 void subMat4MulMat4(float *m1, float *m2, float *m3)
 486 {
 487
 488         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
 489         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
 490         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
 491         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
 492         m1+=4;
 493         m2+=4;
 494         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
 495         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
 496         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
 497         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
 498         m1+=4;
 499         m2+=4;
 500         m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8];
 501         m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9];
 502         m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10];
 503         m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] + m2[3];
 504 }
 505 #endif
 506
 507 #ifndef TEST_ACTIVE
 508 void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3])
 509 #else
 510 void Mat3MulMat3(float *m1, float *m3, float *m2)
 511 #endif
 512 {
 513    /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
 514 #ifndef TEST_ACTIVE
 515         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 516         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 517         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 518
 519         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 520         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 521         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 522
 523         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 524         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 525         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 526 #else
 527         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
 528         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
 529         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
 530         m1+=3;
 531         m2+=3;
 532         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
 533         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
 534         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
 535         m1+=3;
 536         m2+=3;
 537         m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6];
 538         m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7];
 539         m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8];
 540 #endif
 541 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
 542
 543 void Mat4MulMat43(float (*m1)[4], float (*m3)[4], float (*m2)[3])
 544 {
 545         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 546         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 547         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 548         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 549         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 550         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 551         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 552         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 553         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 554 }
 555 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
 556 void Mat3IsMat3MulMat4(float m1[][3], float m2[][3], float m3[][4])
 557 {
 558     /* m1[i][j] = m2[i][k] * m3[k][j] */
 559     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
 560     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
 561     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
 562
 563     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
 564     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
 565     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
 566
 567     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
 568     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
 569     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
 570 }
 571
 572
 573
 574 void Mat4MulMat34(float (*m1)[4], float (*m3)[3], float (*m2)[4])
 575 {
 576         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 577         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 578         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 579         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 580         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 581         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 582         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 583         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 584         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 585 }
 586
 587 void Mat4CpyMat4(float m1[][4], float m2[][4])
 588 {
 589         memcpy(m1, m2, 4*4*sizeof(float));
 590 }
 591
 592 void Mat4SwapMat4(float *m1, float *m2)
 593 {
 594         float t;
 595         int i;
 596
 597         for(i=0;i<16;i++) {
 598                 t= *m1;
 599                 *m1= *m2;
 600                 *m2= t;
 601                 m1++;
 602                 m2++;
 603         }
 604 }
 605
 606 typedef float Mat3Row[3];
 607 typedef float Mat4Row[4];
 608
 609 #ifdef TEST_ACTIVE
 610 void Mat3CpyMat4(float *m1p, float *m2p)
 611 #else
 612 void Mat3CpyMat4(float m1[][3], float m2[][4])
 613 #endif
 614 {
 615 #ifdef TEST_ACTIVE
 616         int i, j;
 617         Mat3Row *m1= (Mat3Row *)m1p;
 618         Mat4Row *m2= (Mat4Row *)m2p;
 619         for ( i = 0; i++; i < 3) {
 620                 for (j = 0; j++; j < 3) {
 621                         m1p[3*i + j] = m2p[4*i + j];
 622                 }
 623         }
 624 #endif
 625         m1[0][0]= m2[0][0];
 626         m1[0][1]= m2[0][1];
 627         m1[0][2]= m2[0][2];
 628
 629         m1[1][0]= m2[1][0];
 630         m1[1][1]= m2[1][1];
 631         m1[1][2]= m2[1][2];
 632
 633         m1[2][0]= m2[2][0];
 634         m1[2][1]= m2[2][1];
 635         m1[2][2]= m2[2][2];
 636 }
 637
 638 /* Butched. See .h for comment */
 639 /*  void Mat4CpyMat3(float m1[][4], float m2[][3]) */
 640 #ifdef TEST_ACTIVE
 641 void Mat4CpyMat3(float* m1, float *m2)
 642 {
 643         int i;
 644         for (i = 0; i < 3; i++) {
 645                 m1[(4*i)]    = m2[(3*i)];
 646                 m1[(4*i) + 1]= m2[(3*i) + 1];
 647                 m1[(4*i) + 2]= m2[(3*i) + 2];
 648                 m1[(4*i) + 3]= 0.0;
 649                 i++;
 650         }
 651
 652         m1[12]=m1[13]= m1[14]= 0.0;
 653         m1[15]= 1.0;
 654 }
 655 #else
 656
 657 void Mat4CpyMat3(float m1[][4], float m2[][3])  /* no clear */
 658 {
 659         m1[0][0]= m2[0][0];
 660         m1[0][1]= m2[0][1];
 661         m1[0][2]= m2[0][2];
 662
 663         m1[1][0]= m2[1][0];
 664         m1[1][1]= m2[1][1];
 665         m1[1][2]= m2[1][2];
 666
 667         m1[2][0]= m2[2][0];
 668         m1[2][1]= m2[2][1];
 669         m1[2][2]= m2[2][2];
 670
 671         /*      Reevan's Bugfix */
 672         m1[0][3]=0.0F;
 673         m1[1][3]=0.0F;
 674         m1[2][3]=0.0F;
 675
 676         m1[3][0]=0.0F;
 677         m1[3][1]=0.0F;
 678         m1[3][2]=0.0F;
 679         m1[3][3]=1.0F;
 680
 681
 682 }
 683 #endif
 684
 685 void Mat3CpyMat3(float m1[][3], float m2[][3])
 686 {
 687         /* destination comes first: */
 688         memcpy(&m1[0], &m2[0], 9*sizeof(float));
 689 }
 690
 691 void Mat3MulSerie(float answ[][3],
 692                                    float m1[][3], float m2[][3], float m3[][3],
 693                                    float m4[][3], float m5[][3], float m6[][3],
 694                                    float m7[][3], float m8[][3])
 695 {
 696         float temp[3][3];
 697
 698         if(m1==0 || m2==0) return;
 699
 700
 701         Mat3MulMat3(answ, m2, m1);
 702         if(m3) {
 703                 Mat3MulMat3(temp, m3, answ);
 704                 if(m4) {
 705                         Mat3MulMat3(answ, m4, temp);
 706                         if(m5) {
 707                                 Mat3MulMat3(temp, m5, answ);
 708                                 if(m6) {
 709                                         Mat3MulMat3(answ, m6, temp);
 710                                         if(m7) {
 711                                                 Mat3MulMat3(temp, m7, answ);
 712                                                 if(m8) {
 713                                                         Mat3MulMat3(answ, m8, temp);
 714                                                 }
 715                                                 else Mat3CpyMat3(answ, temp);
 716                                         }
 717                                 }
 718                                 else Mat3CpyMat3(answ, temp);
 719                         }
 720                 }
 721                 else Mat3CpyMat3(answ, temp);
 722         }
 723 }
 724
 725 void Mat4MulSerie(float answ[][4], float m1[][4],
 726                                 float m2[][4], float m3[][4], float m4[][4],
 727                                 float m5[][4], float m6[][4], float m7[][4],
 728                                 float m8[][4])
 729 {
 730         float temp[4][4];
 731
 732         if(m1==0 || m2==0) return;
 733
 734         Mat4MulMat4(answ, m2, m1);
 735         if(m3) {
 736                 Mat4MulMat4(temp, m3, answ);
 737                 if(m4) {
 738                         Mat4MulMat4(answ, m4, temp);
 739                         if(m5) {
 740                                 Mat4MulMat4(temp, m5, answ);
 741                                 if(m6) {
 742                                         Mat4MulMat4(answ, m6, temp);
 743                                         if(m7) {
 744                                                 Mat4MulMat4(temp, m7, answ);
 745                                                 if(m8) {
 746                                                         Mat4MulMat4(answ, m8, temp);
 747                                                 }
 748                                                 else Mat4CpyMat4(answ, temp);
 749                                         }
 750                                 }
 751                                 else Mat4CpyMat4(answ, temp);
 752                         }
 753                 }
 754                 else Mat4CpyMat4(answ, temp);
 755         }
 756 }
 757
 758
 759
 760 void Mat4Clr(float *m)
 761 {
 762         memset(m, 0, 4*4*sizeof(float));
 763 }
 764
 765 void Mat3Clr(float *m)
 766 {
 767         memset(m, 0, 3*3*sizeof(float));
 768 }
 769
 770 void Mat4One(float m[][4])
 771 {
 772
 773         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
 774         m[0][1]= m[0][2]= m[0][3]= 0.0;
 775         m[1][0]= m[1][2]= m[1][3]= 0.0;
 776         m[2][0]= m[2][1]= m[2][3]= 0.0;
 777         m[3][0]= m[3][1]= m[3][2]= 0.0;
 778 }
 779
 780 void Mat3One(float m[][3])
 781 {
 782
 783         m[0][0]= m[1][1]= m[2][2]= 1.0;
 784         m[0][1]= m[0][2]= 0.0;
 785         m[1][0]= m[1][2]= 0.0;
 786         m[2][0]= m[2][1]= 0.0;
 787 }
 788
 789 void Mat4MulVec( float mat[][4], int *vec)
 790 {
 791         int x,y;
 792
 793         x=vec[0];
 794         y=vec[1];
 795         vec[0]=(int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]);
 796         vec[1]=(int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]);
 797         vec[2]=(int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]);
 798 }
 799
 800 void Mat4MulVecfl( float mat[][4], float *vec)
 801 {
 802         float x,y;
 803
 804         x=vec[0];
 805         y=vec[1];
 806         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
 807         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
 808         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
 809 }
 810
 811 void VecMat4MulVecfl(float *in, float mat[][4], float *vec)
 812 {
 813         float x,y;
 814
 815         x=vec[0];
 816         y=vec[1];
 817         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
 818         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
 819         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
 820 }
 821
 822 void Mat4Mul3Vecfl( float mat[][4], float *vec)
 823 {
 824         float x,y;
 825
 826         x= vec[0];
 827         y= vec[1];
 828         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 829         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 830         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 831 }
 832
 833 void Mat4MulVec4fl( float mat[][4], float *vec)
 834 {
 835         float x,y,z;
 836
 837         x=vec[0];
 838         y=vec[1];
 839         z= vec[2];
 840         vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
 841         vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
 842         vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
 843         vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
 844 }
 845
 846 void Mat3MulVec( float mat[][3], int *vec)
 847 {
 848         int x,y;
 849
 850         x=vec[0];
 851         y=vec[1];
 852         vec[0]= (int)(x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]);
 853         vec[1]= (int)(x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]);
 854         vec[2]= (int)(x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]);
 855 }
 856
 857 void Mat3MulVecfl( float mat[][3], float *vec)
 858 {
 859         float x,y;
 860
 861         x=vec[0];
 862         y=vec[1];
 863         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 864         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 865         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 866 }
 867
 868 void Mat3MulVecd( float mat[][3], double *vec)
 869 {
 870         double x,y;
 871
 872         x=vec[0];
 873         y=vec[1];
 874         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 875         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 876         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 877 }
 878
 879 void Mat3TransMulVecfl( float mat[][3], float *vec)
 880 {
 881         float x,y;
 882
 883         x=vec[0];
 884         y=vec[1];
 885         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
 886         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
 887         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
 888 }
 889
 890 void Mat3MulFloat(float *m, float f)
 891 {
 892         int i;
 893
 894         for(i=0;i<9;i++) m[i]*=f;
 895 }
 896
 897 void Mat4MulFloat(float *m, float f)
 898 {
 899         int i;
 900
 901         for(i=0;i<12;i++) m[i]*=f;      /* count to 12: without vector component */
 902 }
 903
 904
 905 void Mat4MulFloat3(float *m, float f)           /* only scale component */
 906 {
 907         int i,j;
 908
 909         for(i=0; i<3; i++) {
 910                 for(j=0; j<3; j++) {
 911
 912                         m[4*i+j] *= f;
 913                 }
 914         }
 915 }
 916
 917 void VecStar(float mat[][3], float *vec)
 918 {
 919
 920         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
 921         mat[0][1]= -vec[2];
 922         mat[0][2]= vec[1];
 923         mat[1][0]= vec[2];
 924         mat[1][2]= -vec[0];
 925         mat[2][0]= -vec[1];
 926         mat[2][1]= vec[0];
 927
 928 }
 929 #ifdef TEST_ACTIVE
 930 short EenheidsMat(float mat[][3])
 931 {
 932
 933         if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0)
 934                 if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0)
 935                         if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0)
 936                                 return 1;
 937         return 0;
 938 }
 939 #endif
 940
 941 int FloatCompare( float *v1,  float *v2, float limit)
 942 {
 943
 944         if( fabs(v1[0]-v2[0])<limit ) {
 945                 if( fabs(v1[1]-v2[1])<limit ) {
 946                         if( fabs(v1[2]-v2[2])<limit ) return 1;
 947                 }
 948         }
 949         return 0;
 950 }
 951
 952 void printmatrix4( char *str,  float m[][4])
 953 {
 954         printf("%s\n", str);
 955         printf("%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3]);
 956         printf("%f %f %f %f\n",m[1][0],m[1][1],m[1][2],m[1][3]);
 957         printf("%f %f %f %f\n",m[2][0],m[2][1],m[2][2],m[2][3]);
 958         printf("%f %f %f %f\n",m[3][0],m[3][1],m[3][2],m[3][3]);
 959         printf("\n");
 960
 961 }
 962
 963 void printmatrix3( char *str,  float m[][3])
 964 {
 965         printf("%s\n", str);
 966         printf("%f %f %f\n",m[0][0],m[0][1],m[0][2]);
 967         printf("%f %f %f\n",m[1][0],m[1][1],m[1][2]);
 968         printf("%f %f %f\n",m[2][0],m[2][1],m[2][2]);
 969         printf("\n");
 970
 971 }
 972
 973 /* **************** QUATERNIONS ********** */
 974
 975
 976 void QuatMul(float *q, float *q1, float *q2)
 977 {
 978         float t0,t1,t2;
 979
 980         t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
 981         t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
 982         t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
 983         q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
 984         q[0]=t0;
 985         q[1]=t1;
 986         q[2]=t2;
 987 }
 988
 989 void QuatSub(float *q, float *q1, float *q2)
 990 {
 991         q2[0]= -q2[0];
 992         QuatMul(q, q1, q2);
 993         q2[0]= -q2[0];
 994 }
 995
 996
 997 void QuatToMat3( float *q, float m[][3])
 998 {
 999         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1000
1001         q0= M_SQRT2 * q[0];
1002         q1= M_SQRT2 * q[1];
1003         q2= M_SQRT2 * q[2];
1004         q3= M_SQRT2 * q[3];
1005
1006         qda= q0*q1;
1007         qdb= q0*q2;
1008         qdc= q0*q3;
1009         qaa= q1*q1;
1010         qab= q1*q2;
1011         qac= q1*q3;
1012         qbb= q2*q2;
1013         qbc= q2*q3;
1014         qcc= q3*q3;
1015
1016         m[0][0]= (float)(1.0-qbb-qcc);
1017         m[0][1]= (float)(qdc+qab);
1018         m[0][2]= (float)(-qdb+qac);
1019
1020         m[1][0]= (float)(-qdc+qab);
1021         m[1][1]= (float)(1.0-qaa-qcc);
1022         m[1][2]= (float)(qda+qbc);
1023
1024         m[2][0]= (float)(qdb+qac);
1025         m[2][1]= (float)(-qda+qbc);
1026         m[2][2]= (float)(1.0-qaa-qbb);
1027 }
1028
1029
1030 void QuatToMat4( float *q, float m[][4])
1031 {
1032         double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
1033
1034         q0= M_SQRT2 * q[0];
1035         q1= M_SQRT2 * q[1];
1036         q2= M_SQRT2 * q[2];
1037         q3= M_SQRT2 * q[3];
1038
1039         qda= q0*q1;
1040         qdb= q0*q2;
1041         qdc= q0*q3;
1042         qaa= q1*q1;
1043         qab= q1*q2;
1044         qac= q1*q3;
1045         qbb= q2*q2;
1046         qbc= q2*q3;
1047         qcc= q3*q3;
1048
1049         m[0][0]= (float)(1.0-qbb-qcc);
1050         m[0][1]= (float)(qdc+qab);
1051         m[0][2]= (float)(-qdb+qac);
1052         m[0][3]= 0.0f;
1053
1054         m[1][0]= (float)(-qdc+qab);
1055         m[1][1]= (float)(1.0-qaa-qcc);
1056         m[1][2]= (float)(qda+qbc);
1057         m[1][3]= 0.0f;
1058
1059         m[2][0]= (float)(qdb+qac);
1060         m[2][1]= (float)(-qda+qbc);
1061         m[2][2]= (float)(1.0-qaa-qbb);
1062         m[2][3]= 0.0f;
1063
1064         m[3][0]= m[3][1]= m[3][2]= 0.0f;
1065         m[3][3]= 1.0f;
1066 }
1067
1068 void Mat3ToQuat( float wmat[][3], float *q)             /* from Sig.Proc.85 pag 253 */
1069 {
1070         double tr, s;
1071         float mat[3][3];
1072
1073         /* work on a copy */
1074         Mat3CpyMat3(mat, wmat);
1075         Mat3Ortho(mat);                 /* this is needed AND a NormalQuat in the end */
1076
1077         tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
1078
1079         if(tr>FLT_EPSILON) {
1080                 s= sqrt( tr);
1081                 q[0]= (float)s;
1082                 s*= 4.0;
1083                 q[1]= (float)((mat[1][2]-mat[2][1])/s);
1084                 q[2]= (float)((mat[2][0]-mat[0][2])/s);
1085                 q[3]= (float)((mat[0][1]-mat[1][0])/s);
1086         }
1087         else {
1088                 q[0]= 0.0f;
1089                 s= -0.5*(mat[1][1]+mat[2][2]);
1090
1091                 if(s>FLT_EPSILON) {
1092                         s= sqrt(s);
1093                         q[1]= (float)s;
1094                         q[2]= (float)(mat[0][1]/(2*s));
1095                         q[3]= (float)(mat[0][2]/(2*s));
1096                 }
1097                 else {
1098                         q[1]= 0.0f;
1099                         s= 0.5*(1.0-mat[2][2]);
1100
1101                         if(s>FLT_EPSILON) {
1102                                 s= sqrt(s);
1103                                 q[2]= (float)s;
1104                                 q[3]= (float)(mat[1][2]/(2*s));
1105                         }
1106                         else {
1107                                 q[2]= 0.0f;
1108                                 q[3]= 1.0f;
1109                         }
1110                 }
1111         }
1112         NormalQuat(q);
1113 }
1114
1115 void Mat3ToQuat_is_ok( float wmat[][3], float *q)
1116 {
1117         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], hoek, si, co, nor[3];
1118
1119         /* work on a copy */
1120         Mat3CpyMat3(mat, wmat);
1121         Mat3Ortho(mat);
1122
1123         /* rotate z-axis of matrix to z-axis */
1124
1125         nor[0] = mat[2][1];             /* cross product with (0,0,1) */
1126         nor[1] =  -mat[2][0];
1127         nor[2] = 0.0;
1128         Normalise(nor);
1129
1130         co= mat[2][2];
1131         hoek= 0.5f*saacos(co);
1132
1133         co= (float)cos(hoek);
1134         si= (float)sin(hoek);
1135         q1[0]= co;
1136         q1[1]= -nor[0]*si;              /* negative here, but why? */
1137         q1[2]= -nor[1]*si;
1138         q1[3]= -nor[2]*si;
1139
1140         /* rotate back x-axis from mat, using inverse q1 */
1141         QuatToMat3(q1, matr);
1142         Mat3Inv(matn, matr);
1143         Mat3MulVecfl(matn, mat[0]);
1144
1145         /* and align x-axes */
1146         hoek= (float)(0.5*atan2(mat[0][1], mat[0][0]));
1147
1148         co= (float)cos(hoek);
1149         si= (float)sin(hoek);
1150         q2[0]= co;
1151         q2[1]= 0.0f;
1152         q2[2]= 0.0f;
1153         q2[3]= si;
1154
1155         QuatMul(q, q1, q2);
1156 }
1157
1158
1159 void Mat4ToQuat( float m[][4], float *q)
1160 {
1161         float mat[3][3];
1162
1163         Mat3CpyMat4(mat, m);
1164         Mat3ToQuat(mat, q);
1165
1166 }
1167
1168 void QuatOne(float *q)
1169 {
1170         q[0]= q[2]= q[3]= 0.0;
1171         q[1]= 1.0;
1172 }
1173
1174 void NormalQuat(float *q)
1175 {
1176         float len;
1177
1178         len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
1179         if(len!=0.0) {
1180                 q[0]/= len;
1181                 q[1]/= len;
1182                 q[2]/= len;
1183                 q[3]/= len;
1184         } else {
1185                 q[1]= 1.0f;
1186                 q[0]= q[2]= q[3]= 0.0f;
1187         }
1188 }
1189
1190 float *vectoquat( float *vec, short axis, short upflag)
1191 {
1192         static float q1[4];
1193         float q2[4], nor[3], *fp, mat[3][3], hoek, si, co, x2, y2, z2, len1;
1194
1195         /* first rotate to axis */
1196         if(axis>2) {
1197                 x2= vec[0] ; y2= vec[1] ; z2= vec[2];
1198                 axis-= 3;
1199         }
1200         else {
1201                 x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
1202         }
1203
1204         q1[0]=1.0;
1205         q1[1]=q1[2]=q1[3]= 0.0;
1206
1207         len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
1208         if(len1 == 0.0) return(q1);
1209
1210         /* nasty! I need a good routine for this...
1211          * problem is a rotation of an Y axis to the negative Y-axis for example.
1212          */
1213
1214         if(axis==0) {   /* x-axis */
1215                 nor[0]= 0.0;
1216                 nor[1]= -z2;
1217                 nor[2]= y2;
1218
1219                 if( fabs(y2)+fabs(z2)<0.0001 ) {
1220                         nor[1]= 1.0;
1221                 }
1222
1223                 co= x2;
1224         }
1225         else if(axis==1) {      /* y-axis */
1226                 nor[0]= z2;
1227                 nor[1]= 0.0;
1228                 nor[2]= -x2;
1229
1230                 if( fabs(x2)+fabs(z2)<0.0001 ) {
1231                         nor[2]= 1.0;
1232                 }
1233
1234                 co= y2;
1235         }
1236         else {                  /* z-axis */
1237                 nor[0]= -y2;
1238                 nor[1]= x2;
1239                 nor[2]= 0.0;
1240
1241                 if( fabs(x2)+fabs(y2)<0.0001 ) {
1242                         nor[0]= 1.0;
1243                 }
1244
1245                 co= z2;
1246         }
1247         co/= len1;
1248
1249         Normalise(nor);
1250
1251         hoek= 0.5f*saacos(co);
1252         si= (float)sin(hoek);
1253         q1[0]= (float)cos(hoek);
1254         q1[1]= nor[0]*si;
1255         q1[2]= nor[1]*si;
1256         q1[3]= nor[2]*si;
1257
1258         if(axis!=upflag) {
1259                 QuatToMat3(q1, mat);
1260
1261                 fp= mat[2];
1262                 if(axis==0) {
1263                         if(upflag==1) hoek= (float)(0.5*atan2(fp[2], fp[1]));
1264                         else hoek= (float)(-0.5*atan2(fp[1], fp[2]));
1265                 }
1266                 else if(axis==1) {
1267                         if(upflag==0) hoek= (float)(-0.5*atan2(fp[2], fp[0]));
1268                         else hoek= (float)(0.5*atan2(fp[0], fp[2]));
1269                 }
1270                 else {
1271                         if(upflag==0) hoek= (float)(0.5*atan2(-fp[1], -fp[0]));
1272                         else hoek= (float)(-0.5*atan2(-fp[0], -fp[1]));
1273                 }
1274
1275                 co= (float)cos(hoek);
1276                 si= (float)(sin(hoek)/len1);
1277                 q2[0]= co;
1278                 q2[1]= x2*si;
1279                 q2[2]= y2*si;
1280                 q2[3]= z2*si;
1281
1282                 QuatMul(q1,q2,q1);
1283         }
1284
1285         return(q1);
1286 }
1287
1288 void VecUpMat3old( float *vec, float mat[][3], short axis)
1289 {
1290         float inp, up[3];
1291         short cox = 0, coy = 0, coz = 0;
1292
1293         /* using different up's is not useful, infact there is no real 'up'!
1294          */
1295
1296         up[0]= 0.0;
1297         up[1]= 0.0;
1298         up[2]= 1.0;
1299
1300         if(axis==0) {
1301                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1302         }
1303         if(axis==1) {
1304                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1305         }
1306         if(axis==2) {
1307                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1308         }
1309         if(axis==3) {
1310                 cox= 0; coy= 2; coz= 1;         /*  */
1311         }
1312         if(axis==4) {
1313                 cox= 1; coy= 0; coz= 2;         /*  */
1314         }
1315         if(axis==5) {
1316                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1317         }
1318
1319         mat[coz][0]= vec[0];
1320         mat[coz][1]= vec[1];
1321         mat[coz][2]= vec[2];
1322         Normalise((float *)mat[coz]);
1323
1324         inp= mat[coz][0]*up[0] + mat[coz][1]*up[1] + mat[coz][2]*up[2];
1325         mat[coy][0]= up[0] - inp*mat[coz][0];
1326         mat[coy][1]= up[1] - inp*mat[coz][1];
1327         mat[coy][2]= up[2] - inp*mat[coz][2];
1328
1329         Normalise((float *)mat[coy]);
1330
1331         Crossf(mat[cox], mat[coy], mat[coz]);
1332
1333 }
1334
1335 void VecUpMat3(float *vec, float mat[][3], short axis)
1336 {
1337         float inp;
1338         short cox = 0, coy = 0, coz = 0;
1339
1340         /* using different up's is not useful, infact there is no real 'up'!
1341         */
1342
1343         if(axis==0) {
1344                 cox= 0; coy= 1; coz= 2;         /* Y up Z tr */
1345         }
1346         if(axis==1) {
1347                 cox= 1; coy= 2; coz= 0;         /* Z up X tr */
1348         }
1349         if(axis==2) {
1350                 cox= 2; coy= 0; coz= 1;         /* X up Y tr */
1351         }
1352         if(axis==3) {
1353                 cox= 0; coy= 1; coz= 2;         /* Y op -Z tr */
1354                 vec[0]= -vec[0];
1355                 vec[1]= -vec[1];
1356                 vec[2]= -vec[2];
1357         }
1358         if(axis==4) {
1359                 cox= 1; coy= 0; coz= 2;         /*  */
1360         }
1361         if(axis==5) {
1362                 cox= 2; coy= 1; coz= 0;         /* Y up X tr */
1363         }
1364
1365         mat[coz][0]= vec[0];
1366         mat[coz][1]= vec[1];
1367         mat[coz][2]= vec[2];
1368         Normalise((float *)mat[coz]);
1369
1370         inp= mat[coz][2];
1371         mat[coy][0]= - inp*mat[coz][0];
1372         mat[coy][1]= - inp*mat[coz][1];
1373         mat[coy][2]= 1.0f - inp*mat[coz][2];
1374
1375         Normalise((float *)mat[coy]);
1376
1377         Crossf(mat[cox], mat[coy], mat[coz]);
1378
1379 }
1380
1381
1382 /* **************** VIEW / PROJECTION ********************************  */
1383
1384
1385 void i_ortho(
1386         float left, float right,
1387         float bottom, float top,
1388         float nearClip, float farClip,
1389         float matrix[][4]
1390 ){
1391     float Xdelta, Ydelta, Zdelta;
1392
1393     Xdelta = right - left;
1394     Ydelta = top - bottom;
1395     Zdelta = farClip - nearClip;
1396     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1397                 return;
1398     }
1399     Mat4One(matrix);
1400     matrix[0][0] = 2.0f/Xdelta;
1401     matrix[3][0] = -(right + left)/Xdelta;
1402     matrix[1][1] = 2.0f/Ydelta;
1403     matrix[3][1] = -(top + bottom)/Ydelta;
1404     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1405     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1406 }
1407
1408 void i_window(
1409         float left, float right,
1410         float bottom, float top,
1411         float nearClip, float farClip,
1412         float mat[][4]
1413 ){
1414         float Xdelta, Ydelta, Zdelta;
1415
1416         Xdelta = right - left;
1417         Ydelta = top - bottom;
1418         Zdelta = farClip - nearClip;
1419
1420         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1421                 return;
1422         }
1423         mat[0][0] = nearClip * 2.0f/Xdelta;
1424         mat[1][1] = nearClip * 2.0f/Ydelta;
1425         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1426         mat[2][1] = (top + bottom)/Ydelta;
1427         mat[2][2] = -(farClip + nearClip)/Zdelta;
1428         mat[2][3] = -1.0f;
1429         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1430         mat[0][1] = mat[0][2] = mat[0][3] =
1431             mat[1][0] = mat[1][2] = mat[1][3] =
1432             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1433
1434 }
1435
1436 void i_translate(float Tx, float Ty, float Tz, float mat[][4])
1437 {
1438     mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1439     mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1440     mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1441 }
1442
1443 void i_multmatrix( float icand[][4], float Vm[][4])
1444 {
1445     int row, col;
1446     float temp[4][4];
1447
1448     for(row=0 ; row<4 ; row++)
1449         for(col=0 ; col<4 ; col++)
1450             temp[row][col] = icand[row][0] * Vm[0][col]
1451                            + icand[row][1] * Vm[1][col]
1452                            + icand[row][2] * Vm[2][col]
1453                            + icand[row][3] * Vm[3][col];
1454         Mat4CpyMat4(Vm, temp);
1455 }
1456
1457 void i_rotate(float angle, char axis, float mat[][4])
1458 {
1459         int col;
1460     float temp[4];
1461     float cosine, sine;
1462
1463     for(col=0; col<4 ; col++)   /* init temp to zero matrix */
1464         temp[col] = 0;
1465
1466     angle = (float)(angle*(3.1415926535/180.0));
1467     cosine = (float)cos(angle);
1468     sine = (float)sin(angle);
1469     switch(axis){
1470     case 'x':
1471     case 'X':
1472         for(col=0 ; col<4 ; col++)
1473             temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1474         for(col=0 ; col<4 ; col++) {
1475             mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1476             mat[1][col] = temp[col];
1477         }
1478         break;
1479
1480     case 'y':
1481     case 'Y':
1482         for(col=0 ; col<4 ; col++)
1483             temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1484         for(col=0 ; col<4 ; col++) {
1485             mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1486             mat[0][col] = temp[col];
1487         }
1488         break;
1489
1490     case 'z':
1491     case 'Z':
1492         for(col=0 ; col<4 ; col++)
1493             temp[col] = cosine*mat[0][col] + sine*mat[1][col];
1494         for(col=0 ; col<4 ; col++) {
1495             mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
1496             mat[0][col] = temp[col];
1497         }
1498         break;
1499     }
1500 }
1501
1502 void i_polarview(float dist, float azimuth, float incidence, float twist, float Vm[][4])
1503 {
1504
1505         Mat4One(Vm);
1506
1507     i_translate(0.0, 0.0, -dist, Vm);
1508     i_rotate(-twist,'z', Vm);
1509     i_rotate(-incidence,'x', Vm);
1510     i_rotate(-azimuth,'z', Vm);
1511 }
1512
1513 void i_lookat(float vx, float vy, float vz, float px, float py, float pz, float twist, float mat[][4])
1514 {
1515         float sine, cosine, hyp, hyp1, dx, dy, dz;
1516         float mat1[4][4];
1517
1518         Mat4One(mat);
1519         Mat4One(mat1);
1520
1521         i_rotate(-twist,'z', mat);
1522
1523         dx = px - vx;
1524         dy = py - vy;
1525         dz = pz - vz;
1526         hyp = dx * dx + dz * dz;        /* hyp squared  */
1527         hyp1 = (float)sqrt(dy*dy + hyp);
1528         hyp = (float)sqrt(hyp);         /* the real hyp */
1529
1530         if (hyp1 != 0.0) {              /* rotate X     */
1531                 sine = -dy / hyp1;
1532                 cosine = hyp /hyp1;
1533         } else {
1534                 sine = 0;
1535                 cosine = 1.0f;
1536         }
1537         mat1[1][1] = cosine;
1538         mat1[1][2] = sine;
1539         mat1[2][1] = -sine;
1540         mat1[2][2] = cosine;
1541
1542         i_multmatrix(mat1, mat);
1543
1544     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
1545     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
1546
1547         /* paragraph    */
1548         if (hyp != 0.0f) {                      /* rotate Y     */
1549                 sine = dx / hyp;
1550                 cosine = -dz / hyp;
1551         } else {
1552                 sine = 0;
1553                 cosine = 1.0f;
1554         }
1555         mat1[0][0] = cosine;
1556         mat1[0][2] = -sine;
1557         mat1[2][0] = sine;
1558         mat1[2][2] = cosine;
1559
1560         i_multmatrix(mat1, mat);
1561         i_translate(-vx,-vy,-vz, mat);  /* translate viewpoint to origin */
1562 }
1563
1564
1565
1566
1567
1568 /* ************************************************  */
1569
1570 void Mat3Ortho(float mat[][3])
1571 {
1572         Normalise(mat[0]);
1573         Normalise(mat[1]);
1574         Normalise(mat[2]);
1575 }
1576
1577 void Mat4Ortho(float mat[][4])
1578 {
1579         float len;
1580
1581         len= Normalise(mat[0]);
1582         if(len!=0.0) mat[0][3]/= len;
1583         len= Normalise(mat[1]);
1584         if(len!=0.0) mat[1][3]/= len;
1585         len= Normalise(mat[2]);
1586         if(len!=0.0) mat[2][3]/= len;
1587 }
1588
1589 void VecCopyf(float *v1, float *v2)
1590 {
1591
1592         v1[0]= v2[0];
1593         v1[1]= v2[1];
1594         v1[2]= v2[2];
1595 }
1596
1597 int VecLen( int *v1, int *v2)
1598 {
1599         float x,y,z;
1600
1601         x=(float)(v1[0]-v2[0]);
1602         y=(float)(v1[1]-v2[1]);
1603         z=(float)(v1[2]-v2[2]);
1604         return (int)floor(sqrt(x*x+y*y+z*z));
1605 }
1606
1607 float VecLenf( float *v1, float *v2)
1608 {
1609         float x,y,z;
1610
1611         x=v1[0]-v2[0];
1612         y=v1[1]-v2[1];
1613         z=v1[2]-v2[2];
1614         return (float)sqrt(x*x+y*y+z*z);
1615 }
1616
1617 void VecAddf(float *v, float *v1, float *v2)
1618 {
1619         v[0]= v1[0]+ v2[0];
1620         v[1]= v1[1]+ v2[1];
1621         v[2]= v1[2]+ v2[2];
1622 }
1623
1624 void VecSubf(float *v, float *v1, float *v2)
1625 {
1626         v[0]= v1[0]- v2[0];
1627         v[1]= v1[1]- v2[1];
1628         v[2]= v1[2]- v2[2];
1629 }
1630
1631 void VecMidf(float *v, float *v1, float *v2)
1632 {
1633         v[0]= 0.5f*(v1[0]+ v2[0]);
1634         v[1]= 0.5f*(v1[1]+ v2[1]);
1635         v[2]= 0.5f*(v1[2]+ v2[2]);
1636 }
1637
1638 void VecMulf(float *v1, float f)
1639 {
1640
1641         v1[0]*= f;
1642         v1[1]*= f;
1643         v1[2]*= f;
1644 }
1645
1646 int VecCompare( float *v1, float *v2, float limit)
1647 {
1648         if( fabs(v1[0]-v2[0])<limit )
1649                 if( fabs(v1[1]-v2[1])<limit )
1650                         if( fabs(v1[2]-v2[2])<limit ) return 1;
1651         return 0;
1652 }
1653
1654 void CalcNormShort( short *v1, short *v2, short *v3, float *n) /* is also cross product */
1655 {
1656         float n1[3],n2[3];
1657
1658         n1[0]= (float)(v1[0]-v2[0]);
1659         n2[0]= (float)(v2[0]-v3[0]);
1660         n1[1]= (float)(v1[1]-v2[1]);
1661         n2[1]= (float)(v2[1]-v3[1]);
1662         n1[2]= (float)(v1[2]-v2[2]);
1663         n2[2]= (float)(v2[2]-v3[2]);
1664         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1665         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1666         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1667         Normalise(n);
1668 }
1669
1670 void CalcNormLong( int* v1, int*v2, int*v3, float *n)
1671 {
1672         float n1[3],n2[3];
1673
1674         n1[0]= (float)(v1[0]-v2[0]);
1675         n2[0]= (float)(v2[0]-v3[0]);
1676         n1[1]= (float)(v1[1]-v2[1]);
1677         n2[1]= (float)(v2[1]-v3[1]);
1678         n1[2]= (float)(v1[2]-v2[2]);
1679         n2[2]= (float)(v2[2]-v3[2]);
1680         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1681         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1682         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1683         Normalise(n);
1684 }
1685
1686 float CalcNormFloat( float *v1, float *v2, float *v3, float *n)
1687 {
1688         float n1[3],n2[3];
1689
1690         n1[0]= v1[0]-v2[0];
1691         n2[0]= v2[0]-v3[0];
1692         n1[1]= v1[1]-v2[1];
1693         n2[1]= v2[1]-v3[1];
1694         n1[2]= v1[2]-v2[2];
1695         n2[2]= v2[2]-v3[2];
1696         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1697         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1698         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1699         return Normalise(n);
1700 }
1701
1702 float CalcNormFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
1703 {
1704         /* real cross! */
1705         float n1[3],n2[3];
1706
1707         n1[0]= v1[0]-v3[0];
1708         n1[1]= v1[1]-v3[1];
1709         n1[2]= v1[2]-v3[2];
1710
1711         n2[0]= v2[0]-v4[0];
1712         n2[1]= v2[1]-v4[1];
1713         n2[2]= v2[2]-v4[2];
1714
1715         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1716         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1717         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1718
1719         return Normalise(n);
1720 }
1721
1722
1723 void CalcCent3f(float *cent, float *v1, float *v2, float *v3)
1724 {
1725
1726         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
1727         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
1728         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
1729 }
1730
1731 void CalcCent4f(float *cent, float *v1, float *v2, float *v3, float *v4)
1732 {
1733
1734         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
1735         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
1736         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
1737 }
1738
1739 float Sqrt3f(float f)
1740 {
1741         if(f==0.0) return 0;
1742         if(f<0) return (float)(-exp(log(-f)/3));
1743         else return (float)(exp(log(f)/3));
1744 }
1745
1746 double Sqrt3d(double d)
1747 {
1748         if(d==0.0) return 0;
1749         if(d<0) return -exp(log(-d)/3);
1750         else return exp(log(d)/3);
1751 }
1752
1753 /* distance v1 to line v2-v3 */
1754 /* using Hesse formula, NO LINE PIECE! */
1755 float DistVL2Dfl( float *v1, float *v2, float *v3)  {
1756         float a[2],deler;
1757
1758         a[0]= v2[1]-v3[1];
1759         a[1]= v3[0]-v2[0];
1760         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
1761         if(deler== 0.0f) return 0;
1762
1763         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
1764
1765 }
1766
1767 /* distance v1 to line-piece v2-v3 */
1768 float PdistVL2Dfl( float *v1, float *v2, float *v3)
1769 {
1770         float labda, rc[2], pt[2], len;
1771
1772         rc[0]= v3[0]-v2[0];
1773         rc[1]= v3[1]-v2[1];
1774         len= rc[0]*rc[0]+ rc[1]*rc[1];
1775         if(len==0.0) {
1776                 rc[0]= v1[0]-v2[0];
1777                 rc[1]= v1[1]-v2[1];
1778                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
1779         }
1780
1781         labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len;
1782         if(labda<=0.0) {
1783                 pt[0]= v2[0];
1784                 pt[1]= v2[1];
1785         }
1786         else if(labda>=1.0) {
1787                 pt[0]= v3[0];
1788                 pt[1]= v3[1];
1789         }
1790         else {
1791                 pt[0]= labda*rc[0]+v2[0];
1792                 pt[1]= labda*rc[1]+v2[1];
1793         }
1794
1795         rc[0]= pt[0]-v1[0];
1796         rc[1]= pt[1]-v1[1];
1797         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
1798 }
1799
1800 float AreaF2Dfl( float *v1, float *v2, float *v3)
1801 {
1802         return (float)(0.5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ));
1803 }
1804
1805
1806 float AreaQ3Dfl( float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
1807 {
1808         float len, vec1[3], vec2[3], n[3];
1809
1810         VecSubf(vec1, v2, v1);
1811         VecSubf(vec2, v4, v1);
1812         Crossf(n, vec1, vec2);
1813         len= Normalise(n);
1814
1815         VecSubf(vec1, v4, v3);
1816         VecSubf(vec2, v2, v3);
1817         Crossf(n, vec1, vec2);
1818         len+= Normalise(n);
1819
1820         return (len/2.0f);
1821 }
1822
1823 float AreaT3Dfl( float *v1, float *v2, float *v3)  /* Triangles */
1824 {
1825         float len, vec1[3], vec2[3], n[3];
1826
1827         VecSubf(vec1, v3, v2);
1828         VecSubf(vec2, v1, v2);
1829         Crossf(n, vec1, vec2);
1830         len= Normalise(n);
1831
1832         return (len/2.0f);
1833 }
1834
1835 #define MAX2(x,y)               ( (x)>(y) ? (x) : (y) )
1836 #define MAX3(x,y,z)             MAX2( MAX2((x),(y)) , (z) )
1837
1838
1839 float AreaPoly3Dfl(int nr, float *verts, float *normal)
1840 {
1841         float x, y, z, area, max;
1842         float *cur, *prev;
1843         int a, px=0, py=1;
1844
1845         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
1846         x= (float)fabs(normal[0]);
1847         y= (float)fabs(normal[1]);
1848         z= (float)fabs(normal[2]);
1849         max = MAX3(x, y, z);
1850         if(max==y) py=2;
1851         else if(max==x) {
1852                 px=1;
1853                 py= 2;
1854         }
1855
1856         /* The Trapezium Area Rule */
1857         prev= verts+3*(nr-1);
1858         cur= verts;
1859         area= 0;
1860         for(a=0; a<nr; a++) {
1861                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
1862                 prev= cur;
1863                 cur+=3;
1864         }
1865
1866         return (float)fabs(0.5*area/max);
1867 }
1868
1869 void MinMax3(float *min, float *max, float *vec)
1870 {
1871         if(min[0]>vec[0]) min[0]= vec[0];
1872         if(min[1]>vec[1]) min[1]= vec[1];
1873         if(min[2]>vec[2]) min[2]= vec[2];
1874
1875         if(max[0]<vec[0]) max[0]= vec[0];
1876         if(max[1]<vec[1]) max[1]= vec[1];
1877         if(max[2]<vec[2]) max[2]= vec[2];
1878 }
1879
1880 /* ************ EULER *************** */
1881
1882 void EulToMat3( float *eul, float mat[][3])
1883 {
1884         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1885
1886         ci = cos(eul[0]);
1887         cj = cos(eul[1]);
1888         ch = cos(eul[2]);
1889         si = sin(eul[0]);
1890         sj = sin(eul[1]);
1891         sh = sin(eul[2]);
1892         cc = ci*ch;
1893         cs = ci*sh;
1894         sc = si*ch;
1895         ss = si*sh;
1896
1897         mat[0][0] = (float)(cj*ch);
1898         mat[1][0] = (float)(sj*sc-cs);
1899         mat[2][0] = (float)(sj*cc+ss);
1900         mat[0][1] = (float)(cj*sh);
1901         mat[1][1] = (float)(sj*ss+cc);
1902         mat[2][1] = (float)(sj*cs-sc);
1903         mat[0][2] = (float)-sj;
1904         mat[1][2] = (float)(cj*si);
1905         mat[2][2] = (float)(cj*ci);
1906
1907 }
1908
1909 void EulToMat4( float *eul,float mat[][4])
1910 {
1911         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1912
1913         ci = cos(eul[0]);
1914         cj = cos(eul[1]);
1915         ch = cos(eul[2]);
1916         si = sin(eul[0]);
1917         sj = sin(eul[1]);
1918         sh = sin(eul[2]);
1919         cc = ci*ch;
1920         cs = ci*sh;
1921         sc = si*ch;
1922         ss = si*sh;
1923
1924         mat[0][0] = (float)(cj*ch);
1925         mat[1][0] = (float)(sj*sc-cs);
1926         mat[2][0] = (float)(sj*cc+ss);
1927         mat[0][1] = (float)(cj*sh);
1928         mat[1][1] = (float)(sj*ss+cc);
1929         mat[2][1] = (float)(sj*cs-sc);
1930         mat[0][2] = (float)-sj;
1931         mat[1][2] = (float)(cj*si);
1932         mat[2][2] = (float)(cj*ci);
1933
1934
1935         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
1936         mat[3][3]= 1.0f;
1937 }
1938
1939
1940 void Mat3ToEul(float tmat[][3], float *eul)
1941 {
1942         float cy, quat[4], mat[3][3];
1943
1944         Mat3ToQuat(tmat, quat);
1945         QuatToMat3(quat, mat);
1946         Mat3CpyMat3(mat, tmat);
1947         Mat3Ortho(mat);
1948
1949         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
1950
1951         if (cy > 16.0*FLT_EPSILON) {
1952                 eul[0] = (float)atan2(mat[1][2], mat[2][2]);
1953                 eul[1] = (float)atan2(-mat[0][2], cy);
1954                 eul[2] = (float)atan2(mat[0][1], mat[0][0]);
1955         } else {
1956                 eul[0] = (float)atan2(-mat[2][1], mat[1][1]);
1957                 eul[1] = (float)atan2(-mat[0][2], cy);
1958                 eul[2] = 0.0f;
1959         }
1960 }
1961
1962 void Mat3ToEuln( float tmat[][3], float *eul)
1963 {
1964         float sin1, cos1, sin2, cos2, sin3, cos3;
1965
1966         sin1 = -tmat[2][0];
1967         cos1 = (float)sqrt(1 - sin1*sin1);
1968
1969         if ( fabs(cos1) > FLT_EPSILON ) {
1970                 sin2 = tmat[2][1] / cos1;
1971                 cos2 = tmat[2][2] / cos1;
1972                 sin3 = tmat[1][0] / cos1;
1973                 cos3 = tmat[0][0] / cos1;
1974     }
1975         else {
1976                 sin2 = -tmat[1][2];
1977                 cos2 = tmat[1][1];
1978                 sin3 = 0.0;
1979                 cos3 = 1.0;
1980     }
1981
1982         eul[0] = (float)atan2(sin3, cos3);
1983         eul[1] = (float)atan2(sin1, cos1);
1984         eul[2] = (float)atan2(sin2, cos2);
1985
1986 }
1987
1988
1989 void QuatToEul( float *quat, float *eul)
1990 {
1991         float mat[3][3];
1992
1993         QuatToMat3(quat, mat);
1994         Mat3ToEul(mat, eul);
1995 }
1996
1997
1998 void EulToQuat( float *eul, float *quat)
1999 {
2000     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
2001
2002     ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
2003     ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
2004     si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
2005     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
2006
2007         quat[0] = cj*cc + sj*ss;
2008         quat[1] = cj*sc - sj*cs;
2009         quat[2] = cj*ss + sj*cc;
2010         quat[3] = cj*cs - sj*sc;
2011 }
2012
2013 void VecRotToMat3( float *vec, float phi, float mat[][3])
2014 {
2015         /* rotation of phi radials around vec */
2016         float vx, vx2, vy, vy2, vz, vz2, co, si;
2017
2018         vx= vec[0];
2019         vy= vec[1];
2020         vz= vec[2];
2021         vx2= vx*vx;
2022         vy2= vy*vy;
2023         vz2= vz*vz;
2024         co= (float)cos(phi);
2025         si= (float)sin(phi);
2026
2027         mat[0][0]= vx2+co*(1.0f-vx2);
2028         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
2029         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
2030         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
2031         mat[1][1]= vy2+co*(1.0f-vy2);
2032         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
2033         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
2034         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
2035         mat[2][2]= vz2+co*(1.0f-vz2);
2036
2037 }
2038
2039 void VecRotToQuat( float *vec, float phi, float *quat)
2040 {
2041         /* rotation of phi radials around vec */
2042         float si;
2043
2044         quat[1]= vec[0];
2045         quat[2]= vec[1];
2046         quat[3]= vec[2];
2047
2048         if( Normalise(quat+1) == 0.0) {
2049                 QuatOne(quat);
2050         }
2051         else {
2052                 quat[0]= (float)cos( phi/2.0 );
2053                 si= (float)sin( phi/2.0 );
2054                 quat[1] *= si;
2055                 quat[2] *= si;
2056                 quat[3] *= si;
2057         }
2058 }
2059
2060 void euler_rot(float *beul, float ang, char axis)
2061 {
2062         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
2063
2064         eul[0]= eul[1]= eul[2]= 0.0;
2065         if(axis=='x') eul[0]= ang;
2066         else if(axis=='y') eul[1]= ang;
2067         else eul[2]= ang;
2068
2069         EulToMat3(eul, mat1);
2070         EulToMat3(beul, mat2);
2071
2072         Mat3MulMat3(totmat, mat2, mat1);
2073
2074         Mat3ToEul(totmat, beul);
2075
2076 }
2077
2078
2079
2080 void SizeToMat3( float *size, float mat[][3])
2081 {
2082         mat[0][0]= size[0];
2083         mat[0][1]= 0.0;
2084         mat[0][2]= 0.0;
2085         mat[1][1]= size[1];
2086         mat[1][0]= 0.0;
2087         mat[1][2]= 0.0;
2088         mat[2][2]= size[2];
2089         mat[2][1]= 0.0;
2090         mat[2][0]= 0.0;
2091 }
2092
2093 void Mat3ToSize( float mat[][3], float *size)
2094 {
2095         float vec[3];
2096
2097
2098         VecCopyf(vec, mat[0]);
2099         size[0]= Normalise(vec);
2100         VecCopyf(vec, mat[1]);
2101         size[1]= Normalise(vec);
2102         VecCopyf(vec, mat[2]);
2103         size[2]= Normalise(vec);
2104
2105 }
2106
2107 void Mat4ToSize( float mat[][4], float *size)
2108 {
2109         float vec[3];
2110
2111
2112         VecCopyf(vec, mat[0]);
2113         size[0]= Normalise(vec);
2114         VecCopyf(vec, mat[1]);
2115         size[1]= Normalise(vec);
2116         VecCopyf(vec, mat[2]);
2117         size[2]= Normalise(vec);
2118 }
2119
2120 /* ************* SPECIALS ******************* */
2121
2122 void triatoquat( float *v1,  float *v2,  float *v3, float *quat)
2123 {
2124         /* imaginary x-axis, y-axis triangle is being rotated */
2125         float vec[3], q1[4], q2[4], n[3], si, co, hoek, mat[3][3], imat[3][3];
2126
2127         /* move z-axis to face-normal */
2128         CalcNormFloat(v1, v2, v3, vec);
2129
2130         n[0]= vec[1];
2131         n[1]= -vec[0];
2132         n[2]= 0.0;
2133         Normalise(n);
2134
2135         if(n[0]==0.0 && n[1]==0.0) n[0]= 1.0;
2136
2137         hoek= -0.5f*saacos(vec[2]);
2138         co= (float)cos(hoek);
2139         si= (float)sin(hoek);
2140         q1[0]= co;
2141         q1[1]= n[0]*si;
2142         q1[2]= n[1]*si;
2143         q1[3]= 0.0f;
2144
2145         /* rotate back line v1-v2 */
2146         QuatToMat3(q1, mat);
2147         Mat3Inv(imat, mat);
2148         VecSubf(vec, v2, v1);
2149         Mat3MulVecfl(imat, vec);
2150
2151         /* what angle has this line with x-axis? */
2152         vec[2]= 0.0;
2153         Normalise(vec);
2154
2155         hoek= (float)(0.5*atan2(vec[1], vec[0]));
2156         co= (float)cos(hoek);
2157         si= (float)sin(hoek);
2158         q2[0]= co;
2159         q2[1]= 0.0f;
2160         q2[2]= 0.0f;
2161         q2[3]= si;
2162
2163         QuatMul(quat, q1, q2);
2164 }
2165
2166 void MinMaxRGB(short c[])
2167 {
2168         if(c[0]>255) c[0]=255;
2169         else if(c[0]<0) c[0]=0;
2170         if(c[1]>255) c[1]=255;
2171         else if(c[1]<0) c[1]=0;
2172         if(c[2]>255) c[2]=255;
2173         else if(c[2]<0) c[2]=0;
2174 }
2175
2176 void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
2177 {
2178         int i;
2179         float f, p, q, t;
2180
2181         h *= 360.0f;
2182
2183         if(s==0 && 0) {
2184                 *r = v;
2185                 *g = v;
2186                 *b = v;
2187         }
2188         else {
2189                 if(h==360) h = 0;
2190
2191                 h /= 60;
2192                 i = (int)floor(h);
2193                 f = h - i;
2194                 p = v*(1.0f-s);
2195                 q = v*(1.0f-(s*f));
2196                 t = v*(1.0f-(s*(1.0f-f)));
2197
2198                 switch (i) {
2199                 case 0 :
2200                         *r = v;
2201                         *g = t;
2202                         *b = p;
2203                         break;
2204                 case 1 :
2205                         *r = q;
2206                         *g = v;
2207                         *b = p;
2208                         break;
2209                 case 2 :
2210                         *r = p;
2211                         *g = v;
2212                         *b = t;
2213                         break;
2214                 case 3 :
2215                         *r = p;
2216                         *g = q;
2217                         *b = v;
2218                         break;
2219                 case 4 :
2220                         *r = t;
2221                         *g = p;
2222                         *b = v;
2223                         break;
2224                 case 5 :
2225                         *r = v;
2226                         *g = p;
2227                         *b = q;
2228                         break;
2229                 }
2230         }
2231 }
2232
2233 void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
2234 {
2235         float h, s, v;
2236         float cmax, cmin, cdelta;
2237         float rc, gc, bc;
2238
2239         cmax = r;
2240         cmin = r;
2241         cmax = (g>cmax ? g:cmax);
2242         cmin = (g<cmin ? g:cmin);
2243         cmax = (b>cmax ? b:cmax);
2244         cmin = (b<cmin ? b:cmin);
2245
2246         v = cmax;               /* value */
2247         if (cmax!=0.0)
2248                 s = (cmax - cmin)/cmax;
2249         else {
2250                 s = 0.0;
2251                 h = 0.0;
2252         }
2253         if (s == 0.0)
2254                 h = -1.0;
2255         else {
2256                 cdelta = cmax-cmin;
2257                 rc = (cmax-r)/cdelta;
2258                 gc = (cmax-g)/cdelta;
2259                 bc = (cmax-b)/cdelta;
2260                 if (r==cmax)
2261                         h = bc-gc;
2262                 else
2263                         if (g==cmax)
2264                                 h = 2.0f+rc-bc;
2265                         else
2266                                 h = 4.0f+gc-rc;
2267                 h = h*60.0f;
2268                 if (h<0.0f)
2269                         h += 360.0f;
2270         }
2271
2272         *ls = s;
2273         *lh = h/360.0f;
2274         if( *lh < 0.0) *lh= 0.0;
2275         *lv = v;
2276 }
2277
2278
2279 /* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
2280    for that reason it is sensitive for endianness... with this function it works correctly
2281 */
2282
2283 unsigned int hsv_to_cpack(float h, float s, float v)
2284 {
2285         short r, g, b;
2286         float rf, gf, bf;
2287         unsigned int col;
2288
2289         hsv_to_rgb(h, s, v, &rf, &gf, &bf);
2290
2291         r= (short)(rf*255.0f);
2292         g= (short)(gf*255.0f);
2293         b= (short)(bf*255.0f);
2294
2295         col= ( r + (g*256) + (b*256*256) );
2296         return col;
2297 }
2298
2299
2300 unsigned int rgb_to_cpack(float r, float g, float b)
2301 {
2302         int ir, ig, ib;
2303
2304         ir= (int)floor(255.0*r);
2305         if(ir<0) ir= 0; else if(ir>255) ir= 255;
2306         ig= (int)floor(255.0*g);
2307         if(ig<0) ig= 0; else if(ig>255) ig= 255;
2308         ib= (int)floor(255.0*b);
2309         if(ib<0) ib= 0; else if(ib>255) ib= 255;
2310
2311         return (ir+ (ig*256) + (ib*256*256));
2312 }
2313
2314 void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
2315 {
2316
2317         *r= (float)((col)&0xFF);
2318         *r /= 255.0f;
2319
2320         *g= (float)(((col)>>8)&0xFF);
2321         *g /= 255.0f;
2322
2323         *b= (float)(((col)>>16)&0xFF);
2324         *b /= 255.0f;
2325 }