source/blender/blenlib/intern/arithb.c

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