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