libs/mathlib/m4x4.c

   1 /*
   2 Copyright (C) 2001-2006, William Joseph.
   3 All Rights Reserved.
   4
   5 This file is part of GtkRadiant.
   6
   7 GtkRadiant is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 GtkRadiant is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with GtkRadiant; if not, write to the Free Software
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 */
  21
  22 #include "mathlib.h"
  23
  24 const m4x4_t g_m4x4_identity = {
  25   1, 0, 0, 0,
  26   0, 1, 0, 0,
  27   0, 0, 1, 0,
  28   0, 0, 0, 1,
  29 };
  30
  31 void m4x4_identity(m4x4_t matrix)
  32 {
  33   matrix[1] = matrix[2] = matrix[3] =
  34   matrix[4] = matrix[6] = matrix[7] =
  35   matrix[8] = matrix[9] = matrix[11] =
  36   matrix[12] = matrix[13] = matrix[14] = 0;
  37
  38   matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
  39 }
  40
  41 m4x4Handedness_t m4x4_handedness(const m4x4_t matrix)
  42 {
  43   vec3_t cross;
  44   CrossProduct(matrix+0, matrix+4, cross);
  45   return (DotProduct(matrix+8, cross) < 0) ? eLeftHanded : eRightHanded;
  46 }
  47
  48 void m4x4_assign(m4x4_t matrix, const m4x4_t other)
  49 {
  50   M4X4_COPY(matrix, other);
  51 }
  52
  53 void m4x4_translation_for_vec3(m4x4_t matrix, const vec3_t translation)
  54 {
  55   matrix[1] = matrix[2] = matrix[3] =
  56   matrix[4] = matrix[6] = matrix[7] =
  57   matrix[8] = matrix[9] = matrix[11] = 0;
  58
  59   matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
  60
  61   matrix[12] = translation[0];
  62   matrix[13] = translation[1];
  63   matrix[14] = translation[2];
  64 }
  65
  66 /*
  67 clockwise rotation around X, Y, Z, facing along axis
  68  1  0   0    cy 0  sy   cz  sz 0
  69  0  cx  sx   0  1  0   -sz  cz 0
  70  0 -sx  cx  -sy 0  cy   0   0  1
  71
  72 rows of Z by cols of Y
  73  cy*cz -sy*cz+sz -sy*sz+cz
  74 -sz*cy -sz*sy+cz
  75
  76   .. or something like that..
  77
  78 final rotation is Z * Y * X
  79  cy*cz -sx*-sy*cz+cx*sz  cx*-sy*sz+sx*cz
  80 -cy*sz  sx*sy*sz+cx*cz  -cx*-sy*sz+sx*cz
  81  sy    -sx*cy            cx*cy
  82 */
  83
  84 /* transposed
  85 |  cy.cz + 0.sz + sy.0            cy.-sz + 0 .cz +  sy.0          cy.0  + 0 .0  +   sy.1       |
  86 |  sx.sy.cz + cx.sz + -sx.cy.0    sx.sy.-sz + cx.cz + -sx.cy.0    sx.sy.0  + cx.0  + -sx.cy.1  |
  87 | -cx.sy.cz + sx.sz +  cx.cy.0   -cx.sy.-sz + sx.cz +  cx.cy.0   -cx.sy.0  + 0 .0  +  cx.cy.1  |
  88 */
  89 void m4x4_rotation_for_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
  90 {
  91   double cx, sx, cy, sy, cz, sz;
  92
  93   cx = cos(DEG2RAD(euler[0]));
  94   sx = sin(DEG2RAD(euler[0]));
  95   cy = cos(DEG2RAD(euler[1]));
  96   sy = sin(DEG2RAD(euler[1]));
  97   cz = cos(DEG2RAD(euler[2]));
  98   sz = sin(DEG2RAD(euler[2]));
  99
 100   switch(order)
 101   {
 102   case eXYZ:
 103
 104 #if 1
 105
 106     {
 107       matrix[0]  = (vec_t)(cy*cz);
 108       matrix[1]  = (vec_t)(cy*sz);
 109       matrix[2]  = (vec_t)-sy;
 110       matrix[4]  = (vec_t)(sx*sy*cz + cx*-sz);
 111       matrix[5]  = (vec_t)(sx*sy*sz + cx*cz);
 112       matrix[6]  = (vec_t)(sx*cy);
 113       matrix[8]  = (vec_t)(cx*sy*cz + sx*sz);
 114       matrix[9]  = (vec_t)(cx*sy*sz + -sx*cz);
 115       matrix[10] = (vec_t)(cx*cy);
 116     }
 117
 118     matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
 119     matrix[15] =  1;
 120
 121 #else
 122
 123     m4x4_identity(matrix);
 124     matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
 125     matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
 126
 127     {
 128       m4x4_t temp;
 129       m4x4_identity(temp);
 130       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
 131       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
 132       m4x4_premultiply_by_m4x4(matrix, temp);
 133       m4x4_identity(temp);
 134       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
 135       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
 136       m4x4_premultiply_by_m4x4(matrix, temp);
 137     }
 138 #endif
 139
 140     break;
 141
 142   case eYZX:
 143     m4x4_identity(matrix);
 144     matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
 145     matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
 146
 147     {
 148       m4x4_t temp;
 149       m4x4_identity(temp);
 150       temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
 151       temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
 152       m4x4_premultiply_by_m4x4(matrix, temp);
 153       m4x4_identity(temp);
 154       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
 155       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
 156       m4x4_premultiply_by_m4x4(matrix, temp);
 157     }
 158     break;
 159
 160   case eZXY:
 161     m4x4_identity(matrix);
 162     matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
 163     matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
 164
 165     {
 166       m4x4_t temp;
 167       m4x4_identity(temp);
 168       temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
 169       temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
 170       m4x4_premultiply_by_m4x4(matrix, temp);
 171       m4x4_identity(temp);
 172       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
 173       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
 174       m4x4_premultiply_by_m4x4(matrix, temp);
 175     }
 176     break;
 177
 178   case eXZY:
 179     m4x4_identity(matrix);
 180     matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
 181     matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
 182
 183     {
 184       m4x4_t temp;
 185       m4x4_identity(temp);
 186       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
 187       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
 188       m4x4_premultiply_by_m4x4(matrix, temp);
 189       m4x4_identity(temp);
 190       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
 191       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
 192       m4x4_premultiply_by_m4x4(matrix, temp);
 193     }
 194     break;
 195
 196   case eYXZ:
 197
 198 /* transposed
 199 |  cy.cz + sx.sy.-sz + -cx.sy.0   0.cz + cx.-sz + sx.0   sy.cz + -sx.cy.-sz + cx.cy.0 |
 200 |  cy.sz + sx.sy.cz + -cx.sy.0    0.sz + cx.cz + sx.0    sy.sz + -sx.cy.cz + cx.cy.0  |
 201 |  cy.0 + sx.sy.0 + -cx.sy.1      0.0 + cx.0 + sx.1      sy.0 + -sx.cy.0 + cx.cy.1    |
 202 */
 203
 204 #if 1
 205
 206   {
 207     matrix[0]  = (vec_t)(cy*cz + sx*sy*-sz);
 208     matrix[1]  = (vec_t)(cy*sz + sx*sy*cz);
 209     matrix[2]  = (vec_t)(-cx*sy);
 210     matrix[4]  = (vec_t)(cx*-sz);
 211     matrix[5]  = (vec_t)(cx*cz);
 212     matrix[6]  = (vec_t)(sx);
 213     matrix[8]  = (vec_t)(sy*cz + -sx*cy*-sz);
 214     matrix[9]  = (vec_t)(sy*sz + -sx*cy*cz);
 215     matrix[10] = (vec_t)(cx*cy);
 216   }
 217
 218   matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
 219   matrix[15] =  1;
 220
 221 #else
 222
 223   m4x4_identity(matrix);
 224   matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
 225   matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
 226
 227   {
 228     m4x4_t temp;
 229     m4x4_identity(temp);
 230     temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
 231     temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
 232     m4x4_premultiply_by_m4x4(matrix, temp);
 233     m4x4_identity(temp);
 234     temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
 235     temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
 236     m4x4_premultiply_by_m4x4(matrix, temp);
 237   }
 238 #endif
 239   break;
 240
 241   case eZYX:
 242 #if 1
 243
 244   {
 245     matrix[0]  = (vec_t)(cy*cz);
 246     matrix[4]  = (vec_t)(cy*-sz);
 247     matrix[8]  = (vec_t)sy;
 248     matrix[1]  = (vec_t)(sx*sy*cz + cx*sz);
 249     matrix[5]  = (vec_t)(sx*sy*-sz + cx*cz);
 250     matrix[9]  = (vec_t)(-sx*cy);
 251     matrix[2]  = (vec_t)(cx*-sy*cz + sx*sz);
 252     matrix[6]  = (vec_t)(cx*-sy*-sz + sx*cz);
 253     matrix[10] = (vec_t)(cx*cy);
 254   }
 255
 256   matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
 257   matrix[15] =  1;
 258
 259 #else
 260
 261   m4x4_identity(matrix);
 262   matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
 263   matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
 264   {
 265     m4x4_t temp;
 266     m4x4_identity(temp);
 267     temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
 268     temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
 269     m4x4_premultiply_by_m4x4(matrix, temp);
 270     m4x4_identity(temp);
 271     temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
 272     temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
 273     m4x4_premultiply_by_m4x4(matrix, temp);
 274   }
 275
 276 #endif
 277   break;
 278
 279   }
 280 }
 281
 282 void m4x4_scale_for_vec3(m4x4_t matrix, const vec3_t scale)
 283 {
 284   matrix[1] = matrix[2] = matrix[3] =
 285   matrix[4] = matrix[6] = matrix[7] =
 286   matrix[8] = matrix[9] = matrix[11] =
 287   matrix[12] = matrix[13] = matrix[14] = 0;
 288
 289   matrix[15] = 1;
 290
 291   matrix[0] = scale[0];
 292   matrix[5] = scale[1];
 293   matrix[10] = scale[2];
 294 }
 295
 296 void m4x4_rotation_for_quat(m4x4_t matrix, const vec4_t quat)
 297 {
 298 #if 0
 299   const double xx = quat[0] * quat[0];
 300   const double xy = quat[0] * quat[1];
 301   const double xz = quat[0] * quat[2];
 302   const double xw = quat[0] * quat[3];
 303
 304   const double yy = quat[1] * quat[1];
 305   const double yz = quat[1] * quat[2];
 306   const double yw = quat[1] * quat[3];
 307
 308   const double zz = quat[2] * quat[2];
 309   const double zw = quat[2] * quat[3];
 310
 311   matrix[0]  = 1 - 2 * ( yy + zz );
 312   matrix[4]  =     2 * ( xy - zw );
 313   matrix[8]  =     2 * ( xz + yw );
 314
 315   matrix[1]  =     2 * ( xy + zw );
 316   matrix[5]  = 1 - 2 * ( xx + zz );
 317   matrix[9]  =     2 * ( yz - xw );
 318
 319   matrix[2]  =     2 * ( xz - yw );
 320   matrix[6]  =     2 * ( yz + xw );
 321   matrix[10] = 1 - 2 * ( xx + yy );
 322 #else
 323   const double x2 = quat[0] + quat[0];
 324   const double y2 = quat[1] + quat[1];
 325   const double z2 = quat[2] + quat[2];
 326   const double xx = quat[0] * x2;
 327   const double xy = quat[0] * y2;
 328   const double xz = quat[0] * z2;
 329   const double yy = quat[1] * y2;
 330   const double yz = quat[1] * z2;
 331   const double zz = quat[2] * z2;
 332   const double wx = quat[3] * x2;
 333   const double wy = quat[3] * y2;
 334   const double wz = quat[3] * z2;
 335
 336   matrix[0] = (vec_t)( 1.0 - (yy + zz) );
 337   matrix[4] = (vec_t)(xy - wz);
 338   matrix[8] = (vec_t)(xz + wy);
 339
 340   matrix[1] = (vec_t)(xy + wz);
 341   matrix[5] = (vec_t)( 1.0 - (xx + zz) );
 342   matrix[9] = (vec_t)(yz - wx);
 343
 344   matrix[2] = (vec_t)(xz - wy);
 345   matrix[6] = (vec_t)(yz + wx);
 346   matrix[10] = (vec_t)( 1.0 - (xx + yy) );
 347 #endif
 348
 349   matrix[3]  = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
 350   matrix[15] = 1;
 351 }
 352
 353 void m4x4_rotation_for_axisangle(m4x4_t matrix, const vec3_t axis, double angle)
 354 {
 355   vec4_t quat;
 356   quat_for_axisangle(quat, axis, angle);
 357   m4x4_rotation_for_quat(matrix, quat);
 358 }
 359
 360 void m4x4_frustum(m4x4_t matrix,
 361                       vec_t left, vec_t right,
 362                       vec_t bottom, vec_t top,
 363                       vec_t nearval, vec_t farval)
 364 {
 365    matrix[0] = (vec_t)( (2*nearval) / (right-left) );
 366    matrix[1] = 0;
 367    matrix[2] = 0;
 368    matrix[3] = 0;
 369
 370    matrix[4] = 0;
 371    matrix[5] = (vec_t)( (2*nearval) / (top-bottom) );
 372    matrix[6] = 0;
 373    matrix[7] = 0;
 374
 375    matrix[8] = (vec_t)( (right+left) / (right-left) );
 376    matrix[9] = (vec_t)( (top+bottom) / (top-bottom) );
 377    matrix[10] = (vec_t)( -(farval+nearval) / (farval-nearval) );
 378    matrix[11] =-1;
 379
 380    matrix[12] = 0;
 381    matrix[13] = 0;
 382    matrix[14] = (vec_t)( -(2*farval*nearval) / (farval-nearval) );
 383    matrix[15] = 0;
 384 }
 385
 386
 387 void m4x4_get_translation_vec3(const m4x4_t matrix, vec3_t translation)
 388 {
 389   translation[0] = matrix[12];
 390         translation[1] = matrix[13];
 391         translation[2] = matrix[14];
 392 }
 393
 394 void m4x4_get_rotation_vec3(const m4x4_t matrix, vec3_t euler, eulerOrder_t order)
 395 {
 396   double a, ca;
 397
 398   switch(order)
 399   {
 400   case eXYZ:
 401     a = asin(-matrix[2]);
 402     ca = cos(a);
 403     euler[1] = (vec_t)RAD2DEG(a);  /* Calculate Y-axis angle */
 404
 405     if (fabs(ca) > 0.005) /* Gimbal lock? */
 406     {
 407       /* No, so get Z-axis angle */
 408       euler[2] = (vec_t)RAD2DEG(atan2(matrix[1] / ca, matrix[0]/ ca));
 409
 410       /* Get X-axis angle */
 411       euler[0] = (vec_t)RAD2DEG(atan2(matrix[6] / ca, matrix[10] / ca));
 412     }
 413     else /* Gimbal lock has occurred */
 414     {
 415       /* Set Z-axis angle to zero */
 416       euler[2]  = 0;
 417
 418       /* And calculate X-axis angle */
 419       euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9], matrix[5]));
 420     }
 421     break;
 422   case eYZX:
 423     /* NOT IMPLEMENTED */
 424     break;
 425   case eZXY:
 426     /* NOT IMPLEMENTED */
 427     break;
 428   case eXZY:
 429     /* NOT IMPLEMENTED */
 430     break;
 431   case eYXZ:
 432     a = asin(matrix[6]);
 433     ca = cos(a);
 434     euler[0] = (vec_t)RAD2DEG(a);  /* Calculate X-axis angle */
 435
 436     if (fabs(ca) > 0.005) /* Gimbal lock? */
 437     {
 438       /* No, so get Y-axis angle */
 439       euler[1] = (vec_t)RAD2DEG(atan2(-matrix[2] / ca, matrix[10]/ ca));
 440
 441       /* Get Z-axis angle */
 442       euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[5] / ca));
 443     }
 444     else /* Gimbal lock has occurred */
 445     {
 446       /* Set Z-axis angle to zero */
 447       euler[2]  = 0;
 448
 449       /* And calculate Y-axis angle */
 450       euler[1] = (vec_t)RAD2DEG(atan2(matrix[8], matrix[0]));
 451     }
 452     break;
 453   case eZYX:
 454     a = asin(matrix[8]);
 455     ca = cos(a);
 456     euler[1] = (vec_t)RAD2DEG(a);  /* Calculate Y-axis angle */
 457
 458     if (fabs(ca) > 0.005) /* Gimbal lock? */
 459     {
 460       /* No, so get X-axis angle */
 461       euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9] / ca, matrix[10]/ ca));
 462
 463       /* Get Z-axis angle */
 464       euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[0] / ca));
 465     }
 466     else /* Gimbal lock has occurred */
 467     {
 468       /* Set X-axis angle to zero */
 469       euler[0]  = 0;
 470
 471       /* And calculate Z-axis angle */
 472       euler[2] = (vec_t)RAD2DEG(atan2(matrix[1], matrix[5]));
 473     }
 474     break;
 475   }
 476
 477   /* return only positive angles in [0,360] */
 478   if (euler[0] < 0) euler[0] += 360;
 479   if (euler[1] < 0) euler[1] += 360;
 480   if (euler[2] < 0) euler[2] += 360;
 481 }
 482
 483 void m4x4_get_scale_vec3(const m4x4_t matrix, vec3_t scale)
 484 {
 485   scale[0] = VectorLength(matrix+0);
 486   scale[1] = VectorLength(matrix+4);
 487   scale[2] = VectorLength(matrix+8);
 488 }
 489
 490 void m4x4_get_transform_vec3(const m4x4_t matrix, vec3_t translation, vec3_t euler, eulerOrder_t order, vec3_t scale)
 491 {
 492   m4x4_t normalised;
 493   m4x4_assign(normalised, matrix);
 494   scale[0] = VectorNormalize(normalised+0, normalised+0);
 495   scale[1] = VectorNormalize(normalised+4, normalised+4);
 496   scale[2] = VectorNormalize(normalised+8, normalised+8);
 497   if(m4x4_handedness(normalised) == eLeftHanded)
 498   {
 499     VectorNegate(normalised+0, normalised+0);
 500     VectorNegate(normalised+4, normalised+4);
 501     VectorNegate(normalised+8, normalised+8);
 502     scale[0] = -scale[0];
 503     scale[1] = -scale[1];
 504     scale[2] = -scale[2];
 505   }
 506   m4x4_get_rotation_vec3(normalised, euler, order);
 507   m4x4_get_translation_vec3(matrix, translation);
 508 }
 509
 510 void m4x4_translate_by_vec3(m4x4_t matrix, const vec3_t translation)
 511 {
 512   m4x4_t temp;
 513   m4x4_translation_for_vec3(temp, translation);
 514   m4x4_multiply_by_m4x4(matrix, temp);
 515 }
 516
 517 void m4x4_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
 518 {
 519   m4x4_t temp;
 520   m4x4_rotation_for_vec3(temp, euler, order);
 521   m4x4_multiply_by_m4x4(matrix, temp);
 522 }
 523
 524 void m4x4_scale_by_vec3(m4x4_t matrix, const vec3_t scale)
 525 {
 526   m4x4_t temp;
 527   m4x4_scale_for_vec3(temp, scale);
 528   m4x4_multiply_by_m4x4(matrix, temp);
 529 }
 530
 531 void m4x4_rotate_by_quat(m4x4_t matrix, const vec4_t rotation)
 532 {
 533   m4x4_t temp;
 534   m4x4_rotation_for_quat(temp, rotation);
 535   m4x4_multiply_by_m4x4(matrix, temp);
 536 }
 537
 538 void m4x4_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle)
 539 {
 540   m4x4_t temp;
 541   m4x4_rotation_for_axisangle(temp, axis, angle);
 542   m4x4_multiply_by_m4x4(matrix, temp);
 543 }
 544
 545 void m4x4_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale)
 546 {
 547   m4x4_translate_by_vec3(matrix, translation);
 548   m4x4_rotate_by_vec3(matrix, euler, order);
 549   m4x4_scale_by_vec3(matrix, scale);
 550 }
 551
 552 void m4x4_pivoted_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint)
 553 {
 554   vec3_t vec3_temp;
 555   VectorNegate(pivotpoint, vec3_temp);
 556
 557   m4x4_translate_by_vec3(matrix, pivotpoint);
 558   m4x4_rotate_by_vec3(matrix, euler, order);
 559   m4x4_translate_by_vec3(matrix, vec3_temp);
 560 }
 561
 562 void m4x4_pivoted_scale_by_vec3(m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint)
 563 {
 564   vec3_t vec3_temp;
 565   VectorNegate(pivotpoint, vec3_temp);
 566
 567   m4x4_translate_by_vec3(matrix, pivotpoint);
 568   m4x4_scale_by_vec3(matrix, scale);
 569   m4x4_translate_by_vec3(matrix, vec3_temp);
 570 }
 571
 572 void m4x4_pivoted_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale, const vec3_t pivotpoint)
 573 {
 574   vec3_t vec3_temp;
 575
 576   VectorAdd(pivotpoint, translation, vec3_temp);
 577   m4x4_translate_by_vec3(matrix, vec3_temp);
 578   m4x4_rotate_by_vec3(matrix, euler, order);
 579   m4x4_scale_by_vec3(matrix, scale);
 580   VectorNegate(pivotpoint, vec3_temp);
 581   m4x4_translate_by_vec3(matrix, vec3_temp);
 582 }
 583
 584 void m4x4_pivoted_transform_by_rotation(m4x4_t matrix, const vec3_t translation, const m4x4_t rotation, const vec3_t scale, const vec3_t pivotpoint)
 585 {
 586   vec3_t vec3_temp;
 587
 588   VectorAdd(pivotpoint, translation, vec3_temp);
 589   m4x4_translate_by_vec3(matrix, vec3_temp);
 590   m4x4_multiply_by_m4x4(matrix, rotation);
 591   m4x4_scale_by_vec3(matrix, scale);
 592   VectorNegate(pivotpoint, vec3_temp);
 593   m4x4_translate_by_vec3(matrix, vec3_temp);
 594 }
 595
 596 void m4x4_pivoted_rotate_by_quat(m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint)
 597 {
 598   vec3_t vec3_temp;
 599   VectorNegate(pivotpoint, vec3_temp);
 600
 601   m4x4_translate_by_vec3(matrix, pivotpoint);
 602   m4x4_rotate_by_quat(matrix, rotation);
 603   m4x4_translate_by_vec3(matrix, vec3_temp);
 604 }
 605
 606 void m4x4_pivoted_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle, const vec3_t pivotpoint)
 607 {
 608   vec3_t vec3_temp;
 609   VectorNegate(pivotpoint, vec3_temp);
 610
 611   m4x4_translate_by_vec3(matrix, pivotpoint);
 612   m4x4_rotate_by_axisangle(matrix, axis, angle);
 613   m4x4_translate_by_vec3(matrix, vec3_temp);
 614 }
 615
 616 /*
 617 A = A.B
 618
 619 A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
 620 A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
 621 A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
 622 A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
 623
 624 A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
 625 A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
 626 A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
 627 A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
 628
 629 A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
 630 A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
 631 A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
 632 A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
 633
 634 A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
 635 A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
 636 A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
 637 A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
 638 */
 639
 640 void m4x4_multiply_by_m4x4(m4x4_t dst, const m4x4_t src)
 641 {
 642         vec_t dst0, dst1, dst2, dst3;
 643
 644 #if 1
 645
 646   dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
 647   dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
 648   dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8] + src[11]* dst[12];
 649   dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + src[15]* dst[12];
 650   dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3;
 651
 652   dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
 653   dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
 654   dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9] + src[11]* dst[13];
 655   dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + src[15]* dst[13];
 656   dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3;
 657
 658   dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10]+ src[3] * dst[14];
 659   dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10]+ src[7] * dst[14];
 660   dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10]+ src[11]* dst[14];
 661   dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ src[15]* dst[14];
 662   dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3;
 663
 664   dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11]+ src[3] * dst[15];
 665   dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11]+ src[7] * dst[15];
 666   dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10]* dst[11]+ src[11]* dst[15];
 667   dst3 = src[12]* dst[3] + src[13]* dst[7] + src[14]* dst[11]+ src[15]* dst[15];
 668   dst[3] = dst0; dst[7] = dst1; dst[11]= dst2; dst[15]= dst3;
 669
 670 #else
 671
 672   vec_t * p = dst;
 673         for(int i=0;i<4;i++)
 674         {
 675                 dst1 =  src[0]  * p[0];
 676                 dst1 += src[1]  * p[4];
 677                 dst1 += src[2]  * p[8];
 678                 dst1 += src[3]  * p[12];
 679                 dst2 =  src[4]  * p[0];
 680                 dst2 += src[5]  * p[4];
 681                 dst2 += src[6]  * p[8];
 682                 dst2 += src[7]  * p[12];
 683                 dst3 =  src[8]  * p[0];
 684                 dst3 += src[9]  * p[4];
 685                 dst3 += src[10] * p[8];
 686                 dst3 += src[11] * p[12];
 687                 dst4 =  src[12] * p[0];
 688                 dst4 += src[13] * p[4];
 689                 dst4 += src[14] * p[8];
 690                 dst4 += src[15] * p[12];
 691
 692                 p[0] = dst1;
 693                 p[4] = dst2;
 694                 p[8] = dst3;
 695                 p[12] = dst4;
 696     p++;
 697         }
 698
 699 #endif
 700 }
 701
 702 /*
 703 A = B.A
 704
 705 A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
 706 A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
 707 A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
 708 A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
 709
 710 A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
 711 A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
 712 A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
 713 A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
 714
 715 A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
 716 A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
 717 A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
 718 A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
 719
 720 A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
 721 A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
 722 A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
 723 A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
 724 */
 725
 726 void m4x4_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src)
 727 {
 728         vec_t dst0, dst1, dst2, dst3;
 729
 730 #if 1
 731
 732   dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
 733   dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
 734   dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10]+ dst[3] * src[14];
 735   dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11]+ dst[3] * src[15];
 736   dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3]= dst3;
 737
 738   dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
 739   dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
 740   dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10]+ dst[7] * src[14];
 741   dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11]+ dst[7] * src[15];
 742   dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7]= dst3;
 743
 744   dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8] + dst[11]* src[12];
 745   dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9] + dst[11]* src[13];
 746   dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10]+ dst[11]* src[14];
 747   dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10]* src[11]+ dst[11]* src[15];
 748   dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11]= dst3;
 749
 750   dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12];
 751   dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13];
 752   dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14];
 753   dst3 = dst[12]* src[3] + dst[13]* src[7] + dst[14]* src[11]+ dst[15]* src[15];
 754   dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15]= dst3;
 755
 756 #else
 757
 758   vec_t* p = dst;
 759         for(int i=0;i<4;i++)
 760         {
 761                 dst1 =  src[0]  * p[0];
 762                 dst2 =  src[1]  * p[0];
 763                 dst3 =  src[2]  * p[0];
 764                 dst4 =  src[3]  * p[0];
 765                 dst1 += src[4]  * p[1];
 766                 dst2 += src[5]  * p[1];
 767                 dst3 += src[6]  * p[1];
 768                 dst4 += src[7]  * p[1];
 769                 dst1 += src[8]  * p[2];
 770                 dst2 += src[9]  * p[2];
 771                 dst4 += src[11] * p[2];
 772                 dst3 += src[10] * p[2];
 773                 dst1 += src[12] * p[3];
 774                 dst2 += src[13] * p[3];
 775                 dst3 += src[14] * p[3];
 776                 dst4 += src[15] * p[3];
 777
 778                 *p++ = dst1;
 779                 *p++ = dst2;
 780                 *p++ = dst3;
 781                 *p++ = dst4;
 782         }
 783
 784 #endif
 785 }
 786
 787 void m4x4_orthogonal_multiply_by_m4x4(m4x4_t dst, const m4x4_t src)
 788 {
 789         vec_t dst0, dst1, dst2, dst3;
 790
 791   dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8];
 792   dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8];
 793   dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8];
 794   dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + dst[12];
 795   dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3;
 796
 797   dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9];
 798   dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9];
 799   dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9];
 800   dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + dst[13];
 801   dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3;
 802
 803   dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10];
 804   dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10];
 805   dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10];
 806   dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ dst[14];
 807   dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3;
 808 }
 809
 810 void m4x4_orthogonal_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src)
 811 {
 812         vec_t dst0, dst1, dst2;
 813
 814   dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8];
 815   dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9];
 816   dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10];
 817   dst[0] = dst0; dst[1] = dst1; dst[2] = dst2;
 818
 819   dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8];
 820   dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9];
 821   dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10];
 822   dst[4] = dst0; dst[5] = dst1; dst[6] = dst2;
 823
 824   dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8];
 825   dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9];
 826   dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10];
 827   dst[8] = dst0; dst[9] = dst1; dst[10] = dst2;
 828
 829   dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12];
 830   dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13];
 831   dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14];
 832   dst[12] = dst0; dst[13] = dst1; dst[14] = dst2;
 833 }
 834
 835 void m4x4_transform_point(const m4x4_t matrix, vec3_t point)
 836 {
 837   float out1, out2, out3;
 838
 839         out1 =  matrix[0]  * point[0] + matrix[4]  * point[1] + matrix[8]  * point[2] + matrix[12];
 840         out2 =  matrix[1]  * point[0] + matrix[5]  * point[1] + matrix[9]  * point[2] + matrix[13];
 841         out3 =  matrix[2]  * point[0] + matrix[6]  * point[1] + matrix[10] * point[2] + matrix[14];
 842
 843         point[0] = out1;
 844         point[1] = out2;
 845         point[2] = out3;
 846 }
 847
 848 void m4x4_transform_normal(const m4x4_t matrix, vec3_t normal)
 849 {
 850   float out1, out2, out3;
 851
 852         out1 =  matrix[0]  * normal[0] + matrix[4]  * normal[1] + matrix[8]  * normal[2];
 853         out2 =  matrix[1]  * normal[0] + matrix[5]  * normal[1] + matrix[9]  * normal[2];
 854         out3 =  matrix[2]  * normal[0] + matrix[6]  * normal[1] + matrix[10] * normal[2];
 855
 856         normal[0] = out1;
 857         normal[1] = out2;
 858         normal[2] = out3;
 859 }
 860
 861 void m4x4_transform_vec4(const m4x4_t matrix, vec4_t vector)
 862 {
 863   float out1, out2, out3, out4;
 864
 865         out1 =  matrix[0]  * vector[0] + matrix[4]  * vector[1] + matrix[8]  * vector[2] + matrix[12] * vector[3];
 866         out2 =  matrix[1]  * vector[0] + matrix[5]  * vector[1] + matrix[9]  * vector[2] + matrix[13] * vector[3];
 867         out3 =  matrix[2]  * vector[0] + matrix[6]  * vector[1] + matrix[10] * vector[2] + matrix[14] * vector[3];
 868         out4 =  matrix[3]  * vector[0] + matrix[7]  * vector[1] + matrix[11] * vector[2] + matrix[15] * vector[3];
 869
 870         vector[0] = out1;
 871         vector[1] = out2;
 872         vector[2] = out3;
 873   vector[3] = out4;
 874 }
 875
 876 #define CLIP_X_LT_W(p) ((p)[0] < (p)[3])
 877 #define CLIP_X_GT_W(p) ((p)[0] > -(p)[3])
 878 #define CLIP_Y_LT_W(p) ((p)[1] < (p)[3])
 879 #define CLIP_Y_GT_W(p) ((p)[1] > -(p)[3])
 880 #define CLIP_Z_LT_W(p) ((p)[2] < (p)[3])
 881 #define CLIP_Z_GT_W(p) ((p)[2] > -(p)[3])
 882
 883 clipmask_t homogenous_clip_point(const vec4_t clipped)
 884 {
 885   clipmask_t result = CLIP_FAIL;
 886   if(CLIP_X_LT_W(clipped)) result &= ~CLIP_LT_X; // X < W
 887   if(CLIP_X_GT_W(clipped)) result &= ~CLIP_GT_X; // X > -W
 888   if(CLIP_Y_LT_W(clipped)) result &= ~CLIP_LT_Y; // Y < W
 889   if(CLIP_Y_GT_W(clipped)) result &= ~CLIP_GT_Y; // Y > -W
 890   if(CLIP_Z_LT_W(clipped)) result &= ~CLIP_LT_Z; // Z < W
 891   if(CLIP_Z_GT_W(clipped)) result &= ~CLIP_GT_Z; // Z > -W
 892   return result;
 893 }
 894
 895 clipmask_t m4x4_clip_point(const m4x4_t matrix, const vec3_t point, vec4_t clipped)
 896 {
 897   clipped[0] = point[0];
 898   clipped[1] = point[1];
 899   clipped[2] = point[2];
 900   clipped[3] = 1;
 901   m4x4_transform_vec4(matrix, clipped);
 902   return homogenous_clip_point(clipped);
 903 }
 904
 905
 906 unsigned int homogenous_clip_triangle(vec4_t clipped[9])
 907 {
 908   vec4_t buffer[9];
 909   unsigned int rcount = 3;
 910   unsigned int wcount = 0;
 911   vec_t const* rptr = clipped[0];
 912   vec_t* wptr = buffer[0];
 913   const vec_t* p0;
 914   const vec_t* p1;
 915   unsigned char b0, b1;
 916
 917   unsigned int i;
 918   double scale;
 919
 920   p0 = rptr;
 921   b0 = CLIP_X_LT_W(p0);
 922   for(i=0; i<rcount; ++i)
 923   {
 924     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
 925     b1 = CLIP_X_LT_W(p1);
 926     if(b0 ^ b1)
 927     {
 928                         wptr[0] = p1[0] - p0[0];
 929                         wptr[1] = p1[1] - p0[1];
 930                         wptr[2] = p1[2] - p0[2];
 931                         wptr[3] = p1[3] - p0[3];
 932
 933       scale = (p0[0] - p0[3]) / (wptr[3] - wptr[0]);
 934
 935                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
 936                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
 937                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
 938                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
 939
 940       wptr += 4;
 941       ++wcount;
 942     }
 943
 944     if(b1)
 945     {
 946       wptr[0] = p1[0];
 947       wptr[1] = p1[1];
 948       wptr[2] = p1[2];
 949       wptr[3] = p1[3];
 950
 951       wptr += 4;
 952       ++wcount;
 953     }
 954
 955     p0 = p1;
 956     b0 = b1;
 957   }
 958
 959   rcount = wcount;
 960   wcount = 0;
 961   rptr = buffer[0];
 962   wptr = clipped[0];
 963   p0 = rptr;
 964   b0 = CLIP_X_GT_W(p0);
 965
 966   for(i=0; i<rcount; ++i)
 967   {
 968     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
 969     b1 = CLIP_X_GT_W(p1);
 970     if(b0 ^ b1)
 971     {
 972                         wptr[0] = p1[0] - p0[0];
 973                         wptr[1] = p1[1] - p0[1];
 974                         wptr[2] = p1[2] - p0[2];
 975                         wptr[3] = p1[3] - p0[3];
 976
 977       scale = (p0[0] + p0[3]) / (-wptr[3] - wptr[0]);
 978
 979                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
 980                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
 981                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
 982                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
 983
 984       wptr += 4;
 985       ++wcount;
 986     }
 987
 988     if(b1)
 989     {
 990       wptr[0] = p1[0];
 991       wptr[1] = p1[1];
 992       wptr[2] = p1[2];
 993       wptr[3] = p1[3];
 994
 995       wptr += 4;
 996       ++wcount;
 997     }
 998
 999     p0 = p1;
1000     b0 = b1;
1001   }
1002
1003   rcount = wcount;
1004   wcount = 0;
1005   rptr = clipped[0];
1006   wptr = buffer[0];
1007   p0 = rptr;
1008   b0 = CLIP_Y_LT_W(p0);
1009
1010   for(i=0; i<rcount; ++i)
1011   {
1012     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1013     b1 = CLIP_Y_LT_W(p1);
1014     if(b0 ^ b1)
1015     {
1016                         wptr[0] = p1[0] - p0[0];
1017                         wptr[1] = p1[1] - p0[1];
1018                         wptr[2] = p1[2] - p0[2];
1019                         wptr[3] = p1[3] - p0[3];
1020
1021       scale = (p0[1] - p0[3]) / (wptr[3] - wptr[1]);
1022
1023                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1024                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1025                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1026                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1027
1028       wptr += 4;
1029       ++wcount;
1030     }
1031
1032     if(b1)
1033     {
1034       wptr[0] = p1[0];
1035       wptr[1] = p1[1];
1036       wptr[2] = p1[2];
1037       wptr[3] = p1[3];
1038
1039       wptr += 4;
1040       ++wcount;
1041     }
1042
1043     p0 = p1;
1044     b0 = b1;
1045   }
1046
1047   rcount = wcount;
1048   wcount = 0;
1049   rptr = buffer[0];
1050   wptr = clipped[0];
1051   p0 = rptr;
1052   b0 = CLIP_Y_GT_W(p0);
1053
1054   for(i=0; i<rcount; ++i)
1055   {
1056     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1057     b1 = CLIP_Y_GT_W(p1);
1058     if(b0 ^ b1)
1059     {
1060                         wptr[0] = p1[0] - p0[0];
1061                         wptr[1] = p1[1] - p0[1];
1062                         wptr[2] = p1[2] - p0[2];
1063                         wptr[3] = p1[3] - p0[3];
1064
1065       scale = (p0[1] + p0[3]) / (-wptr[3] - wptr[1]);
1066
1067                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1068                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1069                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1070                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1071
1072       wptr += 4;
1073       ++wcount;
1074     }
1075
1076     if(b1)
1077     {
1078       wptr[0] = p1[0];
1079       wptr[1] = p1[1];
1080       wptr[2] = p1[2];
1081       wptr[3] = p1[3];
1082
1083       wptr += 4;
1084       ++wcount;
1085     }
1086
1087     p0 = p1;
1088     b0 = b1;
1089   }
1090
1091   rcount = wcount;
1092   wcount = 0;
1093   rptr = clipped[0];
1094   wptr = buffer[0];
1095   p0 = rptr;
1096   b0 = CLIP_Z_LT_W(p0);
1097
1098   for(i=0; i<rcount; ++i)
1099   {
1100     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1101     b1 = CLIP_Z_LT_W(p1);
1102     if(b0 ^ b1)
1103     {
1104                         wptr[0] = p1[0] - p0[0];
1105                         wptr[1] = p1[1] - p0[1];
1106                         wptr[2] = p1[2] - p0[2];
1107                         wptr[3] = p1[3] - p0[3];
1108
1109       scale = (p0[2] - p0[3]) / (wptr[3] - wptr[2]);
1110
1111                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1112                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1113                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1114                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1115
1116       wptr += 4;
1117       ++wcount;
1118     }
1119
1120     if(b1)
1121     {
1122       wptr[0] = p1[0];
1123       wptr[1] = p1[1];
1124       wptr[2] = p1[2];
1125       wptr[3] = p1[3];
1126
1127       wptr += 4;
1128       ++wcount;
1129     }
1130
1131     p0 = p1;
1132     b0 = b1;
1133   }
1134
1135   rcount = wcount;
1136   wcount = 0;
1137   rptr = buffer[0];
1138   wptr = clipped[0];
1139   p0 = rptr;
1140   b0 = CLIP_Z_GT_W(p0);
1141
1142   for(i=0; i<rcount; ++i)
1143   {
1144     p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1145     b1 = CLIP_Z_GT_W(p1);
1146     if(b0 ^ b1)
1147     {
1148                         wptr[0] = p1[0] - p0[0];
1149                         wptr[1] = p1[1] - p0[1];
1150                         wptr[2] = p1[2] - p0[2];
1151                         wptr[3] = p1[3] - p0[3];
1152
1153       scale = (p0[2] + p0[3]) / (-wptr[3] - wptr[2]);
1154
1155                         wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1156                         wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1157                         wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1158                         wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1159
1160       wptr += 4;
1161       ++wcount;
1162     }
1163
1164     if(b1)
1165     {
1166       wptr[0] = p1[0];
1167       wptr[1] = p1[1];
1168       wptr[2] = p1[2];
1169       wptr[3] = p1[3];
1170
1171       wptr += 4;
1172       ++wcount;
1173     }
1174
1175     p0 = p1;
1176     b0 = b1;
1177   }
1178
1179   return wcount;
1180 }
1181
1182 unsigned int m4x4_clip_triangle(const m4x4_t matrix, const vec3_t p0, const vec3_t p1, const vec3_t p2, vec4_t clipped[9])
1183 {
1184   clipped[0][0] = p0[0];
1185   clipped[0][1] = p0[1];
1186   clipped[0][2] = p0[2];
1187   clipped[0][3] = 1;
1188   clipped[1][0] = p1[0];
1189   clipped[1][1] = p1[1];
1190   clipped[1][2] = p1[2];
1191   clipped[1][3] = 1;
1192   clipped[2][0] = p2[0];
1193   clipped[2][1] = p2[1];
1194   clipped[2][2] = p2[2];
1195   clipped[2][3] = 1;
1196
1197   m4x4_transform_vec4(matrix, clipped[0]);
1198   m4x4_transform_vec4(matrix, clipped[1]);
1199   m4x4_transform_vec4(matrix, clipped[2]);
1200
1201   return homogenous_clip_triangle(clipped);
1202 }
1203
1204 unsigned int homogenous_clip_line(vec4_t clipped[2])
1205 {
1206   vec4_t clip;
1207   double scale;
1208   const vec_t* const p0 = clipped[0];
1209   const vec_t* const p1 = clipped[1];
1210
1211   // early out
1212   {
1213     clipmask_t mask0 = homogenous_clip_point(clipped[0]);
1214     clipmask_t mask1 = homogenous_clip_point(clipped[1]);
1215
1216     if((mask0 | mask1) == CLIP_PASS) // both points passed all planes
1217       return 2;
1218
1219     if(mask0 & mask1) // both points failed any one plane
1220       return 0;
1221   }
1222
1223   {
1224     const unsigned int index = CLIP_X_LT_W(p0);
1225     if(index ^ CLIP_X_LT_W(p1))
1226     {
1227                   clip[0] = p1[0] - p0[0];
1228                   clip[1] = p1[1] - p0[1];
1229                   clip[2] = p1[2] - p0[2];
1230                   clip[3] = p1[3] - p0[3];
1231
1232       scale = (p0[0] - p0[3]) / (clip[3] - clip[0]);
1233
1234                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1235                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1236                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1237                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1238
1239       clipped[index][0] = clip[0];
1240       clipped[index][1] = clip[1];
1241       clipped[index][2] = clip[2];
1242       clipped[index][3] = clip[3];
1243     }
1244     else if(index == 0)
1245       return 0;
1246   }
1247
1248   {
1249     const unsigned int index = CLIP_X_GT_W(p0);
1250     if(index ^ CLIP_X_GT_W(p1))
1251     {
1252                   clip[0] = p1[0] - p0[0];
1253                   clip[1] = p1[1] - p0[1];
1254                   clip[2] = p1[2] - p0[2];
1255                   clip[3] = p1[3] - p0[3];
1256
1257       scale = (p0[0] + p0[3]) / (-clip[3] - clip[0]);
1258
1259                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1260                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1261                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1262                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1263
1264       clipped[index][0] = clip[0];
1265       clipped[index][1] = clip[1];
1266       clipped[index][2] = clip[2];
1267       clipped[index][3] = clip[3];
1268     }
1269     else if(index == 0)
1270       return 0;
1271   }
1272
1273   {
1274     const unsigned int index = CLIP_Y_LT_W(p0);
1275     if(index ^ CLIP_Y_LT_W(p1))
1276     {
1277                   clip[0] = p1[0] - p0[0];
1278                   clip[1] = p1[1] - p0[1];
1279                   clip[2] = p1[2] - p0[2];
1280                   clip[3] = p1[3] - p0[3];
1281
1282       scale = (p0[1] - p0[3]) / (clip[3] - clip[1]);
1283
1284                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1285                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1286                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1287                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1288
1289       clipped[index][0] = clip[0];
1290       clipped[index][1] = clip[1];
1291       clipped[index][2] = clip[2];
1292       clipped[index][3] = clip[3];
1293     }
1294     else if(index == 0)
1295       return 0;
1296   }
1297
1298   {
1299     const unsigned int index = CLIP_Y_GT_W(p0);
1300     if(index ^ CLIP_Y_GT_W(p1))
1301     {
1302                   clip[0] = p1[0] - p0[0];
1303                   clip[1] = p1[1] - p0[1];
1304                   clip[2] = p1[2] - p0[2];
1305                   clip[3] = p1[3] - p0[3];
1306
1307       scale = (p0[1] + p0[3]) / (-clip[3] - clip[1]);
1308
1309                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1310                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1311                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1312                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1313
1314       clipped[index][0] = clip[0];
1315       clipped[index][1] = clip[1];
1316       clipped[index][2] = clip[2];
1317       clipped[index][3] = clip[3];
1318     }
1319     else if(index == 0)
1320       return 0;
1321   }
1322
1323   {
1324     const unsigned int index = CLIP_Z_LT_W(p0);
1325     if(index ^ CLIP_Z_LT_W(p1))
1326     {
1327                   clip[0] = p1[0] - p0[0];
1328                   clip[1] = p1[1] - p0[1];
1329                   clip[2] = p1[2] - p0[2];
1330                   clip[3] = p1[3] - p0[3];
1331
1332       scale = (p0[2] - p0[3]) / (clip[3] - clip[2]);
1333
1334                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1335                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1336                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1337                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1338
1339       clipped[index][0] = clip[0];
1340       clipped[index][1] = clip[1];
1341       clipped[index][2] = clip[2];
1342       clipped[index][3] = clip[3];
1343     }
1344     else if(index == 0)
1345       return 0;
1346   }
1347
1348   {
1349     const unsigned int index = CLIP_Z_GT_W(p0);
1350     if(index ^ CLIP_Z_GT_W(p1))
1351     {
1352                   clip[0] = p1[0] - p0[0];
1353                   clip[1] = p1[1] - p0[1];
1354                   clip[2] = p1[2] - p0[2];
1355                   clip[3] = p1[3] - p0[3];
1356
1357       scale = (p0[2] + p0[3]) / (-clip[3] - clip[2]);
1358
1359                   clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1360                   clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1361                   clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1362                   clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1363
1364       clipped[index][0] = clip[0];
1365       clipped[index][1] = clip[1];
1366       clipped[index][2] = clip[2];
1367       clipped[index][3] = clip[3];
1368     }
1369     else if(index == 0)
1370       return 0;
1371   }
1372
1373   return 2;
1374 }
1375
1376 unsigned int m4x4_clip_line(const m4x4_t matrix, const vec3_t p0, const vec3_t p1, vec4_t clipped[2])
1377 {
1378   clipped[0][0] = p0[0];
1379   clipped[0][1] = p0[1];
1380   clipped[0][2] = p0[2];
1381   clipped[0][3] = 1;
1382   clipped[1][0] = p1[0];
1383   clipped[1][1] = p1[1];
1384   clipped[1][2] = p1[2];
1385   clipped[1][3] = 1;
1386
1387   m4x4_transform_vec4(matrix, clipped[0]);
1388   m4x4_transform_vec4(matrix, clipped[1]);
1389
1390   return homogenous_clip_line(clipped);
1391 }
1392
1393 void m4x4_transpose(m4x4_t matrix)
1394 {
1395         int i, j;
1396         float temp, *p1, *p2;
1397
1398   for (i=1; i<4; i++) {
1399     for (j=0; j<i; j++) {
1400       p1 = matrix+(j*4+i);
1401       p2 = matrix+(i*4+j);
1402       temp = *p1;
1403       *p1=*p2;
1404       *p2=temp;
1405     }
1406   }
1407 }
1408
1409 /* adapted from Graphics Gems 2
1410  invert a 3d matrix (4x3) */
1411 int m4x4_orthogonal_invert(m4x4_t matrix)
1412 {
1413   m4x4_t temp;
1414   vec_t* src = temp;
1415
1416   m4x4_assign(src, matrix);
1417
1418   /* Calculate the determinant of upper left 3x3 submatrix and
1419   * determine if the matrix is singular.
1420   */
1421   {
1422 #if 0
1423   float pos = 0.0f;
1424   float neg = 0.0f;
1425   float det = src[0] * src[5] * src[10];
1426   if (det >= 0.0) pos += det; else neg += det;
1427
1428   det = src[1] * src[6] * src[8];
1429   if (det >= 0.0) pos += det; else neg += det;
1430
1431   det = src[2] * src[4] * src[9];
1432   if (det >= 0.0) pos += det; else neg += det;
1433
1434   det = -src[2] * src[5] * src[8];
1435   if (det >= 0.0) pos += det; else neg += det;
1436
1437   det = -src[1] * src[4] * src[10];
1438   if (det >= 0.0) pos += det; else neg += det;
1439
1440   det = -src[0] * src[6] * src[9];
1441   if (det >= 0.0) pos += det; else neg += det;
1442
1443   det = pos + neg;
1444 #elif 0
1445   float det
1446     = (src[0] * src[5] * src[10])
1447     + (src[1] * src[6] * src[8])
1448     + (src[2] * src[4] * src[9])
1449     - (src[2] * src[5] * src[8])
1450     - (src[1] * src[4] * src[10])
1451     - (src[0] * src[6] * src[9]);
1452 #else
1453   float det
1454     = src[0] * ( src[5]*src[10] - src[9]*src[6] )
1455     - src[1] * ( src[4]*src[10] - src[8]*src[6] )
1456     + src[2] * ( src[4]*src[9] - src[8]*src[5] );
1457
1458 #endif
1459
1460   if (det*det < 1e-25)
1461     return 1;
1462
1463   det = 1.0f / det;
1464   matrix[0] = (  (src[5]*src[10]- src[6]*src[9] )*det);
1465   matrix[1] = (- (src[1]*src[10]- src[2]*src[9] )*det);
1466   matrix[2] = (  (src[1]*src[6] - src[2]*src[5] )*det);
1467   matrix[4] = (- (src[4]*src[10]- src[6]*src[8] )*det);
1468   matrix[5] = (  (src[0]*src[10]- src[2]*src[8] )*det);
1469   matrix[6] = (- (src[0]*src[6] - src[2]*src[4] )*det);
1470   matrix[8] = (  (src[4]*src[9] - src[5]*src[8] )*det);
1471   matrix[9] = (- (src[0]*src[9] - src[1]*src[8] )*det);
1472   matrix[10]= (  (src[0]*src[5] - src[1]*src[4] )*det);
1473   }
1474
1475   /* Do the translation part */
1476   matrix[12] = - (src[12] * matrix[0] +
1477     src[13] * matrix[4] +
1478     src[14] * matrix[8]);
1479   matrix[13] = - (src[12] * matrix[1] +
1480     src[13] * matrix[5] +
1481     src[14] * matrix[9]);
1482   matrix[14] = - (src[12] * matrix[2] +
1483     src[13] * matrix[6] +
1484     src[14] * matrix[10]);
1485
1486   return 0;
1487 }
1488
1489 void quat_identity(vec4_t quat)
1490 {
1491   quat[0] = quat[1] = quat[2] = 0;
1492   quat[3] = 1;
1493 }
1494
1495 void quat_multiply_by_quat(vec4_t quat, const vec4_t other)
1496 {
1497   const vec_t x = quat[3]*other[0] + quat[0]*other[3] + quat[1]*other[2] - quat[2]*other[1];
1498   const vec_t y = quat[3]*other[1] + quat[1]*other[3] + quat[2]*other[0] - quat[0]*other[2];
1499   const vec_t z = quat[3]*other[2] + quat[2]*other[3] + quat[0]*other[1] - quat[1]*other[0];
1500   const vec_t w = quat[3]*other[3] - quat[0]*other[0] - quat[1]*other[1] - quat[2]*other[2];
1501   quat[0] = x;
1502   quat[1] = y;
1503   quat[2] = z;
1504   quat[3] = w;
1505 }
1506
1507 void quat_conjugate(vec4_t quat)
1508 {
1509   VectorNegate(quat, quat);
1510 }
1511
1512 //! quaternion from two unit vectors
1513 void quat_for_unit_vectors(vec4_t quat, const vec3_t from, const vec3_t to)
1514 {
1515   CrossProduct(from, to, quat);
1516   quat[3] = DotProduct(from, to);
1517 }
1518
1519 void quat_normalise(vec4_t quat)
1520 {
1521   const vec_t n = 1 / ( quat[0] * quat[0] +  quat[1] * quat[1] +  quat[2] * quat[2] +  quat[3] *  quat[3] );
1522   quat[0] *= n;
1523   quat[1] *= n;
1524   quat[2] *= n;
1525   quat[3] *= n;
1526 }
1527
1528 void quat_for_axisangle(vec4_t quat, const vec3_t axis, double angle)
1529 {
1530   angle *= 0.5;
1531
1532   quat[3] = (float)sin(angle);
1533
1534   quat[0] = axis[0] * quat[3];
1535   quat[1] = axis[1] * quat[3];
1536   quat[2] = axis[2] * quat[3];
1537   quat[3] = (float)cos(angle);
1538 }
1539
1540 void m3x3_multiply_by_m3x3(m3x3_t matrix, const m3x3_t matrix_src)
1541 {
1542   float *pDest = matrix;
1543         float out1, out2, out3;
1544   int i;
1545
1546         for(i=0;i<3;i++)
1547         {
1548                 out1 =  matrix_src[0] * pDest[0];
1549                 out1 += matrix_src[1] * pDest[3];
1550                 out1 += matrix_src[2] * pDest[6];
1551                 out2 =  matrix_src[3] * pDest[0];
1552                 out2 += matrix_src[4] * pDest[3];
1553                 out2 += matrix_src[5] * pDest[6];
1554                 out3 =  matrix_src[6] * pDest[0];
1555                 out3 += matrix_src[7] * pDest[3];
1556                 out3 += matrix_src[8] * pDest[6];
1557
1558                 pDest[0] = out1;
1559                 pDest[3] = out2;
1560                 pDest[6] = out3;
1561
1562     pDest++;
1563         }
1564 }
1565
1566 void m3x3_transform_vec3(const m3x3_t matrix, vec3_t vector)
1567 {
1568   float out1, out2, out3;
1569
1570         out1 =  matrix[0]  * vector[0];
1571         out1 += matrix[3]  * vector[1];
1572         out1 += matrix[6]  * vector[2];
1573         out2 =  matrix[1]  * vector[0];
1574         out2 += matrix[4]  * vector[1];
1575         out2 += matrix[7]  * vector[2];
1576         out3 =  matrix[2]  * vector[0];
1577         out3 += matrix[5]  * vector[1];
1578         out3 += matrix[8] * vector[2];
1579
1580         vector[0] = out1;
1581         vector[1] = out2;
1582         vector[2] = out3;
1583 }
1584
1585 float m3_det( m3x3_t mat )
1586 {
1587   float det;
1588
1589   det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] )
1590     - mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] )
1591     + mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] );
1592
1593   return( det );
1594 }
1595
1596 int m3_inverse( m3x3_t mr, m3x3_t ma )
1597 {
1598   float det = m3_det( ma );
1599
1600   if (det == 0 )
1601   {
1602     return 1;
1603   }
1604
1605
1606   mr[0] =    ma[4]*ma[8] - ma[5]*ma[7]   / det;
1607   mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
1608   mr[2] =    ma[1]*ma[5] - ma[4]*ma[2]   / det;
1609
1610   mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
1611   mr[4] =    ma[0]*ma[8] - ma[6]*ma[2]   / det;
1612   mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
1613
1614   mr[6] =    ma[3]*ma[7] - ma[6]*ma[4]   / det;
1615   mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
1616   mr[8] =    ma[0]*ma[4] - ma[1]*ma[3]   / det;
1617
1618   return 0;
1619 }
1620
1621 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j )
1622 {
1623   int ti, tj, idst, jdst;
1624
1625   for ( ti = 0; ti < 4; ti++ )
1626   {
1627     if ( ti < i )
1628       idst = ti;
1629     else
1630       if ( ti > i )
1631         idst = ti-1;
1632
1633       for ( tj = 0; tj < 4; tj++ )
1634       {
1635         if ( tj < j )
1636           jdst = tj;
1637         else
1638           if ( tj > j )
1639             jdst = tj-1;
1640
1641           if ( ti != i && tj != j )
1642             mb[idst*3 + jdst] = mr[ti*4 + tj ];
1643       }
1644   }
1645 }
1646
1647 float m4_det( m4x4_t mr )
1648 {
1649   float  det, result = 0, i = 1;
1650   m3x3_t msub3;
1651   int     n;
1652
1653   for ( n = 0; n < 4; n++, i *= -1 )
1654   {
1655     m4_submat( mr, msub3, 0, n );
1656
1657     det     = m3_det( msub3 );
1658     result += mr[n] * det * i;
1659   }
1660
1661   return result;
1662 }
1663
1664 int m4x4_invert(m4x4_t matrix)
1665 {
1666   float  mdet = m4_det( matrix );
1667   m3x3_t mtemp;
1668   int     i, j, sign;
1669   m4x4_t m4x4_temp;
1670
1671 #if 0
1672   if ( fabs( mdet ) < 0.0000000001 )
1673     return 1;
1674 #endif
1675
1676   m4x4_assign(m4x4_temp, matrix);
1677
1678   for ( i = 0; i < 4; i++ )
1679     for ( j = 0; j < 4; j++ )
1680     {
1681       sign = 1 - ( (i +j) % 2 ) * 2;
1682
1683       m4_submat( m4x4_temp, mtemp, i, j );
1684
1685       matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet; /*  FIXME: try using * inverse det and see if speed/accuracy are good enough */
1686     }
1687
1688   return 0;
1689 }
1690 #if 0
1691 void m4x4_solve_ge(m4x4_t matrix, vec4_t x)
1692 {
1693   int indx[4];
1694   int c,r;
1695   int i;
1696   int best;
1697   float scale[4];
1698   float f, pivot;
1699   float aug[4];
1700   float recip, ratio;
1701   float* p;
1702
1703   for(r=0; r<4; r++)
1704   {
1705     aug[r] = 0;
1706     indx[r] = r;
1707   }
1708
1709   for (r=0; r<4; r++)
1710   {
1711     scale[r] = 0;
1712     for (c=0; c<4; c++, p++)
1713     {
1714       if (fabs(*p) > scale[r])
1715       {
1716         scale[r] = (float)fabs(*p);
1717       }
1718     }
1719   }
1720
1721   for (c=0; c<3; c++)
1722   {
1723     pivot = 0;
1724     for (r=c; r<4; r++)
1725     {
1726       f = (float)fabs(matrix[(indx[r]<<2)+c]) / scale[indx[r]];
1727       if (f > pivot)
1728       {
1729         pivot = f;
1730         best = r;
1731       }
1732     }
1733
1734     i = indx[c];
1735     indx[c] = indx[best];
1736     indx[best] = i;
1737
1738     recip = 1 / matrix[(indx[c]<<2)+c];
1739
1740     for (r=c+1; r<4; r++)
1741     {
1742       p = matrix + (indx[r]<<2);
1743       ratio = p[c] * recip;
1744
1745       for (i=c+1; i<4; i++)
1746         p[i] -= ratio * matrix[(indx[c]<<2)+i];
1747       aug[indx[r]] -= ratio * aug[indx[c]];
1748     }
1749   }
1750
1751   x[indx[3]] = aug[indx[3]] / matrix[(indx[3]<<2)+3];
1752   for(r=2; r>=0; r--)
1753   {
1754     f = aug[indx[r]];
1755     p = matrix + (indx[r]<<2);
1756     recip = 1 / p[r];
1757     for(c=(r+1); c<4; c++)
1758     {
1759       f -= (p[c] * x[indx[c]]);
1760     }
1761     x[indx[r]] = f * recip;
1762   }
1763 }
1764 #endif
1765
1766 #define N 3
1767
1768 int matrix_solve_ge(vec_t* matrix, vec_t* aug, vec3_t x)
1769 {
1770   int indx[N];
1771   int c,r;
1772   int i;
1773   int best;
1774   float scale[N];
1775   float f, pivot;
1776   float ratio;
1777   float* p;
1778
1779   for(r=0; r<N; r++)
1780   {
1781     indx[r] = r;
1782   }
1783
1784   for (r=0; r<N; r++)
1785   {
1786     p = matrix+r;
1787     scale[r] = 0;
1788     for (c=0; c<N; c++, p++)
1789     {
1790       if (fabs(*p) > scale[r])
1791       {
1792         scale[r] = (float)fabs(*p);
1793       }
1794     }
1795   }
1796
1797   for (c=0; c<N; c++)
1798   {
1799     pivot = 0;
1800     best = -1;
1801     for (r=c; r<N; r++)
1802     {
1803       f = (float)fabs(matrix[(indx[r]*N)+c]) / scale[indx[r]];
1804       if (f > pivot)
1805       {
1806         pivot = f;
1807         best = r;
1808       }
1809     }
1810
1811     if(best == -1) return 1;
1812
1813     i = indx[c];
1814     indx[c] = indx[best];
1815     indx[best] = i;
1816
1817     for (r=c+1; r<N; r++)
1818     {
1819       p = matrix + (indx[r]*N);
1820       ratio = p[c] / matrix[(indx[c]*N)+c];
1821
1822       for (i=c+1; i<N; i++) p[i] -= ratio * matrix[(indx[c]*N)+i];
1823       aug[indx[r]] -= ratio * aug[indx[c]];
1824     }
1825   }
1826
1827   x[N-1] = aug[indx[N-1]] / matrix[(indx[N-1]*N)+N-1];
1828   for(r=1; r>=0; r--)
1829   {
1830     f = aug[indx[r]];
1831     p = matrix + (indx[r]*N);
1832     for(c=(r+1); c<N; c++) f -= (p[c] * x[c]);
1833     x[r] = f / p[r];
1834   }
1835   return 0;
1836 }
1837
1838 #ifdef YOU_WANT_IT_TO_BORK
1839  /* Gaussian elimination */
1840   for(i=0;i<4;i++)
1841   {
1842     for(j=(i+1);j<4;j++)
1843     {
1844       ratio = matrix[j][i] / matrix[i][i];
1845       for(count=i;count<n;count++) {
1846         matrix[j][count] -= (ratio * matrix[i][count]);
1847       }
1848       b[j] -= (ratio * b[i]);
1849     }
1850   }
1851
1852   /* Back substitution */
1853   x[n-1] = b[n-1] / matrix[n-1][n-1];
1854   for(i=(n-2);i>=0;i--)
1855   {
1856     temp = b[i];
1857     for(j=(i+1);j<n;j++)
1858     {
1859       temp -= (matrix[i][j] * x[j]);
1860     }
1861     x[i] = temp / matrix[i][i];
1862   }
1863 #endif
1864
1865 int plane_intersect_planes(const vec4_t plane1, const vec4_t plane2, const vec4_t plane3, vec3_t intersection)
1866 {
1867   m3x3_t planes;
1868   vec3_t b;
1869   VectorCopy(plane1, planes+0);
1870   b[0] = plane1[3];
1871   VectorCopy(plane2, planes+3);
1872   b[1] = plane2[3];
1873   VectorCopy(plane3, planes+6);
1874   b[2] = plane3[3];
1875
1876   return matrix_solve_ge(planes, b, intersection);
1877 }