doom3-gpl
Doom 3 GPL source release
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Matrix.cpp
Go to the documentation of this file.
1 /*
2 ===========================================================================
3 
4 Doom 3 GPL Source Code
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
6 
7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
8 
9 Doom 3 Source Code is free software: you can redistribute it and/or modify
10 it under the terms of the GNU General Public License as published by
11 the Free Software Foundation, either version 3 of the License, or
12 (at your option) any later version.
13 
14 Doom 3 Source Code is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 GNU General Public License for more details.
18 
19 You should have received a copy of the GNU General Public License
20 along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
21 
22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
23 
24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
25 
26 ===========================================================================
27 */
28 
29 #include "../precompiled.h"
30 #pragma hdrstop
31 
32 
33 //===============================================================
34 //
35 // idMat2
36 //
37 //===============================================================
38 
39 idMat2 mat2_zero( idVec2( 0, 0 ), idVec2( 0, 0 ) );
40 idMat2 mat2_identity( idVec2( 1, 0 ), idVec2( 0, 1 ) );
41 
42 /*
43 ============
44 idMat2::InverseSelf
45 ============
46 */
47 bool idMat2::InverseSelf( void ) {
48  // 2+4 = 6 multiplications
49  // 1 division
50  double det, invDet, a;
51 
52  det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
53 
54  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
55  return false;
56  }
57 
58  invDet = 1.0f / det;
59 
60  a = mat[0][0];
61  mat[0][0] = mat[1][1] * invDet;
62  mat[0][1] = - mat[0][1] * invDet;
63  mat[1][0] = - mat[1][0] * invDet;
64  mat[1][1] = a * invDet;
65 
66  return true;
67 }
68 
69 /*
70 ============
71 idMat2::InverseFastSelf
72 ============
73 */
74 bool idMat2::InverseFastSelf( void ) {
75 #if 1
76  // 2+4 = 6 multiplications
77  // 1 division
78  double det, invDet, a;
79 
80  det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
81 
82  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
83  return false;
84  }
85 
86  invDet = 1.0f / det;
87 
88  a = mat[0][0];
89  mat[0][0] = mat[1][1] * invDet;
90  mat[0][1] = - mat[0][1] * invDet;
91  mat[1][0] = - mat[1][0] * invDet;
92  mat[1][1] = a * invDet;
93 
94  return true;
95 #else
96  // 2*4 = 8 multiplications
97  // 2 division
98  float *mat = reinterpret_cast<float *>(this);
99  double d, di;
100  float s;
101 
102  di = mat[0];
103  s = di;
104  mat[0*2+0] = d = 1.0f / di;
105  mat[0*2+1] *= d;
106  d = -d;
107  mat[1*2+0] *= d;
108  d = mat[1*2+0] * di;
109  mat[1*2+1] += mat[0*2+1] * d;
110  di = mat[1*2+1];
111  s *= di;
112  mat[1*2+1] = d = 1.0f / di;
113  mat[1*2+0] *= d;
114  d = -d;
115  mat[0*2+1] *= d;
116  d = mat[0*2+1] * di;
117  mat[0*2+0] += mat[1*2+0] * d;
118 
119  return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
120 #endif
121 }
122 
123 /*
124 =============
125 idMat2::ToString
126 =============
127 */
128 const char *idMat2::ToString( int precision ) const {
129  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
130 }
131 
132 
133 //===============================================================
134 //
135 // idMat3
136 //
137 //===============================================================
138 
139 idMat3 mat3_zero( idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ) );
140 idMat3 mat3_identity( idVec3( 1, 0, 0 ), idVec3( 0, 1, 0 ), idVec3( 0, 0, 1 ) );
141 
142 /*
143 ============
144 idMat3::ToAngles
145 ============
146 */
147 idAngles idMat3::ToAngles( void ) const {
148  idAngles angles;
149  double theta;
150  double cp;
151  float sp;
152 
153  sp = mat[ 0 ][ 2 ];
154 
155  // cap off our sin value so that we don't get any NANs
156  if ( sp > 1.0f ) {
157  sp = 1.0f;
158  } else if ( sp < -1.0f ) {
159  sp = -1.0f;
160  }
161 
162  theta = -asin( sp );
163  cp = cos( theta );
164 
165  if ( cp > 8192.0f * idMath::FLT_EPSILON ) {
166  angles.pitch = RAD2DEG( theta );
167  angles.yaw = RAD2DEG( atan2( mat[ 0 ][ 1 ], mat[ 0 ][ 0 ] ) );
168  angles.roll = RAD2DEG( atan2( mat[ 1 ][ 2 ], mat[ 2 ][ 2 ] ) );
169  } else {
170  angles.pitch = RAD2DEG( theta );
171  angles.yaw = RAD2DEG( -atan2( mat[ 1 ][ 0 ], mat[ 1 ][ 1 ] ) );
172  angles.roll = 0;
173  }
174  return angles;
175 }
176 
177 /*
178 ============
179 idMat3::ToQuat
180 ============
181 */
182 idQuat idMat3::ToQuat( void ) const {
183  idQuat q;
184  float trace;
185  float s;
186  float t;
187  int i;
188  int j;
189  int k;
190 
191  static int next[ 3 ] = { 1, 2, 0 };
192 
193  trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
194 
195  if ( trace > 0.0f ) {
196 
197  t = trace + 1.0f;
198  s = idMath::InvSqrt( t ) * 0.5f;
199 
200  q[3] = s * t;
201  q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
202  q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
203  q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
204 
205  } else {
206 
207  i = 0;
208  if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
209  i = 1;
210  }
211  if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
212  i = 2;
213  }
214  j = next[ i ];
215  k = next[ j ];
216 
217  t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
218  s = idMath::InvSqrt( t ) * 0.5f;
219 
220  q[i] = s * t;
221  q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
222  q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
223  q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
224  }
225  return q;
226 }
227 
228 /*
229 ============
230 idMat3::ToCQuat
231 ============
232 */
233 idCQuat idMat3::ToCQuat( void ) const {
234  idQuat q = ToQuat();
235  if ( q.w < 0.0f ) {
236  return idCQuat( -q.x, -q.y, -q.z );
237  }
238  return idCQuat( q.x, q.y, q.z );
239 }
240 
241 /*
242 ============
243 idMat3::ToRotation
244 ============
245 */
246 idRotation idMat3::ToRotation( void ) const {
247  idRotation r;
248  float trace;
249  float s;
250  float t;
251  int i;
252  int j;
253  int k;
254  static int next[ 3 ] = { 1, 2, 0 };
255 
256  trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
257  if ( trace > 0.0f ) {
258 
259  t = trace + 1.0f;
260  s = idMath::InvSqrt( t ) * 0.5f;
261 
262  r.angle = s * t;
263  r.vec[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
264  r.vec[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
265  r.vec[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
266 
267  } else {
268 
269  i = 0;
270  if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
271  i = 1;
272  }
273  if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
274  i = 2;
275  }
276  j = next[ i ];
277  k = next[ j ];
278 
279  t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
280  s = idMath::InvSqrt( t ) * 0.5f;
281 
282  r.vec[i] = s * t;
283  r.angle = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
284  r.vec[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
285  r.vec[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
286  }
287  r.angle = idMath::ACos( r.angle );
288  if ( idMath::Fabs( r.angle ) < 1e-10f ) {
289  r.vec.Set( 0.0f, 0.0f, 1.0f );
290  r.angle = 0.0f;
291  } else {
292  //vec *= (1.0f / sin( angle ));
293  r.vec.Normalize();
294  r.vec.FixDegenerateNormal();
295  r.angle *= 2.0f * idMath::M_RAD2DEG;
296  }
297 
298  r.origin.Zero();
299  r.axis = *this;
300  r.axisValid = true;
301  return r;
302 }
303 
304 /*
305 =================
306 idMat3::ToAngularVelocity
307 =================
308 */
309 idVec3 idMat3::ToAngularVelocity( void ) const {
310  idRotation rotation = ToRotation();
311  return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
312 }
313 
314 /*
315 ============
316 idMat3::Determinant
317 ============
318 */
319 float idMat3::Determinant( void ) const {
320 
321  float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
322  float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
323  float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
324 
325  return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01;
326 }
327 
328 /*
329 ============
330 idMat3::InverseSelf
331 ============
332 */
333 bool idMat3::InverseSelf( void ) {
334  // 18+3+9 = 30 multiplications
335  // 1 division
336  idMat3 inverse;
337  double det, invDet;
338 
339  inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
340  inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
341  inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
342 
343  det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
344 
345  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
346  return false;
347  }
348 
349  invDet = 1.0f / det;
350 
351  inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
352  inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
353  inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
354  inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
355  inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
356  inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
357 
358  mat[0][0] = inverse[0][0] * invDet;
359  mat[0][1] = inverse[0][1] * invDet;
360  mat[0][2] = inverse[0][2] * invDet;
361 
362  mat[1][0] = inverse[1][0] * invDet;
363  mat[1][1] = inverse[1][1] * invDet;
364  mat[1][2] = inverse[1][2] * invDet;
365 
366  mat[2][0] = inverse[2][0] * invDet;
367  mat[2][1] = inverse[2][1] * invDet;
368  mat[2][2] = inverse[2][2] * invDet;
369 
370  return true;
371 }
372 
373 /*
374 ============
375 idMat3::InverseFastSelf
376 ============
377 */
378 bool idMat3::InverseFastSelf( void ) {
379 #if 1
380  // 18+3+9 = 30 multiplications
381  // 1 division
382  idMat3 inverse;
383  double det, invDet;
384 
385  inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
386  inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
387  inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
388 
389  det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
390 
391  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
392  return false;
393  }
394 
395  invDet = 1.0f / det;
396 
397  inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
398  inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
399  inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
400  inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
401  inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
402  inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
403 
404  mat[0][0] = inverse[0][0] * invDet;
405  mat[0][1] = inverse[0][1] * invDet;
406  mat[0][2] = inverse[0][2] * invDet;
407 
408  mat[1][0] = inverse[1][0] * invDet;
409  mat[1][1] = inverse[1][1] * invDet;
410  mat[1][2] = inverse[1][2] * invDet;
411 
412  mat[2][0] = inverse[2][0] * invDet;
413  mat[2][1] = inverse[2][1] * invDet;
414  mat[2][2] = inverse[2][2] * invDet;
415 
416  return true;
417 #elif 0
418  // 3*10 = 30 multiplications
419  // 3 divisions
420  float *mat = reinterpret_cast<float *>(this);
421  float s;
422  double d, di;
423 
424  di = mat[0];
425  s = di;
426  mat[0] = d = 1.0f / di;
427  mat[1] *= d;
428  mat[2] *= d;
429  d = -d;
430  mat[3] *= d;
431  mat[6] *= d;
432  d = mat[3] * di;
433  mat[4] += mat[1] * d;
434  mat[5] += mat[2] * d;
435  d = mat[6] * di;
436  mat[7] += mat[1] * d;
437  mat[8] += mat[2] * d;
438  di = mat[4];
439  s *= di;
440  mat[4] = d = 1.0f / di;
441  mat[3] *= d;
442  mat[5] *= d;
443  d = -d;
444  mat[1] *= d;
445  mat[7] *= d;
446  d = mat[1] * di;
447  mat[0] += mat[3] * d;
448  mat[2] += mat[5] * d;
449  d = mat[7] * di;
450  mat[6] += mat[3] * d;
451  mat[8] += mat[5] * d;
452  di = mat[8];
453  s *= di;
454  mat[8] = d = 1.0f / di;
455  mat[6] *= d;
456  mat[7] *= d;
457  d = -d;
458  mat[2] *= d;
459  mat[5] *= d;
460  d = mat[2] * di;
461  mat[0] += mat[6] * d;
462  mat[1] += mat[7] * d;
463  d = mat[5] * di;
464  mat[3] += mat[6] * d;
465  mat[4] += mat[7] * d;
466 
467  return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
468 #else
469  // 4*2+4*4 = 24 multiplications
470  // 2*1 = 2 divisions
471  idMat2 r0;
472  float r1[2], r2[2], r3;
473  float det, invDet;
474  float *mat = reinterpret_cast<float *>(this);
475 
476  // r0 = m0.Inverse(); // 2x2
477  det = mat[0*3+0] * mat[1*3+1] - mat[0*3+1] * mat[1*3+0];
478 
479  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
480  return false;
481  }
482 
483  invDet = 1.0f / det;
484 
485  r0[0][0] = mat[1*3+1] * invDet;
486  r0[0][1] = - mat[0*3+1] * invDet;
487  r0[1][0] = - mat[1*3+0] * invDet;
488  r0[1][1] = mat[0*3+0] * invDet;
489 
490  // r1 = r0 * m1; // 2x1 = 2x2 * 2x1
491  r1[0] = r0[0][0] * mat[0*3+2] + r0[0][1] * mat[1*3+2];
492  r1[1] = r0[1][0] * mat[0*3+2] + r0[1][1] * mat[1*3+2];
493 
494  // r2 = m2 * r1; // 1x1 = 1x2 * 2x1
495  r2[0] = mat[2*3+0] * r1[0] + mat[2*3+1] * r1[1];
496 
497  // r3 = r2 - m3; // 1x1 = 1x1 - 1x1
498  r3 = r2[0] - mat[2*3+2];
499 
500  // r3.InverseSelf();
501  if ( idMath::Fabs( r3 ) < MATRIX_INVERSE_EPSILON ) {
502  return false;
503  }
504 
505  r3 = 1.0f / r3;
506 
507  // r2 = m2 * r0; // 1x2 = 1x2 * 2x2
508  r2[0] = mat[2*3+0] * r0[0][0] + mat[2*3+1] * r0[1][0];
509  r2[1] = mat[2*3+0] * r0[0][1] + mat[2*3+1] * r0[1][1];
510 
511  // m2 = r3 * r2; // 1x2 = 1x1 * 1x2
512  mat[2*3+0] = r3 * r2[0];
513  mat[2*3+1] = r3 * r2[1];
514 
515  // m0 = r0 - r1 * m2; // 2x2 - 2x1 * 1x2
516  mat[0*3+0] = r0[0][0] - r1[0] * mat[2*3+0];
517  mat[0*3+1] = r0[0][1] - r1[0] * mat[2*3+1];
518  mat[1*3+0] = r0[1][0] - r1[1] * mat[2*3+0];
519  mat[1*3+1] = r0[1][1] - r1[1] * mat[2*3+1];
520 
521  // m1 = r1 * r3; // 2x1 = 2x1 * 1x1
522  mat[0*3+2] = r1[0] * r3;
523  mat[1*3+2] = r1[1] * r3;
524 
525  // m3 = -r3;
526  mat[2*3+2] = -r3;
527 
528  return true;
529 #endif
530 }
531 
532 /*
533 ============
534 idMat3::InertiaTranslate
535 ============
536 */
537 idMat3 idMat3::InertiaTranslate( const float mass, const idVec3 &centerOfMass, const idVec3 &translation ) const {
538  idMat3 m;
539  idVec3 newCenter;
540 
541  newCenter = centerOfMass + translation;
542 
543  m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
544  - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
545  m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
546  - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
547  m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
548  - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
549 
550  m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
551  m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
552  m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
553 
554  return (*this) + m;
555 }
556 
557 /*
558 ============
559 idMat3::InertiaTranslateSelf
560 ============
561 */
562 idMat3 &idMat3::InertiaTranslateSelf( const float mass, const idVec3 &centerOfMass, const idVec3 &translation ) {
563  idMat3 m;
564  idVec3 newCenter;
565 
566  newCenter = centerOfMass + translation;
567 
568  m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
569  - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
570  m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
571  - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
572  m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
573  - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
574 
575  m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
576  m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
577  m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
578 
579  (*this) += m;
580 
581  return (*this);
582 }
583 
584 /*
585 ============
586 idMat3::InertiaRotate
587 ============
588 */
589 idMat3 idMat3::InertiaRotate( const idMat3 &rotation ) const {
590  // NOTE: the rotation matrix is stored column-major
591  return rotation.Transpose() * (*this) * rotation;
592 }
593 
594 /*
595 ============
596 idMat3::InertiaRotateSelf
597 ============
598 */
599 idMat3 &idMat3::InertiaRotateSelf( const idMat3 &rotation ) {
600  // NOTE: the rotation matrix is stored column-major
601  *this = rotation.Transpose() * (*this) * rotation;
602  return *this;
603 }
604 
605 /*
606 =============
607 idMat3::ToString
608 =============
609 */
610 const char *idMat3::ToString( int precision ) const {
611  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
612 }
613 
614 
615 //===============================================================
616 //
617 // idMat4
618 //
619 //===============================================================
620 
621 idMat4 mat4_zero( idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ) );
622 idMat4 mat4_identity( idVec4( 1, 0, 0, 0 ), idVec4( 0, 1, 0, 0 ), idVec4( 0, 0, 1, 0 ), idVec4( 0, 0, 0, 1 ) );
623 
624 /*
625 ============
626 idMat4::Transpose
627 ============
628 */
629 idMat4 idMat4::Transpose( void ) const {
630  idMat4 transpose;
631  int i, j;
632 
633  for( i = 0; i < 4; i++ ) {
634  for( j = 0; j < 4; j++ ) {
635  transpose[ i ][ j ] = mat[ j ][ i ];
636  }
637  }
638  return transpose;
639 }
640 
641 /*
642 ============
643 idMat4::TransposeSelf
644 ============
645 */
646 idMat4 &idMat4::TransposeSelf( void ) {
647  float temp;
648  int i, j;
649 
650  for( i = 0; i < 4; i++ ) {
651  for( j = i + 1; j < 4; j++ ) {
652  temp = mat[ i ][ j ];
653  mat[ i ][ j ] = mat[ j ][ i ];
654  mat[ j ][ i ] = temp;
655  }
656  }
657  return *this;
658 }
659 
660 /*
661 ============
662 idMat4::Determinant
663 ============
664 */
665 float idMat4::Determinant( void ) const {
666 
667  // 2x2 sub-determinants
668  float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
669  float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
670  float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
671  float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
672  float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
673  float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
674 
675  // 3x3 sub-determinants
676  float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
677  float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
678  float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
679  float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
680 
681  return ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
682 }
683 
684 /*
685 ============
686 idMat4::InverseSelf
687 ============
688 */
689 bool idMat4::InverseSelf( void ) {
690  // 84+4+16 = 104 multiplications
691  // 1 division
692  double det, invDet;
693 
694  // 2x2 sub-determinants required to calculate 4x4 determinant
695  float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
696  float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
697  float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
698  float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
699  float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
700  float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
701 
702  // 3x3 sub-determinants required to calculate 4x4 determinant
703  float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
704  float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
705  float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
706  float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
707 
708  det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
709 
710  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
711  return false;
712  }
713 
714  invDet = 1.0f / det;
715 
716  // remaining 2x2 sub-determinants
717  float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
718  float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
719  float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
720  float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
721  float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
722  float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
723 
724  float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
725  float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
726  float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
727  float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
728  float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
729  float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
730 
731  // remaining 3x3 sub-determinants
732  float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
733  float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
734  float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
735  float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
736 
737  float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
738  float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
739  float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
740  float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
741 
742  float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
743  float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
744  float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
745  float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
746 
747  mat[0][0] = - det3_213_123 * invDet;
748  mat[1][0] = + det3_213_023 * invDet;
749  mat[2][0] = - det3_213_013 * invDet;
750  mat[3][0] = + det3_213_012 * invDet;
751 
752  mat[0][1] = + det3_203_123 * invDet;
753  mat[1][1] = - det3_203_023 * invDet;
754  mat[2][1] = + det3_203_013 * invDet;
755  mat[3][1] = - det3_203_012 * invDet;
756 
757  mat[0][2] = + det3_301_123 * invDet;
758  mat[1][2] = - det3_301_023 * invDet;
759  mat[2][2] = + det3_301_013 * invDet;
760  mat[3][2] = - det3_301_012 * invDet;
761 
762  mat[0][3] = - det3_201_123 * invDet;
763  mat[1][3] = + det3_201_023 * invDet;
764  mat[2][3] = - det3_201_013 * invDet;
765  mat[3][3] = + det3_201_012 * invDet;
766 
767  return true;
768 }
769 
770 /*
771 ============
772 idMat4::InverseFastSelf
773 ============
774 */
775 bool idMat4::InverseFastSelf( void ) {
776 #if 0
777  // 84+4+16 = 104 multiplications
778  // 1 division
779  double det, invDet;
780 
781  // 2x2 sub-determinants required to calculate 4x4 determinant
782  float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
783  float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
784  float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
785  float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
786  float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
787  float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
788 
789  // 3x3 sub-determinants required to calculate 4x4 determinant
790  float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
791  float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
792  float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
793  float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
794 
795  det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
796 
797  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
798  return false;
799  }
800 
801  invDet = 1.0f / det;
802 
803  // remaining 2x2 sub-determinants
804  float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
805  float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
806  float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
807  float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
808  float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
809  float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
810 
811  float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
812  float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
813  float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
814  float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
815  float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
816  float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
817 
818  // remaining 3x3 sub-determinants
819  float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
820  float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
821  float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
822  float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
823 
824  float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
825  float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
826  float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
827  float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
828 
829  float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
830  float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
831  float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
832  float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
833 
834  mat[0][0] = - det3_213_123 * invDet;
835  mat[1][0] = + det3_213_023 * invDet;
836  mat[2][0] = - det3_213_013 * invDet;
837  mat[3][0] = + det3_213_012 * invDet;
838 
839  mat[0][1] = + det3_203_123 * invDet;
840  mat[1][1] = - det3_203_023 * invDet;
841  mat[2][1] = + det3_203_013 * invDet;
842  mat[3][1] = - det3_203_012 * invDet;
843 
844  mat[0][2] = + det3_301_123 * invDet;
845  mat[1][2] = - det3_301_023 * invDet;
846  mat[2][2] = + det3_301_013 * invDet;
847  mat[3][2] = - det3_301_012 * invDet;
848 
849  mat[0][3] = - det3_201_123 * invDet;
850  mat[1][3] = + det3_201_023 * invDet;
851  mat[2][3] = - det3_201_013 * invDet;
852  mat[3][3] = + det3_201_012 * invDet;
853 
854  return true;
855 #elif 0
856  // 4*18 = 72 multiplications
857  // 4 divisions
858  float *mat = reinterpret_cast<float *>(this);
859  float s;
860  double d, di;
861 
862  di = mat[0];
863  s = di;
864  mat[0] = d = 1.0f / di;
865  mat[1] *= d;
866  mat[2] *= d;
867  mat[3] *= d;
868  d = -d;
869  mat[4] *= d;
870  mat[8] *= d;
871  mat[12] *= d;
872  d = mat[4] * di;
873  mat[5] += mat[1] * d;
874  mat[6] += mat[2] * d;
875  mat[7] += mat[3] * d;
876  d = mat[8] * di;
877  mat[9] += mat[1] * d;
878  mat[10] += mat[2] * d;
879  mat[11] += mat[3] * d;
880  d = mat[12] * di;
881  mat[13] += mat[1] * d;
882  mat[14] += mat[2] * d;
883  mat[15] += mat[3] * d;
884  di = mat[5];
885  s *= di;
886  mat[5] = d = 1.0f / di;
887  mat[4] *= d;
888  mat[6] *= d;
889  mat[7] *= d;
890  d = -d;
891  mat[1] *= d;
892  mat[9] *= d;
893  mat[13] *= d;
894  d = mat[1] * di;
895  mat[0] += mat[4] * d;
896  mat[2] += mat[6] * d;
897  mat[3] += mat[7] * d;
898  d = mat[9] * di;
899  mat[8] += mat[4] * d;
900  mat[10] += mat[6] * d;
901  mat[11] += mat[7] * d;
902  d = mat[13] * di;
903  mat[12] += mat[4] * d;
904  mat[14] += mat[6] * d;
905  mat[15] += mat[7] * d;
906  di = mat[10];
907  s *= di;
908  mat[10] = d = 1.0f / di;
909  mat[8] *= d;
910  mat[9] *= d;
911  mat[11] *= d;
912  d = -d;
913  mat[2] *= d;
914  mat[6] *= d;
915  mat[14] *= d;
916  d = mat[2] * di;
917  mat[0] += mat[8] * d;
918  mat[1] += mat[9] * d;
919  mat[3] += mat[11] * d;
920  d = mat[6] * di;
921  mat[4] += mat[8] * d;
922  mat[5] += mat[9] * d;
923  mat[7] += mat[11] * d;
924  d = mat[14] * di;
925  mat[12] += mat[8] * d;
926  mat[13] += mat[9] * d;
927  mat[15] += mat[11] * d;
928  di = mat[15];
929  s *= di;
930  mat[15] = d = 1.0f / di;
931  mat[12] *= d;
932  mat[13] *= d;
933  mat[14] *= d;
934  d = -d;
935  mat[3] *= d;
936  mat[7] *= d;
937  mat[11] *= d;
938  d = mat[3] * di;
939  mat[0] += mat[12] * d;
940  mat[1] += mat[13] * d;
941  mat[2] += mat[14] * d;
942  d = mat[7] * di;
943  mat[4] += mat[12] * d;
944  mat[5] += mat[13] * d;
945  mat[6] += mat[14] * d;
946  d = mat[11] * di;
947  mat[8] += mat[12] * d;
948  mat[9] += mat[13] * d;
949  mat[10] += mat[14] * d;
950 
951  return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
952 #else
953  // 6*8+2*6 = 60 multiplications
954  // 2*1 = 2 divisions
955  idMat2 r0, r1, r2, r3;
956  float a, det, invDet;
957  float *mat = reinterpret_cast<float *>(this);
958 
959  // r0 = m0.Inverse();
960  det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0];
961 
962  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
963  return false;
964  }
965 
966  invDet = 1.0f / det;
967 
968  r0[0][0] = mat[1*4+1] * invDet;
969  r0[0][1] = - mat[0*4+1] * invDet;
970  r0[1][0] = - mat[1*4+0] * invDet;
971  r0[1][1] = mat[0*4+0] * invDet;
972 
973  // r1 = r0 * m1;
974  r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2];
975  r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3];
976  r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2];
977  r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3];
978 
979  // r2 = m2 * r1;
980  r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0];
981  r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1];
982  r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0];
983  r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1];
984 
985  // r3 = r2 - m3;
986  r3[0][0] = r2[0][0] - mat[2*4+2];
987  r3[0][1] = r2[0][1] - mat[2*4+3];
988  r3[1][0] = r2[1][0] - mat[3*4+2];
989  r3[1][1] = r2[1][1] - mat[3*4+3];
990 
991  // r3.InverseSelf();
992  det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
993 
994  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
995  return false;
996  }
997 
998  invDet = 1.0f / det;
999 
1000  a = r3[0][0];
1001  r3[0][0] = r3[1][1] * invDet;
1002  r3[0][1] = - r3[0][1] * invDet;
1003  r3[1][0] = - r3[1][0] * invDet;
1004  r3[1][1] = a * invDet;
1005 
1006  // r2 = m2 * r0;
1007  r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0];
1008  r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1];
1009  r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0];
1010  r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1];
1011 
1012  // m2 = r3 * r2;
1013  mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
1014  mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
1015  mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
1016  mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
1017 
1018  // m0 = r0 - r1 * m2;
1019  mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0];
1020  mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1];
1021  mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0];
1022  mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1];
1023 
1024  // m1 = r1 * r3;
1025  mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
1026  mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
1027  mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
1028  mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
1029 
1030  // m3 = -r3;
1031  mat[2*4+2] = -r3[0][0];
1032  mat[2*4+3] = -r3[0][1];
1033  mat[3*4+2] = -r3[1][0];
1034  mat[3*4+3] = -r3[1][1];
1035 
1036  return true;
1037 #endif
1038 }
1039 
1040 /*
1041 =============
1042 idMat4::ToString
1043 =============
1044 */
1045 const char *idMat4::ToString( int precision ) const {
1046  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
1047 }
1048 
1049 
1050 //===============================================================
1051 //
1052 // idMat5
1053 //
1054 //===============================================================
1055 
1056 idMat5 mat5_zero( idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ) );
1057 idMat5 mat5_identity( idVec5( 1, 0, 0, 0, 0 ), idVec5( 0, 1, 0, 0, 0 ), idVec5( 0, 0, 1, 0, 0 ), idVec5( 0, 0, 0, 1, 0 ), idVec5( 0, 0, 0, 0, 1 ) );
1058 
1059 /*
1060 ============
1061 idMat5::Transpose
1062 ============
1063 */
1064 idMat5 idMat5::Transpose( void ) const {
1065  idMat5 transpose;
1066  int i, j;
1067 
1068  for( i = 0; i < 5; i++ ) {
1069  for( j = 0; j < 5; j++ ) {
1070  transpose[ i ][ j ] = mat[ j ][ i ];
1071  }
1072  }
1073  return transpose;
1074 }
1075 
1076 /*
1077 ============
1078 idMat5::TransposeSelf
1079 ============
1080 */
1081 idMat5 &idMat5::TransposeSelf( void ) {
1082  float temp;
1083  int i, j;
1084 
1085  for( i = 0; i < 5; i++ ) {
1086  for( j = i + 1; j < 5; j++ ) {
1087  temp = mat[ i ][ j ];
1088  mat[ i ][ j ] = mat[ j ][ i ];
1089  mat[ j ][ i ] = temp;
1090  }
1091  }
1092  return *this;
1093 }
1094 
1095 /*
1096 ============
1097 idMat5::Determinant
1098 ============
1099 */
1100 float idMat5::Determinant( void ) const {
1101 
1102  // 2x2 sub-determinants required to calculate 5x5 determinant
1103  float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
1104  float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
1105  float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
1106  float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
1107  float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
1108  float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
1109  float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
1110  float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
1111  float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
1112  float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
1113 
1114  // 3x3 sub-determinants required to calculate 5x5 determinant
1115  float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
1116  float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
1117  float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
1118  float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
1119  float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
1120  float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
1121  float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
1122  float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
1123  float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
1124  float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
1125 
1126  // 4x4 sub-determinants required to calculate 5x5 determinant
1127  float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
1128  float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
1129  float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
1130  float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
1131  float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
1132 
1133  // determinant of 5x5 matrix
1134  return mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
1135 }
1136 
1137 /*
1138 ============
1139 idMat5::InverseSelf
1140 ============
1141 */
1142 bool idMat5::InverseSelf( void ) {
1143  // 280+5+25 = 310 multiplications
1144  // 1 division
1145  double det, invDet;
1146 
1147  // 2x2 sub-determinants required to calculate 5x5 determinant
1148  float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
1149  float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
1150  float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
1151  float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
1152  float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
1153  float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
1154  float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
1155  float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
1156  float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
1157  float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
1158 
1159  // 3x3 sub-determinants required to calculate 5x5 determinant
1160  float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
1161  float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
1162  float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
1163  float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
1164  float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
1165  float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
1166  float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
1167  float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
1168  float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
1169  float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
1170 
1171  // 4x4 sub-determinants required to calculate 5x5 determinant
1172  float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
1173  float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
1174  float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
1175  float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
1176  float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
1177 
1178  // determinant of 5x5 matrix
1179  det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
1180 
1181  if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
1182  return false;
1183  }
1184 
1185  invDet = 1.0f / det;
1186 
1187  // remaining 2x2 sub-determinants
1188  float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
1189  float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
1190  float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
1191  float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
1192  float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
1193  float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
1194  float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
1195  float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
1196  float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
1197  float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
1198  float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
1199  float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
1200  float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
1201  float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
1202  float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
1203  float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
1204  float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
1205  float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
1206  float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
1207  float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
1208 
1209  // remaining 3x3 sub-determinants
1210  float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
1211  float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
1212  float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
1213  float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
1214  float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
1215  float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
1216  float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
1217  float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
1218  float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
1219  float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
1220  float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
1221  float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
1222  float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
1223  float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
1224  float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
1225  float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
1226  float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
1227  float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
1228  float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
1229  float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
1230  float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
1231  float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
1232  float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
1233  float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
1234  float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
1235  float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
1236  float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
1237  float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
1238  float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
1239  float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
1240 
1241  // remaining 4x4 sub-determinants
1242  float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
1243  float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
1244  float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
1245  float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
1246  float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
1247  float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
1248  float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
1249  float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
1250  float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
1251  float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
1252  float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
1253  float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
1254  float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
1255  float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
1256  float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
1257  float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
1258  float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
1259  float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
1260  float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
1261  float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
1262 
1263  mat[0][0] = det4_1234_1234 * invDet;
1264  mat[0][1] = -det4_0234_1234 * invDet;
1265  mat[0][2] = det4_0134_1234 * invDet;
1266  mat[0][3] = -det4_0124_1234 * invDet;
1267  mat[0][4] = det4_0123_1234 * invDet;
1268 
1269  mat[1][0] = -det4_1234_0234 * invDet;
1270  mat[1][1] = det4_0234_0234 * invDet;
1271  mat[1][2] = -det4_0134_0234 * invDet;
1272  mat[1][3] = det4_0124_0234 * invDet;
1273  mat[1][4] = -det4_0123_0234 * invDet;
1274 
1275  mat[2][0] = det4_1234_0134 * invDet;
1276  mat[2][1] = -det4_0234_0134 * invDet;
1277  mat[2][2] = det4_0134_0134 * invDet;
1278  mat[2][3] = -det4_0124_0134 * invDet;
1279  mat[2][4] = det4_0123_0134 * invDet;
1280 
1281  mat[3][0] = -det4_1234_0124 * invDet;
1282  mat[3][1] = det4_0234_0124 * invDet;
1283  mat[3][2] = -det4_0134_0124 * invDet;
1284  mat[3][3] = det4_0124_0124 * invDet;
1285  mat[3][4] = -det4_0123_0124 * invDet;
1286 
1287  mat[4][0] = det4_1234_0123 * invDet;
1288  mat[4][1] = -det4_0234_0123 * invDet;
1289  mat[4][2] = det4_0134_0123 * invDet;
1290  mat[4][3] = -det4_0124_0123 * invDet;
1291  mat[4][4] = det4_0123_0123 * invDet;
1292 
1293  return true;
1294 }
1295 
1296 /*
1297 ============
1298 idMat5::InverseFastSelf
1299 ============
1300 */
1301 bool idMat5::InverseFastSelf( void ) {
1302 #if 0
1303  // 280+5+25 = 310 multiplications
1304  // 1 division
1305  double det, invDet;
1306 
1307  // 2x2 sub-determinants required to calculate 5x5 determinant
1308  float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
1309  float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
1310  float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
1311  float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
1312  float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
1313  float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
1314  float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
1315  float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
1316  float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
1317  float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
1318 
1319  // 3x3 sub-determinants required to calculate 5x5 determinant
1320  float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
1321  float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
1322  float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
1323  float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
1324  float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
1325  float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
1326  float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
1327  float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
1328  float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
1329  float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
1330 
1331  // 4x4 sub-determinants required to calculate 5x5 determinant
1332  float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
1333  float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
1334  float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
1335  float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
1336  float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
1337 
1338  // determinant of 5x5 matrix
1339  det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
1340 
1341  if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
1342  return false;
1343  }
1344 
1345  invDet = 1.0f / det;
1346 
1347  // remaining 2x2 sub-determinants
1348  float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
1349  float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
1350  float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
1351  float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
1352  float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
1353  float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
1354  float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
1355  float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
1356  float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
1357  float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
1358  float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
1359  float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
1360  float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
1361  float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
1362  float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
1363  float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
1364  float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
1365  float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
1366  float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
1367  float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
1368 
1369  // remaining 3x3 sub-determinants
1370  float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
1371  float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
1372  float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
1373  float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
1374  float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
1375  float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
1376  float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
1377  float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
1378  float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
1379  float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
1380  float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
1381  float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
1382  float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
1383  float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
1384  float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
1385  float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
1386  float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
1387  float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
1388  float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
1389  float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
1390  float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
1391  float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
1392  float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
1393  float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
1394  float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
1395  float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
1396  float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
1397  float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
1398  float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
1399  float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
1400 
1401  // remaining 4x4 sub-determinants
1402  float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
1403  float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
1404  float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
1405  float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
1406  float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
1407  float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
1408  float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
1409  float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
1410  float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
1411  float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
1412  float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
1413  float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
1414  float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
1415  float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
1416  float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
1417  float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
1418  float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
1419  float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
1420  float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
1421  float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
1422 
1423  mat[0][0] = det4_1234_1234 * invDet;
1424  mat[0][1] = -det4_0234_1234 * invDet;
1425  mat[0][2] = det4_0134_1234 * invDet;
1426  mat[0][3] = -det4_0124_1234 * invDet;
1427  mat[0][4] = det4_0123_1234 * invDet;
1428 
1429  mat[1][0] = -det4_1234_0234 * invDet;
1430  mat[1][1] = det4_0234_0234 * invDet;
1431  mat[1][2] = -det4_0134_0234 * invDet;
1432  mat[1][3] = det4_0124_0234 * invDet;
1433  mat[1][4] = -det4_0123_0234 * invDet;
1434 
1435  mat[2][0] = det4_1234_0134 * invDet;
1436  mat[2][1] = -det4_0234_0134 * invDet;
1437  mat[2][2] = det4_0134_0134 * invDet;
1438  mat[2][3] = -det4_0124_0134 * invDet;
1439  mat[2][4] = det4_0123_0134 * invDet;
1440 
1441  mat[3][0] = -det4_1234_0124 * invDet;
1442  mat[3][1] = det4_0234_0124 * invDet;
1443  mat[3][2] = -det4_0134_0124 * invDet;
1444  mat[3][3] = det4_0124_0124 * invDet;
1445  mat[3][4] = -det4_0123_0124 * invDet;
1446 
1447  mat[4][0] = det4_1234_0123 * invDet;
1448  mat[4][1] = -det4_0234_0123 * invDet;
1449  mat[4][2] = det4_0134_0123 * invDet;
1450  mat[4][3] = -det4_0124_0123 * invDet;
1451  mat[4][4] = det4_0123_0123 * invDet;
1452 
1453  return true;
1454 #elif 0
1455  // 5*28 = 140 multiplications
1456  // 5 divisions
1457  float *mat = reinterpret_cast<float *>(this);
1458  float s;
1459  double d, di;
1460 
1461  di = mat[0];
1462  s = di;
1463  mat[0] = d = 1.0f / di;
1464  mat[1] *= d;
1465  mat[2] *= d;
1466  mat[3] *= d;
1467  mat[4] *= d;
1468  d = -d;
1469  mat[5] *= d;
1470  mat[10] *= d;
1471  mat[15] *= d;
1472  mat[20] *= d;
1473  d = mat[5] * di;
1474  mat[6] += mat[1] * d;
1475  mat[7] += mat[2] * d;
1476  mat[8] += mat[3] * d;
1477  mat[9] += mat[4] * d;
1478  d = mat[10] * di;
1479  mat[11] += mat[1] * d;
1480  mat[12] += mat[2] * d;
1481  mat[13] += mat[3] * d;
1482  mat[14] += mat[4] * d;
1483  d = mat[15] * di;
1484  mat[16] += mat[1] * d;
1485  mat[17] += mat[2] * d;
1486  mat[18] += mat[3] * d;
1487  mat[19] += mat[4] * d;
1488  d = mat[20] * di;
1489  mat[21] += mat[1] * d;
1490  mat[22] += mat[2] * d;
1491  mat[23] += mat[3] * d;
1492  mat[24] += mat[4] * d;
1493  di = mat[6];
1494  s *= di;
1495  mat[6] = d = 1.0f / di;
1496  mat[5] *= d;
1497  mat[7] *= d;
1498  mat[8] *= d;
1499  mat[9] *= d;
1500  d = -d;
1501  mat[1] *= d;
1502  mat[11] *= d;
1503  mat[16] *= d;
1504  mat[21] *= d;
1505  d = mat[1] * di;
1506  mat[0] += mat[5] * d;
1507  mat[2] += mat[7] * d;
1508  mat[3] += mat[8] * d;
1509  mat[4] += mat[9] * d;
1510  d = mat[11] * di;
1511  mat[10] += mat[5] * d;
1512  mat[12] += mat[7] * d;
1513  mat[13] += mat[8] * d;
1514  mat[14] += mat[9] * d;
1515  d = mat[16] * di;
1516  mat[15] += mat[5] * d;
1517  mat[17] += mat[7] * d;
1518  mat[18] += mat[8] * d;
1519  mat[19] += mat[9] * d;
1520  d = mat[21] * di;
1521  mat[20] += mat[5] * d;
1522  mat[22] += mat[7] * d;
1523  mat[23] += mat[8] * d;
1524  mat[24] += mat[9] * d;
1525  di = mat[12];
1526  s *= di;
1527  mat[12] = d = 1.0f / di;
1528  mat[10] *= d;
1529  mat[11] *= d;
1530  mat[13] *= d;
1531  mat[14] *= d;
1532  d = -d;
1533  mat[2] *= d;
1534  mat[7] *= d;
1535  mat[17] *= d;
1536  mat[22] *= d;
1537  d = mat[2] * di;
1538  mat[0] += mat[10] * d;
1539  mat[1] += mat[11] * d;
1540  mat[3] += mat[13] * d;
1541  mat[4] += mat[14] * d;
1542  d = mat[7] * di;
1543  mat[5] += mat[10] * d;
1544  mat[6] += mat[11] * d;
1545  mat[8] += mat[13] * d;
1546  mat[9] += mat[14] * d;
1547  d = mat[17] * di;
1548  mat[15] += mat[10] * d;
1549  mat[16] += mat[11] * d;
1550  mat[18] += mat[13] * d;
1551  mat[19] += mat[14] * d;
1552  d = mat[22] * di;
1553  mat[20] += mat[10] * d;
1554  mat[21] += mat[11] * d;
1555  mat[23] += mat[13] * d;
1556  mat[24] += mat[14] * d;
1557  di = mat[18];
1558  s *= di;
1559  mat[18] = d = 1.0f / di;
1560  mat[15] *= d;
1561  mat[16] *= d;
1562  mat[17] *= d;
1563  mat[19] *= d;
1564  d = -d;
1565  mat[3] *= d;
1566  mat[8] *= d;
1567  mat[13] *= d;
1568  mat[23] *= d;
1569  d = mat[3] * di;
1570  mat[0] += mat[15] * d;
1571  mat[1] += mat[16] * d;
1572  mat[2] += mat[17] * d;
1573  mat[4] += mat[19] * d;
1574  d = mat[8] * di;
1575  mat[5] += mat[15] * d;
1576  mat[6] += mat[16] * d;
1577  mat[7] += mat[17] * d;
1578  mat[9] += mat[19] * d;
1579  d = mat[13] * di;
1580  mat[10] += mat[15] * d;
1581  mat[11] += mat[16] * d;
1582  mat[12] += mat[17] * d;
1583  mat[14] += mat[19] * d;
1584  d = mat[23] * di;
1585  mat[20] += mat[15] * d;
1586  mat[21] += mat[16] * d;
1587  mat[22] += mat[17] * d;
1588  mat[24] += mat[19] * d;
1589  di = mat[24];
1590  s *= di;
1591  mat[24] = d = 1.0f / di;
1592  mat[20] *= d;
1593  mat[21] *= d;
1594  mat[22] *= d;
1595  mat[23] *= d;
1596  d = -d;
1597  mat[4] *= d;
1598  mat[9] *= d;
1599  mat[14] *= d;
1600  mat[19] *= d;
1601  d = mat[4] * di;
1602  mat[0] += mat[20] * d;
1603  mat[1] += mat[21] * d;
1604  mat[2] += mat[22] * d;
1605  mat[3] += mat[23] * d;
1606  d = mat[9] * di;
1607  mat[5] += mat[20] * d;
1608  mat[6] += mat[21] * d;
1609  mat[7] += mat[22] * d;
1610  mat[8] += mat[23] * d;
1611  d = mat[14] * di;
1612  mat[10] += mat[20] * d;
1613  mat[11] += mat[21] * d;
1614  mat[12] += mat[22] * d;
1615  mat[13] += mat[23] * d;
1616  d = mat[19] * di;
1617  mat[15] += mat[20] * d;
1618  mat[16] += mat[21] * d;
1619  mat[17] += mat[22] * d;
1620  mat[18] += mat[23] * d;
1621 
1622  return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
1623 #else
1624  // 86+30+6 = 122 multiplications
1625  // 2*1 = 2 divisions
1626  idMat3 r0, r1, r2, r3;
1627  float c0, c1, c2, det, invDet;
1628  float *mat = reinterpret_cast<float *>(this);
1629 
1630  // r0 = m0.Inverse(); // 3x3
1631  c0 = mat[1*5+1] * mat[2*5+2] - mat[1*5+2] * mat[2*5+1];
1632  c1 = mat[1*5+2] * mat[2*5+0] - mat[1*5+0] * mat[2*5+2];
1633  c2 = mat[1*5+0] * mat[2*5+1] - mat[1*5+1] * mat[2*5+0];
1634 
1635  det = mat[0*5+0] * c0 + mat[0*5+1] * c1 + mat[0*5+2] * c2;
1636 
1637  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
1638  return false;
1639  }
1640 
1641  invDet = 1.0f / det;
1642 
1643  r0[0][0] = c0 * invDet;
1644  r0[0][1] = ( mat[0*5+2] * mat[2*5+1] - mat[0*5+1] * mat[2*5+2] ) * invDet;
1645  r0[0][2] = ( mat[0*5+1] * mat[1*5+2] - mat[0*5+2] * mat[1*5+1] ) * invDet;
1646  r0[1][0] = c1 * invDet;
1647  r0[1][1] = ( mat[0*5+0] * mat[2*5+2] - mat[0*5+2] * mat[2*5+0] ) * invDet;
1648  r0[1][2] = ( mat[0*5+2] * mat[1*5+0] - mat[0*5+0] * mat[1*5+2] ) * invDet;
1649  r0[2][0] = c2 * invDet;
1650  r0[2][1] = ( mat[0*5+1] * mat[2*5+0] - mat[0*5+0] * mat[2*5+1] ) * invDet;
1651  r0[2][2] = ( mat[0*5+0] * mat[1*5+1] - mat[0*5+1] * mat[1*5+0] ) * invDet;
1652 
1653  // r1 = r0 * m1; // 3x2 = 3x3 * 3x2
1654  r1[0][0] = r0[0][0] * mat[0*5+3] + r0[0][1] * mat[1*5+3] + r0[0][2] * mat[2*5+3];
1655  r1[0][1] = r0[0][0] * mat[0*5+4] + r0[0][1] * mat[1*5+4] + r0[0][2] * mat[2*5+4];
1656  r1[1][0] = r0[1][0] * mat[0*5+3] + r0[1][1] * mat[1*5+3] + r0[1][2] * mat[2*5+3];
1657  r1[1][1] = r0[1][0] * mat[0*5+4] + r0[1][1] * mat[1*5+4] + r0[1][2] * mat[2*5+4];
1658  r1[2][0] = r0[2][0] * mat[0*5+3] + r0[2][1] * mat[1*5+3] + r0[2][2] * mat[2*5+3];
1659  r1[2][1] = r0[2][0] * mat[0*5+4] + r0[2][1] * mat[1*5+4] + r0[2][2] * mat[2*5+4];
1660 
1661  // r2 = m2 * r1; // 2x2 = 2x3 * 3x2
1662  r2[0][0] = mat[3*5+0] * r1[0][0] + mat[3*5+1] * r1[1][0] + mat[3*5+2] * r1[2][0];
1663  r2[0][1] = mat[3*5+0] * r1[0][1] + mat[3*5+1] * r1[1][1] + mat[3*5+2] * r1[2][1];
1664  r2[1][0] = mat[4*5+0] * r1[0][0] + mat[4*5+1] * r1[1][0] + mat[4*5+2] * r1[2][0];
1665  r2[1][1] = mat[4*5+0] * r1[0][1] + mat[4*5+1] * r1[1][1] + mat[4*5+2] * r1[2][1];
1666 
1667  // r3 = r2 - m3; // 2x2 = 2x2 - 2x2
1668  r3[0][0] = r2[0][0] - mat[3*5+3];
1669  r3[0][1] = r2[0][1] - mat[3*5+4];
1670  r3[1][0] = r2[1][0] - mat[4*5+3];
1671  r3[1][1] = r2[1][1] - mat[4*5+4];
1672 
1673  // r3.InverseSelf(); // 2x2
1674  det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
1675 
1676  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
1677  return false;
1678  }
1679 
1680  invDet = 1.0f / det;
1681 
1682  c0 = r3[0][0];
1683  r3[0][0] = r3[1][1] * invDet;
1684  r3[0][1] = - r3[0][1] * invDet;
1685  r3[1][0] = - r3[1][0] * invDet;
1686  r3[1][1] = c0 * invDet;
1687 
1688  // r2 = m2 * r0; // 2x3 = 2x3 * 3x3
1689  r2[0][0] = mat[3*5+0] * r0[0][0] + mat[3*5+1] * r0[1][0] + mat[3*5+2] * r0[2][0];
1690  r2[0][1] = mat[3*5+0] * r0[0][1] + mat[3*5+1] * r0[1][1] + mat[3*5+2] * r0[2][1];
1691  r2[0][2] = mat[3*5+0] * r0[0][2] + mat[3*5+1] * r0[1][2] + mat[3*5+2] * r0[2][2];
1692  r2[1][0] = mat[4*5+0] * r0[0][0] + mat[4*5+1] * r0[1][0] + mat[4*5+2] * r0[2][0];
1693  r2[1][1] = mat[4*5+0] * r0[0][1] + mat[4*5+1] * r0[1][1] + mat[4*5+2] * r0[2][1];
1694  r2[1][2] = mat[4*5+0] * r0[0][2] + mat[4*5+1] * r0[1][2] + mat[4*5+2] * r0[2][2];
1695 
1696  // m2 = r3 * r2; // 2x3 = 2x2 * 2x3
1697  mat[3*5+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
1698  mat[3*5+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
1699  mat[3*5+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2];
1700  mat[4*5+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
1701  mat[4*5+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
1702  mat[4*5+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2];
1703 
1704  // m0 = r0 - r1 * m2; // 3x3 = 3x3 - 3x2 * 2x3
1705  mat[0*5+0] = r0[0][0] - r1[0][0] * mat[3*5+0] - r1[0][1] * mat[4*5+0];
1706  mat[0*5+1] = r0[0][1] - r1[0][0] * mat[3*5+1] - r1[0][1] * mat[4*5+1];
1707  mat[0*5+2] = r0[0][2] - r1[0][0] * mat[3*5+2] - r1[0][1] * mat[4*5+2];
1708  mat[1*5+0] = r0[1][0] - r1[1][0] * mat[3*5+0] - r1[1][1] * mat[4*5+0];
1709  mat[1*5+1] = r0[1][1] - r1[1][0] * mat[3*5+1] - r1[1][1] * mat[4*5+1];
1710  mat[1*5+2] = r0[1][2] - r1[1][0] * mat[3*5+2] - r1[1][1] * mat[4*5+2];
1711  mat[2*5+0] = r0[2][0] - r1[2][0] * mat[3*5+0] - r1[2][1] * mat[4*5+0];
1712  mat[2*5+1] = r0[2][1] - r1[2][0] * mat[3*5+1] - r1[2][1] * mat[4*5+1];
1713  mat[2*5+2] = r0[2][2] - r1[2][0] * mat[3*5+2] - r1[2][1] * mat[4*5+2];
1714 
1715  // m1 = r1 * r3; // 3x2 = 3x2 * 2x2
1716  mat[0*5+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
1717  mat[0*5+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
1718  mat[1*5+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
1719  mat[1*5+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
1720  mat[2*5+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0];
1721  mat[2*5+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1];
1722 
1723  // m3 = -r3; // 2x2 = - 2x2
1724  mat[3*5+3] = -r3[0][0];
1725  mat[3*5+4] = -r3[0][1];
1726  mat[4*5+3] = -r3[1][0];
1727  mat[4*5+4] = -r3[1][1];
1728 
1729  return true;
1730 #endif
1731 }
1732 
1733 /*
1734 =============
1735 idMat5::ToString
1736 =============
1737 */
1738 const char *idMat5::ToString( int precision ) const {
1739  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
1740 }
1741 
1742 
1743 //===============================================================
1744 //
1745 // idMat6
1746 //
1747 //===============================================================
1748 
1749 idMat6 mat6_zero( idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ) );
1750 idMat6 mat6_identity( idVec6( 1, 0, 0, 0, 0, 0 ), idVec6( 0, 1, 0, 0, 0, 0 ), idVec6( 0, 0, 1, 0, 0, 0 ), idVec6( 0, 0, 0, 1, 0, 0 ), idVec6( 0, 0, 0, 0, 1, 0 ), idVec6( 0, 0, 0, 0, 0, 1 ) );
1751 
1752 /*
1753 ============
1754 idMat6::Transpose
1755 ============
1756 */
1757 idMat6 idMat6::Transpose( void ) const {
1758  idMat6 transpose;
1759  int i, j;
1760 
1761  for( i = 0; i < 6; i++ ) {
1762  for( j = 0; j < 6; j++ ) {
1763  transpose[ i ][ j ] = mat[ j ][ i ];
1764  }
1765  }
1766  return transpose;
1767 }
1768 
1769 /*
1770 ============
1771 idMat6::TransposeSelf
1772 ============
1773 */
1774 idMat6 &idMat6::TransposeSelf( void ) {
1775  float temp;
1776  int i, j;
1777 
1778  for( i = 0; i < 6; i++ ) {
1779  for( j = i + 1; j < 6; j++ ) {
1780  temp = mat[ i ][ j ];
1781  mat[ i ][ j ] = mat[ j ][ i ];
1782  mat[ j ][ i ] = temp;
1783  }
1784  }
1785  return *this;
1786 }
1787 
1788 /*
1789 ============
1790 idMat6::Determinant
1791 ============
1792 */
1793 float idMat6::Determinant( void ) const {
1794 
1795  // 2x2 sub-determinants required to calculate 6x6 determinant
1796  float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
1797  float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
1798  float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
1799  float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
1800  float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
1801  float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
1802  float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
1803  float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
1804  float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
1805  float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
1806  float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
1807  float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
1808  float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
1809  float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
1810  float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
1811 
1812  // 3x3 sub-determinants required to calculate 6x6 determinant
1813  float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
1814  float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
1815  float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
1816  float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
1817  float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
1818  float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
1819  float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
1820  float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
1821  float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
1822  float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
1823  float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
1824  float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
1825  float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
1826  float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
1827  float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
1828  float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
1829  float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
1830  float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
1831  float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
1832  float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
1833 
1834  // 4x4 sub-determinants required to calculate 6x6 determinant
1835  float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
1836  float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
1837  float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
1838  float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
1839  float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
1840  float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
1841  float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
1842  float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
1843  float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
1844  float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
1845  float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
1846  float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
1847  float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
1848  float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
1849  float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
1850 
1851  // 5x5 sub-determinants required to calculate 6x6 determinant
1852  float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
1853  float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
1854  float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
1855  float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
1856  float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
1857  float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
1858 
1859  // determinant of 6x6 matrix
1860  return mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
1861  mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
1862 }
1863 
1864 /*
1865 ============
1866 idMat6::InverseSelf
1867 ============
1868 */
1869 bool idMat6::InverseSelf( void ) {
1870  // 810+6+36 = 852 multiplications
1871  // 1 division
1872  double det, invDet;
1873 
1874  // 2x2 sub-determinants required to calculate 6x6 determinant
1875  float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
1876  float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
1877  float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
1878  float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
1879  float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
1880  float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
1881  float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
1882  float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
1883  float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
1884  float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
1885  float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
1886  float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
1887  float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
1888  float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
1889  float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
1890 
1891  // 3x3 sub-determinants required to calculate 6x6 determinant
1892  float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
1893  float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
1894  float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
1895  float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
1896  float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
1897  float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
1898  float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
1899  float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
1900  float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
1901  float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
1902  float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
1903  float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
1904  float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
1905  float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
1906  float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
1907  float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
1908  float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
1909  float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
1910  float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
1911  float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
1912 
1913  // 4x4 sub-determinants required to calculate 6x6 determinant
1914  float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
1915  float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
1916  float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
1917  float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
1918  float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
1919  float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
1920  float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
1921  float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
1922  float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
1923  float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
1924  float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
1925  float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
1926  float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
1927  float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
1928  float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
1929 
1930  // 5x5 sub-determinants required to calculate 6x6 determinant
1931  float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
1932  float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
1933  float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
1934  float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
1935  float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
1936  float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
1937 
1938  // determinant of 6x6 matrix
1939  det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
1940  mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
1941 
1942  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
1943  return false;
1944  }
1945 
1946  invDet = 1.0f / det;
1947 
1948  // remaining 2x2 sub-determinants
1949  float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
1950  float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
1951  float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
1952  float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
1953  float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
1954  float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
1955  float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
1956  float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
1957  float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
1958  float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
1959  float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
1960  float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
1961  float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
1962  float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
1963  float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
1964  float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
1965  float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
1966  float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
1967  float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
1968  float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
1969  float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
1970  float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
1971  float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
1972  float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
1973  float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
1974  float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
1975  float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
1976  float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
1977  float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
1978  float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
1979 
1980  // remaining 3x3 sub-determinants
1981  float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
1982  float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
1983  float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
1984  float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
1985  float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
1986  float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
1987  float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
1988  float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
1989  float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
1990  float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
1991  float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
1992  float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
1993  float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
1994  float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
1995  float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
1996  float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
1997  float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
1998  float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
1999  float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
2000  float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
2001  float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
2002  float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
2003  float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
2004  float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
2005  float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
2006  float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
2007  float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
2008  float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
2009  float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
2010  float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
2011  float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
2012  float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
2013  float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
2014  float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
2015  float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
2016  float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
2017  float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
2018  float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
2019  float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
2020  float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
2021  float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
2022  float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
2023  float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
2024  float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
2025  float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
2026  float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
2027  float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
2028  float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
2029  float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
2030  float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
2031  float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
2032  float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
2033  float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
2034  float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
2035  float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
2036  float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
2037  float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
2038  float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
2039  float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
2040  float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
2041 
2042  // remaining 4x4 sub-determinants
2043  float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
2044  float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
2045  float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
2046  float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
2047  float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
2048  float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
2049  float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
2050  float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
2051  float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
2052  float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
2053  float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
2054  float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
2055  float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
2056  float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
2057  float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
2058  float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
2059  float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
2060  float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
2061  float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
2062  float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
2063  float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
2064  float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
2065  float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
2066  float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
2067  float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
2068  float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
2069  float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
2070  float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
2071  float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
2072  float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
2073  float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
2074  float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
2075  float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
2076  float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
2077  float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
2078  float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
2079  float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
2080  float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
2081  float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
2082  float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
2083  float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
2084  float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
2085  float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
2086  float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
2087  float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
2088  float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
2089  float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
2090  float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
2091  float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
2092  float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
2093  float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
2094  float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
2095  float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
2096  float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
2097  float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
2098  float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
2099  float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
2100  float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
2101  float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
2102  float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
2103 
2104  // remaining 5x5 sub-determinants
2105  float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
2106  float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
2107  float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
2108  float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
2109  float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
2110  float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
2111  float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
2112  float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
2113  float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
2114  float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
2115  float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
2116  float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
2117  float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
2118  float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
2119  float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
2120  float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
2121  float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
2122  float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
2123  float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
2124  float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
2125  float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
2126  float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
2127  float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
2128  float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
2129  float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
2130  float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
2131  float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
2132  float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
2133  float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
2134  float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
2135 
2136  mat[0][0] = det5_12345_12345 * invDet;
2137  mat[0][1] = -det5_02345_12345 * invDet;
2138  mat[0][2] = det5_01345_12345 * invDet;
2139  mat[0][3] = -det5_01245_12345 * invDet;
2140  mat[0][4] = det5_01235_12345 * invDet;
2141  mat[0][5] = -det5_01234_12345 * invDet;
2142 
2143  mat[1][0] = -det5_12345_02345 * invDet;
2144  mat[1][1] = det5_02345_02345 * invDet;
2145  mat[1][2] = -det5_01345_02345 * invDet;
2146  mat[1][3] = det5_01245_02345 * invDet;
2147  mat[1][4] = -det5_01235_02345 * invDet;
2148  mat[1][5] = det5_01234_02345 * invDet;
2149 
2150  mat[2][0] = det5_12345_01345 * invDet;
2151  mat[2][1] = -det5_02345_01345 * invDet;
2152  mat[2][2] = det5_01345_01345 * invDet;
2153  mat[2][3] = -det5_01245_01345 * invDet;
2154  mat[2][4] = det5_01235_01345 * invDet;
2155  mat[2][5] = -det5_01234_01345 * invDet;
2156 
2157  mat[3][0] = -det5_12345_01245 * invDet;
2158  mat[3][1] = det5_02345_01245 * invDet;
2159  mat[3][2] = -det5_01345_01245 * invDet;
2160  mat[3][3] = det5_01245_01245 * invDet;
2161  mat[3][4] = -det5_01235_01245 * invDet;
2162  mat[3][5] = det5_01234_01245 * invDet;
2163 
2164  mat[4][0] = det5_12345_01235 * invDet;
2165  mat[4][1] = -det5_02345_01235 * invDet;
2166  mat[4][2] = det5_01345_01235 * invDet;
2167  mat[4][3] = -det5_01245_01235 * invDet;
2168  mat[4][4] = det5_01235_01235 * invDet;
2169  mat[4][5] = -det5_01234_01235 * invDet;
2170 
2171  mat[5][0] = -det5_12345_01234 * invDet;
2172  mat[5][1] = det5_02345_01234 * invDet;
2173  mat[5][2] = -det5_01345_01234 * invDet;
2174  mat[5][3] = det5_01245_01234 * invDet;
2175  mat[5][4] = -det5_01235_01234 * invDet;
2176  mat[5][5] = det5_01234_01234 * invDet;
2177 
2178  return true;
2179 }
2180 
2181 /*
2182 ============
2183 idMat6::InverseFastSelf
2184 ============
2185 */
2186 bool idMat6::InverseFastSelf( void ) {
2187 #if 0
2188  // 810+6+36 = 852 multiplications
2189  // 1 division
2190  double det, invDet;
2191 
2192  // 2x2 sub-determinants required to calculate 6x6 determinant
2193  float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
2194  float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
2195  float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
2196  float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
2197  float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
2198  float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
2199  float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
2200  float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
2201  float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
2202  float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
2203  float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
2204  float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
2205  float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
2206  float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
2207  float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
2208 
2209  // 3x3 sub-determinants required to calculate 6x6 determinant
2210  float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
2211  float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
2212  float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
2213  float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
2214  float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
2215  float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
2216  float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
2217  float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
2218  float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
2219  float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
2220  float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
2221  float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
2222  float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
2223  float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
2224  float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
2225  float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
2226  float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
2227  float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
2228  float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
2229  float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
2230 
2231  // 4x4 sub-determinants required to calculate 6x6 determinant
2232  float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
2233  float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
2234  float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
2235  float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
2236  float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
2237  float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
2238  float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
2239  float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
2240  float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
2241  float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
2242  float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
2243  float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
2244  float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
2245  float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
2246  float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
2247 
2248  // 5x5 sub-determinants required to calculate 6x6 determinant
2249  float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
2250  float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
2251  float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
2252  float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
2253  float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
2254  float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
2255 
2256  // determinant of 6x6 matrix
2257  det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
2258  mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
2259 
2260  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
2261  return false;
2262  }
2263 
2264  invDet = 1.0f / det;
2265 
2266  // remaining 2x2 sub-determinants
2267  float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
2268  float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
2269  float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
2270  float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
2271  float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
2272  float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
2273  float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
2274  float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
2275  float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
2276  float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
2277  float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
2278  float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
2279  float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
2280  float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
2281  float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
2282  float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
2283  float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
2284  float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
2285  float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
2286  float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
2287  float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
2288  float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
2289  float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
2290  float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
2291  float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
2292  float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
2293  float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
2294  float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
2295  float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
2296  float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
2297 
2298  // remaining 3x3 sub-determinants
2299  float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
2300  float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
2301  float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
2302  float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
2303  float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
2304  float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
2305  float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
2306  float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
2307  float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
2308  float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
2309  float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
2310  float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
2311  float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
2312  float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
2313  float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
2314  float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
2315  float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
2316  float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
2317  float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
2318  float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
2319  float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
2320  float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
2321  float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
2322  float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
2323  float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
2324  float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
2325  float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
2326  float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
2327  float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
2328  float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
2329  float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
2330  float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
2331  float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
2332  float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
2333  float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
2334  float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
2335  float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
2336  float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
2337  float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
2338  float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
2339  float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
2340  float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
2341  float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
2342  float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
2343  float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
2344  float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
2345  float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
2346  float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
2347  float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
2348  float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
2349  float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
2350  float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
2351  float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
2352  float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
2353  float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
2354  float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
2355  float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
2356  float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
2357  float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
2358  float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
2359 
2360  // remaining 4x4 sub-determinants
2361  float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
2362  float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
2363  float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
2364  float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
2365  float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
2366  float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
2367  float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
2368  float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
2369  float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
2370  float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
2371  float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
2372  float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
2373  float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
2374  float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
2375  float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
2376  float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
2377  float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
2378  float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
2379  float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
2380  float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
2381  float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
2382  float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
2383  float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
2384  float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
2385  float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
2386  float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
2387  float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
2388  float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
2389  float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
2390  float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
2391  float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
2392  float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
2393  float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
2394  float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
2395  float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
2396  float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
2397  float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
2398  float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
2399  float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
2400  float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
2401  float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
2402  float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
2403  float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
2404  float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
2405  float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
2406  float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
2407  float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
2408  float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
2409  float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
2410  float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
2411  float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
2412  float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
2413  float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
2414  float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
2415  float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
2416  float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
2417  float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
2418  float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
2419  float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
2420  float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
2421 
2422  // remaining 5x5 sub-determinants
2423  float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
2424  float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
2425  float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
2426  float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
2427  float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
2428  float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
2429  float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
2430  float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
2431  float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
2432  float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
2433  float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
2434  float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
2435  float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
2436  float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
2437  float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
2438  float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
2439  float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
2440  float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
2441  float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
2442  float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
2443  float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
2444  float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
2445  float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
2446  float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
2447  float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
2448  float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
2449  float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
2450  float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
2451  float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
2452  float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
2453 
2454  mat[0][0] = det5_12345_12345 * invDet;
2455  mat[0][1] = -det5_02345_12345 * invDet;
2456  mat[0][2] = det5_01345_12345 * invDet;
2457  mat[0][3] = -det5_01245_12345 * invDet;
2458  mat[0][4] = det5_01235_12345 * invDet;
2459  mat[0][5] = -det5_01234_12345 * invDet;
2460 
2461  mat[1][0] = -det5_12345_02345 * invDet;
2462  mat[1][1] = det5_02345_02345 * invDet;
2463  mat[1][2] = -det5_01345_02345 * invDet;
2464  mat[1][3] = det5_01245_02345 * invDet;
2465  mat[1][4] = -det5_01235_02345 * invDet;
2466  mat[1][5] = det5_01234_02345 * invDet;
2467 
2468  mat[2][0] = det5_12345_01345 * invDet;
2469  mat[2][1] = -det5_02345_01345 * invDet;
2470  mat[2][2] = det5_01345_01345 * invDet;
2471  mat[2][3] = -det5_01245_01345 * invDet;
2472  mat[2][4] = det5_01235_01345 * invDet;
2473  mat[2][5] = -det5_01234_01345 * invDet;
2474 
2475  mat[3][0] = -det5_12345_01245 * invDet;
2476  mat[3][1] = det5_02345_01245 * invDet;
2477  mat[3][2] = -det5_01345_01245 * invDet;
2478  mat[3][3] = det5_01245_01245 * invDet;
2479  mat[3][4] = -det5_01235_01245 * invDet;
2480  mat[3][5] = det5_01234_01245 * invDet;
2481 
2482  mat[4][0] = det5_12345_01235 * invDet;
2483  mat[4][1] = -det5_02345_01235 * invDet;
2484  mat[4][2] = det5_01345_01235 * invDet;
2485  mat[4][3] = -det5_01245_01235 * invDet;
2486  mat[4][4] = det5_01235_01235 * invDet;
2487  mat[4][5] = -det5_01234_01235 * invDet;
2488 
2489  mat[5][0] = -det5_12345_01234 * invDet;
2490  mat[5][1] = det5_02345_01234 * invDet;
2491  mat[5][2] = -det5_01345_01234 * invDet;
2492  mat[5][3] = det5_01245_01234 * invDet;
2493  mat[5][4] = -det5_01235_01234 * invDet;
2494  mat[5][5] = det5_01234_01234 * invDet;
2495 
2496  return true;
2497 #elif 0
2498  // 6*40 = 240 multiplications
2499  // 6 divisions
2500  float *mat = reinterpret_cast<float *>(this);
2501  float s;
2502  double d, di;
2503 
2504  di = mat[0];
2505  s = di;
2506  mat[0] = d = 1.0f / di;
2507  mat[1] *= d;
2508  mat[2] *= d;
2509  mat[3] *= d;
2510  mat[4] *= d;
2511  mat[5] *= d;
2512  d = -d;
2513  mat[6] *= d;
2514  mat[12] *= d;
2515  mat[18] *= d;
2516  mat[24] *= d;
2517  mat[30] *= d;
2518  d = mat[6] * di;
2519  mat[7] += mat[1] * d;
2520  mat[8] += mat[2] * d;
2521  mat[9] += mat[3] * d;
2522  mat[10] += mat[4] * d;
2523  mat[11] += mat[5] * d;
2524  d = mat[12] * di;
2525  mat[13] += mat[1] * d;
2526  mat[14] += mat[2] * d;
2527  mat[15] += mat[3] * d;
2528  mat[16] += mat[4] * d;
2529  mat[17] += mat[5] * d;
2530  d = mat[18] * di;
2531  mat[19] += mat[1] * d;
2532  mat[20] += mat[2] * d;
2533  mat[21] += mat[3] * d;
2534  mat[22] += mat[4] * d;
2535  mat[23] += mat[5] * d;
2536  d = mat[24] * di;
2537  mat[25] += mat[1] * d;
2538  mat[26] += mat[2] * d;
2539  mat[27] += mat[3] * d;
2540  mat[28] += mat[4] * d;
2541  mat[29] += mat[5] * d;
2542  d = mat[30] * di;
2543  mat[31] += mat[1] * d;
2544  mat[32] += mat[2] * d;
2545  mat[33] += mat[3] * d;
2546  mat[34] += mat[4] * d;
2547  mat[35] += mat[5] * d;
2548  di = mat[7];
2549  s *= di;
2550  mat[7] = d = 1.0f / di;
2551  mat[6] *= d;
2552  mat[8] *= d;
2553  mat[9] *= d;
2554  mat[10] *= d;
2555  mat[11] *= d;
2556  d = -d;
2557  mat[1] *= d;
2558  mat[13] *= d;
2559  mat[19] *= d;
2560  mat[25] *= d;
2561  mat[31] *= d;
2562  d = mat[1] * di;
2563  mat[0] += mat[6] * d;
2564  mat[2] += mat[8] * d;
2565  mat[3] += mat[9] * d;
2566  mat[4] += mat[10] * d;
2567  mat[5] += mat[11] * d;
2568  d = mat[13] * di;
2569  mat[12] += mat[6] * d;
2570  mat[14] += mat[8] * d;
2571  mat[15] += mat[9] * d;
2572  mat[16] += mat[10] * d;
2573  mat[17] += mat[11] * d;
2574  d = mat[19] * di;
2575  mat[18] += mat[6] * d;
2576  mat[20] += mat[8] * d;
2577  mat[21] += mat[9] * d;
2578  mat[22] += mat[10] * d;
2579  mat[23] += mat[11] * d;
2580  d = mat[25] * di;
2581  mat[24] += mat[6] * d;
2582  mat[26] += mat[8] * d;
2583  mat[27] += mat[9] * d;
2584  mat[28] += mat[10] * d;
2585  mat[29] += mat[11] * d;
2586  d = mat[31] * di;
2587  mat[30] += mat[6] * d;
2588  mat[32] += mat[8] * d;
2589  mat[33] += mat[9] * d;
2590  mat[34] += mat[10] * d;
2591  mat[35] += mat[11] * d;
2592  di = mat[14];
2593  s *= di;
2594  mat[14] = d = 1.0f / di;
2595  mat[12] *= d;
2596  mat[13] *= d;
2597  mat[15] *= d;
2598  mat[16] *= d;
2599  mat[17] *= d;
2600  d = -d;
2601  mat[2] *= d;
2602  mat[8] *= d;
2603  mat[20] *= d;
2604  mat[26] *= d;
2605  mat[32] *= d;
2606  d = mat[2] * di;
2607  mat[0] += mat[12] * d;
2608  mat[1] += mat[13] * d;
2609  mat[3] += mat[15] * d;
2610  mat[4] += mat[16] * d;
2611  mat[5] += mat[17] * d;
2612  d = mat[8] * di;
2613  mat[6] += mat[12] * d;
2614  mat[7] += mat[13] * d;
2615  mat[9] += mat[15] * d;
2616  mat[10] += mat[16] * d;
2617  mat[11] += mat[17] * d;
2618  d = mat[20] * di;
2619  mat[18] += mat[12] * d;
2620  mat[19] += mat[13] * d;
2621  mat[21] += mat[15] * d;
2622  mat[22] += mat[16] * d;
2623  mat[23] += mat[17] * d;
2624  d = mat[26] * di;
2625  mat[24] += mat[12] * d;
2626  mat[25] += mat[13] * d;
2627  mat[27] += mat[15] * d;
2628  mat[28] += mat[16] * d;
2629  mat[29] += mat[17] * d;
2630  d = mat[32] * di;
2631  mat[30] += mat[12] * d;
2632  mat[31] += mat[13] * d;
2633  mat[33] += mat[15] * d;
2634  mat[34] += mat[16] * d;
2635  mat[35] += mat[17] * d;
2636  di = mat[21];
2637  s *= di;
2638  mat[21] = d = 1.0f / di;
2639  mat[18] *= d;
2640  mat[19] *= d;
2641  mat[20] *= d;
2642  mat[22] *= d;
2643  mat[23] *= d;
2644  d = -d;
2645  mat[3] *= d;
2646  mat[9] *= d;
2647  mat[15] *= d;
2648  mat[27] *= d;
2649  mat[33] *= d;
2650  d = mat[3] * di;
2651  mat[0] += mat[18] * d;
2652  mat[1] += mat[19] * d;
2653  mat[2] += mat[20] * d;
2654  mat[4] += mat[22] * d;
2655  mat[5] += mat[23] * d;
2656  d = mat[9] * di;
2657  mat[6] += mat[18] * d;
2658  mat[7] += mat[19] * d;
2659  mat[8] += mat[20] * d;
2660  mat[10] += mat[22] * d;
2661  mat[11] += mat[23] * d;
2662  d = mat[15] * di;
2663  mat[12] += mat[18] * d;
2664  mat[13] += mat[19] * d;
2665  mat[14] += mat[20] * d;
2666  mat[16] += mat[22] * d;
2667  mat[17] += mat[23] * d;
2668  d = mat[27] * di;
2669  mat[24] += mat[18] * d;
2670  mat[25] += mat[19] * d;
2671  mat[26] += mat[20] * d;
2672  mat[28] += mat[22] * d;
2673  mat[29] += mat[23] * d;
2674  d = mat[33] * di;
2675  mat[30] += mat[18] * d;
2676  mat[31] += mat[19] * d;
2677  mat[32] += mat[20] * d;
2678  mat[34] += mat[22] * d;
2679  mat[35] += mat[23] * d;
2680  di = mat[28];
2681  s *= di;
2682  mat[28] = d = 1.0f / di;
2683  mat[24] *= d;
2684  mat[25] *= d;
2685  mat[26] *= d;
2686  mat[27] *= d;
2687  mat[29] *= d;
2688  d = -d;
2689  mat[4] *= d;
2690  mat[10] *= d;
2691  mat[16] *= d;
2692  mat[22] *= d;
2693  mat[34] *= d;
2694  d = mat[4] * di;
2695  mat[0] += mat[24] * d;
2696  mat[1] += mat[25] * d;
2697  mat[2] += mat[26] * d;
2698  mat[3] += mat[27] * d;
2699  mat[5] += mat[29] * d;
2700  d = mat[10] * di;
2701  mat[6] += mat[24] * d;
2702  mat[7] += mat[25] * d;
2703  mat[8] += mat[26] * d;
2704  mat[9] += mat[27] * d;
2705  mat[11] += mat[29] * d;
2706  d = mat[16] * di;
2707  mat[12] += mat[24] * d;
2708  mat[13] += mat[25] * d;
2709  mat[14] += mat[26] * d;
2710  mat[15] += mat[27] * d;
2711  mat[17] += mat[29] * d;
2712  d = mat[22] * di;
2713  mat[18] += mat[24] * d;
2714  mat[19] += mat[25] * d;
2715  mat[20] += mat[26] * d;
2716  mat[21] += mat[27] * d;
2717  mat[23] += mat[29] * d;
2718  d = mat[34] * di;
2719  mat[30] += mat[24] * d;
2720  mat[31] += mat[25] * d;
2721  mat[32] += mat[26] * d;
2722  mat[33] += mat[27] * d;
2723  mat[35] += mat[29] * d;
2724  di = mat[35];
2725  s *= di;
2726  mat[35] = d = 1.0f / di;
2727  mat[30] *= d;
2728  mat[31] *= d;
2729  mat[32] *= d;
2730  mat[33] *= d;
2731  mat[34] *= d;
2732  d = -d;
2733  mat[5] *= d;
2734  mat[11] *= d;
2735  mat[17] *= d;
2736  mat[23] *= d;
2737  mat[29] *= d;
2738  d = mat[5] * di;
2739  mat[0] += mat[30] * d;
2740  mat[1] += mat[31] * d;
2741  mat[2] += mat[32] * d;
2742  mat[3] += mat[33] * d;
2743  mat[4] += mat[34] * d;
2744  d = mat[11] * di;
2745  mat[6] += mat[30] * d;
2746  mat[7] += mat[31] * d;
2747  mat[8] += mat[32] * d;
2748  mat[9] += mat[33] * d;
2749  mat[10] += mat[34] * d;
2750  d = mat[17] * di;
2751  mat[12] += mat[30] * d;
2752  mat[13] += mat[31] * d;
2753  mat[14] += mat[32] * d;
2754  mat[15] += mat[33] * d;
2755  mat[16] += mat[34] * d;
2756  d = mat[23] * di;
2757  mat[18] += mat[30] * d;
2758  mat[19] += mat[31] * d;
2759  mat[20] += mat[32] * d;
2760  mat[21] += mat[33] * d;
2761  mat[22] += mat[34] * d;
2762  d = mat[29] * di;
2763  mat[24] += mat[30] * d;
2764  mat[25] += mat[31] * d;
2765  mat[26] += mat[32] * d;
2766  mat[27] += mat[33] * d;
2767  mat[28] += mat[34] * d;
2768 
2769  return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
2770 #else
2771  // 6*27+2*30 = 222 multiplications
2772  // 2*1 = 2 divisions
2773  idMat3 r0, r1, r2, r3;
2774  float c0, c1, c2, det, invDet;
2775  float *mat = reinterpret_cast<float *>(this);
2776 
2777  // r0 = m0.Inverse();
2778  c0 = mat[1*6+1] * mat[2*6+2] - mat[1*6+2] * mat[2*6+1];
2779  c1 = mat[1*6+2] * mat[2*6+0] - mat[1*6+0] * mat[2*6+2];
2780  c2 = mat[1*6+0] * mat[2*6+1] - mat[1*6+1] * mat[2*6+0];
2781 
2782  det = mat[0*6+0] * c0 + mat[0*6+1] * c1 + mat[0*6+2] * c2;
2783 
2784  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
2785  return false;
2786  }
2787 
2788  invDet = 1.0f / det;
2789 
2790  r0[0][0] = c0 * invDet;
2791  r0[0][1] = ( mat[0*6+2] * mat[2*6+1] - mat[0*6+1] * mat[2*6+2] ) * invDet;
2792  r0[0][2] = ( mat[0*6+1] * mat[1*6+2] - mat[0*6+2] * mat[1*6+1] ) * invDet;
2793  r0[1][0] = c1 * invDet;
2794  r0[1][1] = ( mat[0*6+0] * mat[2*6+2] - mat[0*6+2] * mat[2*6+0] ) * invDet;
2795  r0[1][2] = ( mat[0*6+2] * mat[1*6+0] - mat[0*6+0] * mat[1*6+2] ) * invDet;
2796  r0[2][0] = c2 * invDet;
2797  r0[2][1] = ( mat[0*6+1] * mat[2*6+0] - mat[0*6+0] * mat[2*6+1] ) * invDet;
2798  r0[2][2] = ( mat[0*6+0] * mat[1*6+1] - mat[0*6+1] * mat[1*6+0] ) * invDet;
2799 
2800  // r1 = r0 * m1;
2801  r1[0][0] = r0[0][0] * mat[0*6+3] + r0[0][1] * mat[1*6+3] + r0[0][2] * mat[2*6+3];
2802  r1[0][1] = r0[0][0] * mat[0*6+4] + r0[0][1] * mat[1*6+4] + r0[0][2] * mat[2*6+4];
2803  r1[0][2] = r0[0][0] * mat[0*6+5] + r0[0][1] * mat[1*6+5] + r0[0][2] * mat[2*6+5];
2804  r1[1][0] = r0[1][0] * mat[0*6+3] + r0[1][1] * mat[1*6+3] + r0[1][2] * mat[2*6+3];
2805  r1[1][1] = r0[1][0] * mat[0*6+4] + r0[1][1] * mat[1*6+4] + r0[1][2] * mat[2*6+4];
2806  r1[1][2] = r0[1][0] * mat[0*6+5] + r0[1][1] * mat[1*6+5] + r0[1][2] * mat[2*6+5];
2807  r1[2][0] = r0[2][0] * mat[0*6+3] + r0[2][1] * mat[1*6+3] + r0[2][2] * mat[2*6+3];
2808  r1[2][1] = r0[2][0] * mat[0*6+4] + r0[2][1] * mat[1*6+4] + r0[2][2] * mat[2*6+4];
2809  r1[2][2] = r0[2][0] * mat[0*6+5] + r0[2][1] * mat[1*6+5] + r0[2][2] * mat[2*6+5];
2810 
2811  // r2 = m2 * r1;
2812  r2[0][0] = mat[3*6+0] * r1[0][0] + mat[3*6+1] * r1[1][0] + mat[3*6+2] * r1[2][0];
2813  r2[0][1] = mat[3*6+0] * r1[0][1] + mat[3*6+1] * r1[1][1] + mat[3*6+2] * r1[2][1];
2814  r2[0][2] = mat[3*6+0] * r1[0][2] + mat[3*6+1] * r1[1][2] + mat[3*6+2] * r1[2][2];
2815  r2[1][0] = mat[4*6+0] * r1[0][0] + mat[4*6+1] * r1[1][0] + mat[4*6+2] * r1[2][0];
2816  r2[1][1] = mat[4*6+0] * r1[0][1] + mat[4*6+1] * r1[1][1] + mat[4*6+2] * r1[2][1];
2817  r2[1][2] = mat[4*6+0] * r1[0][2] + mat[4*6+1] * r1[1][2] + mat[4*6+2] * r1[2][2];
2818  r2[2][0] = mat[5*6+0] * r1[0][0] + mat[5*6+1] * r1[1][0] + mat[5*6+2] * r1[2][0];
2819  r2[2][1] = mat[5*6+0] * r1[0][1] + mat[5*6+1] * r1[1][1] + mat[5*6+2] * r1[2][1];
2820  r2[2][2] = mat[5*6+0] * r1[0][2] + mat[5*6+1] * r1[1][2] + mat[5*6+2] * r1[2][2];
2821 
2822  // r3 = r2 - m3;
2823  r3[0][0] = r2[0][0] - mat[3*6+3];
2824  r3[0][1] = r2[0][1] - mat[3*6+4];
2825  r3[0][2] = r2[0][2] - mat[3*6+5];
2826  r3[1][0] = r2[1][0] - mat[4*6+3];
2827  r3[1][1] = r2[1][1] - mat[4*6+4];
2828  r3[1][2] = r2[1][2] - mat[4*6+5];
2829  r3[2][0] = r2[2][0] - mat[5*6+3];
2830  r3[2][1] = r2[2][1] - mat[5*6+4];
2831  r3[2][2] = r2[2][2] - mat[5*6+5];
2832 
2833  // r3.InverseSelf();
2834  r2[0][0] = r3[1][1] * r3[2][2] - r3[1][2] * r3[2][1];
2835  r2[1][0] = r3[1][2] * r3[2][0] - r3[1][0] * r3[2][2];
2836  r2[2][0] = r3[1][0] * r3[2][1] - r3[1][1] * r3[2][0];
2837 
2838  det = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
2839 
2840  if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
2841  return false;
2842  }
2843 
2844  invDet = 1.0f / det;
2845 
2846  r2[0][1] = r3[0][2] * r3[2][1] - r3[0][1] * r3[2][2];
2847  r2[0][2] = r3[0][1] * r3[1][2] - r3[0][2] * r3[1][1];
2848  r2[1][1] = r3[0][0] * r3[2][2] - r3[0][2] * r3[2][0];
2849  r2[1][2] = r3[0][2] * r3[1][0] - r3[0][0] * r3[1][2];
2850  r2[2][1] = r3[0][1] * r3[2][0] - r3[0][0] * r3[2][1];
2851  r2[2][2] = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
2852 
2853  r3[0][0] = r2[0][0] * invDet;
2854  r3[0][1] = r2[0][1] * invDet;
2855  r3[0][2] = r2[0][2] * invDet;
2856  r3[1][0] = r2[1][0] * invDet;
2857  r3[1][1] = r2[1][1] * invDet;
2858  r3[1][2] = r2[1][2] * invDet;
2859  r3[2][0] = r2[2][0] * invDet;
2860  r3[2][1] = r2[2][1] * invDet;
2861  r3[2][2] = r2[2][2] * invDet;
2862 
2863  // r2 = m2 * r0;
2864  r2[0][0] = mat[3*6+0] * r0[0][0] + mat[3*6+1] * r0[1][0] + mat[3*6+2] * r0[2][0];
2865  r2[0][1] = mat[3*6+0] * r0[0][1] + mat[3*6+1] * r0[1][1] + mat[3*6+2] * r0[2][1];
2866  r2[0][2] = mat[3*6+0] * r0[0][2] + mat[3*6+1] * r0[1][2] + mat[3*6+2] * r0[2][2];
2867  r2[1][0] = mat[4*6+0] * r0[0][0] + mat[4*6+1] * r0[1][0] + mat[4*6+2] * r0[2][0];
2868  r2[1][1] = mat[4*6+0] * r0[0][1] + mat[4*6+1] * r0[1][1] + mat[4*6+2] * r0[2][1];
2869  r2[1][2] = mat[4*6+0] * r0[0][2] + mat[4*6+1] * r0[1][2] + mat[4*6+2] * r0[2][2];
2870  r2[2][0] = mat[5*6+0] * r0[0][0] + mat[5*6+1] * r0[1][0] + mat[5*6+2] * r0[2][0];
2871  r2[2][1] = mat[5*6+0] * r0[0][1] + mat[5*6+1] * r0[1][1] + mat[5*6+2] * r0[2][1];
2872  r2[2][2] = mat[5*6+0] * r0[0][2] + mat[5*6+1] * r0[1][2] + mat[5*6+2] * r0[2][2];
2873 
2874  // m2 = r3 * r2;
2875  mat[3*6+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
2876  mat[3*6+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1] + r3[0][2] * r2[2][1];
2877  mat[3*6+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2] + r3[0][2] * r2[2][2];
2878  mat[4*6+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0] + r3[1][2] * r2[2][0];
2879  mat[4*6+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1] + r3[1][2] * r2[2][1];
2880  mat[4*6+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2] + r3[1][2] * r2[2][2];
2881  mat[5*6+0] = r3[2][0] * r2[0][0] + r3[2][1] * r2[1][0] + r3[2][2] * r2[2][0];
2882  mat[5*6+1] = r3[2][0] * r2[0][1] + r3[2][1] * r2[1][1] + r3[2][2] * r2[2][1];
2883  mat[5*6+2] = r3[2][0] * r2[0][2] + r3[2][1] * r2[1][2] + r3[2][2] * r2[2][2];
2884 
2885  // m0 = r0 - r1 * m2;
2886  mat[0*6+0] = r0[0][0] - r1[0][0] * mat[3*6+0] - r1[0][1] * mat[4*6+0] - r1[0][2] * mat[5*6+0];
2887  mat[0*6+1] = r0[0][1] - r1[0][0] * mat[3*6+1] - r1[0][1] * mat[4*6+1] - r1[0][2] * mat[5*6+1];
2888  mat[0*6+2] = r0[0][2] - r1[0][0] * mat[3*6+2] - r1[0][1] * mat[4*6+2] - r1[0][2] * mat[5*6+2];
2889  mat[1*6+0] = r0[1][0] - r1[1][0] * mat[3*6+0] - r1[1][1] * mat[4*6+0] - r1[1][2] * mat[5*6+0];
2890  mat[1*6+1] = r0[1][1] - r1[1][0] * mat[3*6+1] - r1[1][1] * mat[4*6+1] - r1[1][2] * mat[5*6+1];
2891  mat[1*6+2] = r0[1][2] - r1[1][0] * mat[3*6+2] - r1[1][1] * mat[4*6+2] - r1[1][2] * mat[5*6+2];
2892  mat[2*6+0] = r0[2][0] - r1[2][0] * mat[3*6+0] - r1[2][1] * mat[4*6+0] - r1[2][2] * mat[5*6+0];
2893  mat[2*6+1] = r0[2][1] - r1[2][0] * mat[3*6+1] - r1[2][1] * mat[4*6+1] - r1[2][2] * mat[5*6+1];
2894  mat[2*6+2] = r0[2][2] - r1[2][0] * mat[3*6+2] - r1[2][1] * mat[4*6+2] - r1[2][2] * mat[5*6+2];
2895 
2896  // m1 = r1 * r3;
2897  mat[0*6+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0] + r1[0][2] * r3[2][0];
2898  mat[0*6+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1] + r1[0][2] * r3[2][1];
2899  mat[0*6+5] = r1[0][0] * r3[0][2] + r1[0][1] * r3[1][2] + r1[0][2] * r3[2][2];
2900  mat[1*6+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0] + r1[1][2] * r3[2][0];
2901  mat[1*6+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1] + r1[1][2] * r3[2][1];
2902  mat[1*6+5] = r1[1][0] * r3[0][2] + r1[1][1] * r3[1][2] + r1[1][2] * r3[2][2];
2903  mat[2*6+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0] + r1[2][2] * r3[2][0];
2904  mat[2*6+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1] + r1[2][2] * r3[2][1];
2905  mat[2*6+5] = r1[2][0] * r3[0][2] + r1[2][1] * r3[1][2] + r1[2][2] * r3[2][2];
2906 
2907  // m3 = -r3;
2908  mat[3*6+3] = -r3[0][0];
2909  mat[3*6+4] = -r3[0][1];
2910  mat[3*6+5] = -r3[0][2];
2911  mat[4*6+3] = -r3[1][0];
2912  mat[4*6+4] = -r3[1][1];
2913  mat[4*6+5] = -r3[1][2];
2914  mat[5*6+3] = -r3[2][0];
2915  mat[5*6+4] = -r3[2][1];
2916  mat[5*6+5] = -r3[2][2];
2917 
2918  return true;
2919 #endif
2920 }
2921 
2922 /*
2923 =============
2924 idMat6::ToString
2925 =============
2926 */
2927 const char *idMat6::ToString( int precision ) const {
2928  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
2929 }
2930 
2931 
2932 //===============================================================
2933 //
2934 // idMatX
2935 //
2936 //===============================================================
2937 
2938 float idMatX::temp[MATX_MAX_TEMP+4];
2939 float * idMatX::tempPtr = (float *) ( ( (int) idMatX::temp + 15 ) & ~15 );
2940 int idMatX::tempIndex = 0;
2941 
2942 
2943 /*
2944 ============
2945 idMatX::ChangeSize
2946 ============
2947 */
2948 void idMatX::ChangeSize( int rows, int columns, bool makeZero ) {
2949  int alloc = ( rows * columns + 3 ) & ~3;
2950  if ( alloc > alloced && alloced != -1 ) {
2951  float *oldMat = mat;
2952  mat = (float *) Mem_Alloc16( alloc * sizeof( float ) );
2953  if ( makeZero ) {
2954  memset( mat, 0, alloc * sizeof( float ) );
2955  }
2956  alloced = alloc;
2957  if ( oldMat ) {
2958  int minRow = Min( numRows, rows );
2959  int minColumn = Min( numColumns, columns );
2960  for ( int i = 0; i < minRow; i++ ) {
2961  for ( int j = 0; j < minColumn; j++ ) {
2962  mat[ i * columns + j ] = oldMat[ i * numColumns + j ];
2963  }
2964  }
2965  Mem_Free16( oldMat );
2966  }
2967  } else {
2968  if ( columns < numColumns ) {
2969  int minRow = Min( numRows, rows );
2970  for ( int i = 0; i < minRow; i++ ) {
2971  for ( int j = 0; j < columns; j++ ) {
2972  mat[ i * columns + j ] = mat[ i * numColumns + j ];
2973  }
2974  }
2975  } else if ( columns > numColumns ) {
2976  for ( int i = Min( numRows, rows ) - 1; i >= 0; i-- ) {
2977  if ( makeZero ) {
2978  for ( int j = columns - 1; j >= numColumns; j-- ) {
2979  mat[ i * columns + j ] = 0.0f;
2980  }
2981  }
2982  for ( int j = numColumns - 1; j >= 0; j-- ) {
2983  mat[ i * columns + j ] = mat[ i * numColumns + j ];
2984  }
2985  }
2986  }
2987  if ( makeZero && rows > numRows ) {
2988  memset( mat + numRows * columns, 0, ( rows - numRows ) * columns * sizeof( float ) );
2989  }
2990  }
2991  numRows = rows;
2992  numColumns = columns;
2993  MATX_CLEAREND();
2994 }
2995 
2996 /*
2997 ============
2998 idMatX::RemoveRow
2999 ============
3000 */
3001 idMatX &idMatX::RemoveRow( int r ) {
3002  int i;
3003 
3004  assert( r < numRows );
3005 
3006  numRows--;
3007 
3008  for ( i = r; i < numRows; i++ ) {
3009  memcpy( &mat[i * numColumns], &mat[( i + 1 ) * numColumns], numColumns * sizeof( float ) );
3010  }
3011 
3012  return *this;
3013 }
3014 
3015 /*
3016 ============
3017 idMatX::RemoveColumn
3018 ============
3019 */
3020 idMatX &idMatX::RemoveColumn( int r ) {
3021  int i;
3022 
3023  assert( r < numColumns );
3024 
3025  numColumns--;
3026 
3027  for ( i = 0; i < numRows - 1; i++ ) {
3028  memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
3029  }
3030  memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
3031 
3032  return *this;
3033 }
3034 
3035 /*
3036 ============
3037 idMatX::RemoveRowColumn
3038 ============
3039 */
3040 idMatX &idMatX::RemoveRowColumn( int r ) {
3041  int i;
3042 
3043  assert( r < numRows && r < numColumns );
3044 
3045  numRows--;
3046  numColumns--;
3047 
3048  if ( r > 0 ) {
3049  for ( i = 0; i < r - 1; i++ ) {
3050  memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
3051  }
3052  memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
3053  }
3054 
3055  memcpy( &mat[r * numColumns], &mat[( r + 1 ) * ( numColumns + 1 )], r * sizeof( float ) );
3056 
3057  for ( i = r; i < numRows - 1; i++ ) {
3058  memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
3059  }
3060  memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
3061 
3062  return *this;
3063 }
3064 
3065 /*
3066 ============
3067 idMatX::IsOrthogonal
3068 
3069  returns true if (*this) * this->Transpose() == Identity
3070 ============
3071 */
3072 bool idMatX::IsOrthogonal( const float epsilon ) const {
3073  float *ptr1, *ptr2, sum;
3074 
3075  if ( !IsSquare() ) {
3076  return false;
3077  }
3078 
3079  ptr1 = mat;
3080  for ( int i = 0; i < numRows; i++ ) {
3081  for ( int j = 0; j < numColumns; j++ ) {
3082  ptr2 = mat + j;
3083  sum = ptr1[0] * ptr2[0] - (float) ( i == j );
3084  for ( int n = 1; n < numColumns; n++ ) {
3085  ptr2 += numColumns;
3086  sum += ptr1[n] * ptr2[0];
3087  }
3088  if ( idMath::Fabs( sum ) > epsilon ) {
3089  return false;
3090  }
3091  }
3092  ptr1 += numColumns;
3093  }
3094  return true;
3095 }
3096 
3097 /*
3098 ============
3099 idMatX::IsOrthonormal
3100 
3101  returns true if (*this) * this->Transpose() == Identity and the length of each column vector is 1
3102 ============
3103 */
3104 bool idMatX::IsOrthonormal( const float epsilon ) const {
3105  float *ptr1, *ptr2, sum;
3106 
3107  if ( !IsSquare() ) {
3108  return false;
3109  }
3110 
3111  ptr1 = mat;
3112  for ( int i = 0; i < numRows; i++ ) {
3113  for ( int j = 0; j < numColumns; j++ ) {
3114  ptr2 = mat + j;
3115  sum = ptr1[0] * ptr2[0] - (float) ( i == j );
3116  for ( int n = 1; n < numColumns; n++ ) {
3117  ptr2 += numColumns;
3118  sum += ptr1[n] * ptr2[0];
3119  }
3120  if ( idMath::Fabs( sum ) > epsilon ) {
3121  return false;
3122  }
3123  }
3124  ptr1 += numColumns;
3125 
3126  ptr2 = mat + i;
3127  sum = ptr2[0] * ptr2[0] - 1.0f;
3128  for ( i = 1; i < numRows; i++ ) {
3129  ptr2 += numColumns;
3130  sum += ptr2[i] * ptr2[i];
3131  }
3132  if ( idMath::Fabs( sum ) > epsilon ) {
3133  return false;
3134  }
3135  }
3136  return true;
3137 }
3138 
3139 /*
3140 ============
3141 idMatX::IsPMatrix
3142 
3143  returns true if the matrix is a P-matrix
3144  A square matrix is a P-matrix if all its principal minors are positive.
3145 ============
3146 */
3147 bool idMatX::IsPMatrix( const float epsilon ) const {
3148  int i, j;
3149  float d;
3150  idMatX m;
3151 
3152  if ( !IsSquare() ) {
3153  return false;
3154  }
3155 
3156  if ( numRows <= 0 ) {
3157  return true;
3158  }
3159 
3160  if ( (*this)[0][0] <= epsilon ) {
3161  return false;
3162  }
3163 
3164  if ( numRows <= 1 ) {
3165  return true;
3166  }
3167 
3168  m.SetData( numRows - 1, numColumns - 1, MATX_ALLOCA( ( numRows - 1 ) * ( numColumns - 1 ) ) );
3169 
3170  for ( i = 1; i < numRows; i++ ) {
3171  for ( j = 1; j < numColumns; j++ ) {
3172  m[i-1][j-1] = (*this)[i][j];
3173  }
3174  }
3175 
3176  if ( !m.IsPMatrix( epsilon ) ) {
3177  return false;
3178  }
3179 
3180  for ( i = 1; i < numRows; i++ ) {
3181  d = (*this)[i][0] / (*this)[0][0];
3182  for ( j = 1; j < numColumns; j++ ) {
3183  m[i-1][j-1] = (*this)[i][j] - d * (*this)[0][j];
3184  }
3185  }
3186 
3187  if ( !m.IsPMatrix( epsilon ) ) {
3188  return false;
3189  }
3190 
3191  return true;
3192 }
3193 
3194 /*
3195 ============
3196 idMatX::IsZMatrix
3197 
3198  returns true if the matrix is a Z-matrix
3199  A square matrix M is a Z-matrix if M[i][j] <= 0 for all i != j.
3200 ============
3201 */
3202 bool idMatX::IsZMatrix( const float epsilon ) const {
3203  int i, j;
3204 
3205  if ( !IsSquare() ) {
3206  return false;
3207  }
3208 
3209  for ( i = 0; i < numRows; i++ ) {
3210  for ( j = 0; j < numColumns; j++ ) {
3211  if ( (*this)[i][j] > epsilon && i != j ) {
3212  return false;
3213  }
3214  }
3215  }
3216  return true;
3217 }
3218 
3219 /*
3220 ============
3221 idMatX::IsPositiveDefinite
3222 
3223  returns true if the matrix is Positive Definite (PD)
3224  A square matrix M of order n is said to be PD if y'My > 0 for all vectors y of dimension n, y != 0.
3225 ============
3226 */
3227 bool idMatX::IsPositiveDefinite( const float epsilon ) const {
3228  int i, j, k;
3229  float d, s;
3230  idMatX m;
3231 
3232  // the matrix must be square
3233  if ( !IsSquare() ) {
3234  return false;
3235  }
3236 
3237  // copy matrix
3238  m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
3239  m = *this;
3240 
3241  // add transpose
3242  for ( i = 0; i < numRows; i++ ) {
3243  for ( j = 0; j < numColumns; j++ ) {
3244  m[i][j] += (*this)[j][i];
3245  }
3246  }
3247 
3248  // test Positive Definiteness with Gaussian pivot steps
3249  for ( i = 0; i < numRows; i++ ) {
3250 
3251  for ( j = i; j < numColumns; j++ ) {
3252  if ( m[j][j] <= epsilon ) {
3253  return false;
3254  }
3255  }
3256 
3257  d = 1.0f / m[i][i];
3258  for ( j = i + 1; j < numColumns; j++ ) {
3259  s = d * m[j][i];
3260  m[j][i] = 0.0f;
3261  for ( k = i + 1; k < numRows; k++ ) {
3262  m[j][k] -= s * m[i][k];
3263  }
3264  }
3265  }
3266 
3267  return true;
3268 }
3269 
3270 /*
3271 ============
3272 idMatX::IsSymmetricPositiveDefinite
3273 
3274  returns true if the matrix is Symmetric Positive Definite (PD)
3275 ============
3276 */
3277 bool idMatX::IsSymmetricPositiveDefinite( const float epsilon ) const {
3278  idMatX m;
3279 
3280  // the matrix must be symmetric
3281  if ( !IsSymmetric( epsilon ) ) {
3282  return false;
3283  }
3284 
3285  // copy matrix
3286  m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
3287  m = *this;
3288 
3289  // being able to obtain Cholesky factors is both a necessary and sufficient condition for positive definiteness
3290  return m.Cholesky_Factor();
3291 }
3292 
3293 /*
3294 ============
3295 idMatX::IsPositiveSemiDefinite
3296 
3297  returns true if the matrix is Positive Semi Definite (PSD)
3298  A square matrix M of order n is said to be PSD if y'My >= 0 for all vectors y of dimension n, y != 0.
3299 ============
3300 */
3301 bool idMatX::IsPositiveSemiDefinite( const float epsilon ) const {
3302  int i, j, k;
3303  float d, s;
3304  idMatX m;
3305 
3306  // the matrix must be square
3307  if ( !IsSquare() ) {
3308  return false;
3309  }
3310 
3311  // copy original matrix
3312  m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
3313  m = *this;
3314 
3315  // add transpose
3316  for ( i = 0; i < numRows; i++ ) {
3317  for ( j = 0; j < numColumns; j++ ) {
3318  m[i][j] += (*this)[j][i];
3319  }
3320  }
3321 
3322  // test Positive Semi Definiteness with Gaussian pivot steps
3323  for ( i = 0; i < numRows; i++ ) {
3324 
3325  for ( j = i; j < numColumns; j++ ) {
3326  if ( m[j][j] < -epsilon ) {
3327  return false;
3328  }
3329  if ( m[j][j] > epsilon ) {
3330  continue;
3331  }
3332  for ( k = 0; k < numRows; k++ ) {
3333  if ( idMath::Fabs( m[k][j] ) > epsilon ) {
3334  return false;
3335  }
3336  if ( idMath::Fabs( m[j][k] ) > epsilon ) {
3337  return false;
3338  }
3339  }
3340  }
3341 
3342  if ( m[i][i] <= epsilon ) {
3343  continue;
3344  }
3345 
3346  d = 1.0f / m[i][i];
3347  for ( j = i + 1; j < numColumns; j++ ) {
3348  s = d * m[j][i];
3349  m[j][i] = 0.0f;
3350  for ( k = i + 1; k < numRows; k++ ) {
3351  m[j][k] -= s * m[i][k];
3352  }
3353  }
3354  }
3355 
3356  return true;
3357 }
3358 
3359 /*
3360 ============
3361 idMatX::IsSymmetricPositiveSemiDefinite
3362 
3363  returns true if the matrix is Symmetric Positive Semi Definite (PSD)
3364 ============
3365 */
3366 bool idMatX::IsSymmetricPositiveSemiDefinite( const float epsilon ) const {
3367 
3368  // the matrix must be symmetric
3369  if ( !IsSymmetric( epsilon ) ) {
3370  return false;
3371  }
3372 
3373  return IsPositiveSemiDefinite( epsilon );
3374 }
3375 
3376 /*
3377 ============
3378 idMatX::LowerTriangularInverse
3379 
3380  in-place inversion of the lower triangular matrix
3381 ============
3382 */
3383 bool idMatX::LowerTriangularInverse( void ) {
3384  int i, j, k;
3385  double d, sum;
3386 
3387  for ( i = 0; i < numRows; i++ ) {
3388  d = (*this)[i][i];
3389  if ( d == 0.0f ) {
3390  return false;
3391  }
3392  (*this)[i][i] = d = 1.0f / d;
3393 
3394  for ( j = 0; j < i; j++ ) {
3395  sum = 0.0f;
3396  for ( k = j; k < i; k++ ) {
3397  sum -= (*this)[i][k] * (*this)[k][j];
3398  }
3399  (*this)[i][j] = sum * d;
3400  }
3401  }
3402  return true;
3403 }
3404 
3405 /*
3406 ============
3407 idMatX::UpperTriangularInverse
3408 
3409  in-place inversion of the upper triangular matrix
3410 ============
3411 */
3412 bool idMatX::UpperTriangularInverse( void ) {
3413  int i, j, k;
3414  double d, sum;
3415 
3416  for ( i = numRows-1; i >= 0; i-- ) {
3417  d = (*this)[i][i];
3418  if ( d == 0.0f ) {
3419  return false;
3420  }
3421  (*this)[i][i] = d = 1.0f / d;
3422 
3423  for ( j = numRows-1; j > i; j-- ) {
3424  sum = 0.0f;
3425  for ( k = j; k > i; k-- ) {
3426  sum -= (*this)[i][k] * (*this)[k][j];
3427  }
3428  (*this)[i][j] = sum * d;
3429  }
3430  }
3431  return true;
3432 }
3433 
3434 /*
3435 =============
3436 idMatX::ToString
3437 =============
3438 */
3439 const char *idMatX::ToString( int precision ) const {
3440  return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
3441 }
3442 
3443 /*
3444 ============
3445 idMatX::Update_RankOne
3446 
3447  Updates the matrix to obtain the matrix: A + alpha * v * w'
3448 ============
3449 */
3450 void idMatX::Update_RankOne( const idVecX &v, const idVecX &w, float alpha ) {
3451  int i, j;
3452  float s;
3453 
3454  assert( v.GetSize() >= numRows );
3455  assert( w.GetSize() >= numColumns );
3456 
3457  for ( i = 0; i < numRows; i++ ) {
3458  s = alpha * v[i];
3459  for ( j = 0; j < numColumns; j++ ) {
3460  (*this)[i][j] += s * w[j];
3461  }
3462  }
3463 }
3464 
3465 /*
3466 ============
3467 idMatX::Update_RankOneSymmetric
3468 
3469  Updates the matrix to obtain the matrix: A + alpha * v * v'
3470 ============
3471 */
3472 void idMatX::Update_RankOneSymmetric( const idVecX &v, float alpha ) {
3473  int i, j;
3474  float s;
3475 
3476  assert( numRows == numColumns );
3477  assert( v.GetSize() >= numRows );
3478 
3479  for ( i = 0; i < numRows; i++ ) {
3480  s = alpha * v[i];
3481  for ( j = 0; j < numColumns; j++ ) {
3482  (*this)[i][j] += s * v[j];
3483  }
3484  }
3485 }
3486 
3487 /*
3488 ============
3489 idMatX::Update_RowColumn
3490 
3491  Updates the matrix to obtain the matrix:
3492 
3493  [ 0 a 0 ]
3494  A + [ d b e ]
3495  [ 0 c 0 ]
3496 
3497  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
3498 ============
3499 */
3500 void idMatX::Update_RowColumn( const idVecX &v, const idVecX &w, int r ) {
3501  int i;
3502 
3503  assert( w[r] == 0.0f );
3504  assert( v.GetSize() >= numColumns );
3505  assert( w.GetSize() >= numRows );
3506 
3507  for ( i = 0; i < numRows; i++ ) {
3508  (*this)[i][r] += v[i];
3509  }
3510  for ( i = 0; i < numColumns; i++ ) {
3511  (*this)[r][i] += w[i];
3512  }
3513 }
3514 
3515 /*
3516 ============
3517 idMatX::Update_RowColumnSymmetric
3518 
3519  Updates the matrix to obtain the matrix:
3520 
3521  [ 0 a 0 ]
3522  A + [ a b c ]
3523  [ 0 c 0 ]
3524 
3525  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
3526 ============
3527 */
3528 void idMatX::Update_RowColumnSymmetric( const idVecX &v, int r ) {
3529  int i;
3530 
3531  assert( numRows == numColumns );
3532  assert( v.GetSize() >= numRows );
3533 
3534  for ( i = 0; i < r; i++ ) {
3535  (*this)[i][r] += v[i];
3536  (*this)[r][i] += v[i];
3537  }
3538  (*this)[r][r] += v[r];
3539  for ( i = r+1; i < numRows; i++ ) {
3540  (*this)[i][r] += v[i];
3541  (*this)[r][i] += v[i];
3542  }
3543 }
3544 
3545 /*
3546 ============
3547 idMatX::Update_Increment
3548 
3549  Updates the matrix to obtain the matrix:
3550 
3551  [ A a ]
3552  [ c b ]
3553 
3554  where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1]], w[numColumns] = 0
3555 ============
3556 */
3557 void idMatX::Update_Increment( const idVecX &v, const idVecX &w ) {
3558  int i;
3559 
3560  assert( numRows == numColumns );
3561  assert( v.GetSize() >= numRows+1 );
3562  assert( w.GetSize() >= numColumns+1 );
3563 
3564  ChangeSize( numRows+1, numColumns+1, false );
3565 
3566  for ( i = 0; i < numRows; i++ ) {
3567  (*this)[i][numColumns-1] = v[i];
3568  }
3569  for ( i = 0; i < numColumns-1; i++ ) {
3570  (*this)[numRows-1][i] = w[i];
3571  }
3572 }
3573 
3574 /*
3575 ============
3576 idMatX::Update_IncrementSymmetric
3577 
3578  Updates the matrix to obtain the matrix:
3579 
3580  [ A a ]
3581  [ a b ]
3582 
3583  where: a = v[0,numRows-1], b = v[numRows]
3584 ============
3585 */
3586 void idMatX::Update_IncrementSymmetric( const idVecX &v ) {
3587  int i;
3588 
3589  assert( numRows == numColumns );
3590  assert( v.GetSize() >= numRows+1 );
3591 
3592  ChangeSize( numRows+1, numColumns+1, false );
3593 
3594  for ( i = 0; i < numRows-1; i++ ) {
3595  (*this)[i][numColumns-1] = v[i];
3596  }
3597  for ( i = 0; i < numColumns; i++ ) {
3598  (*this)[numRows-1][i] = v[i];
3599  }
3600 }
3601 
3602 /*
3603 ============
3604 idMatX::Update_Decrement
3605 
3606  Updates the matrix to obtain a matrix with row r and column r removed.
3607 ============
3608 */
3609 void idMatX::Update_Decrement( int r ) {
3610  RemoveRowColumn( r );
3611 }
3612 
3613 /*
3614 ============
3615 idMatX::Inverse_GaussJordan
3616 
3617  in-place inversion using Gauss-Jordan elimination
3618 ============
3619 */
3620 bool idMatX::Inverse_GaussJordan( void ) {
3621  int i, j, k, r, c;
3622  float d, max;
3623 
3624  assert( numRows == numColumns );
3625 
3626  int *columnIndex = (int *) _alloca16( numRows * sizeof( int ) );
3627  int *rowIndex = (int *) _alloca16( numRows * sizeof( int ) );
3628  bool *pivot = (bool *) _alloca16( numRows * sizeof( bool ) );
3629 
3630  memset( pivot, 0, numRows * sizeof( bool ) );
3631 
3632  // elimination with full pivoting
3633  for ( i = 0; i < numRows; i++ ) {
3634 
3635  // search the whole matrix except for pivoted rows for the maximum absolute value
3636  max = 0.0f;
3637  r = c = 0;
3638  for ( j = 0; j < numRows; j++ ) {
3639  if ( !pivot[j] ) {
3640  for ( k = 0; k < numRows; k++ ) {
3641  if ( !pivot[k] ) {
3642  d = idMath::Fabs( (*this)[j][k] );
3643  if ( d > max ) {
3644  max = d;
3645  r = j;
3646  c = k;
3647  }
3648  }
3649  }
3650  }
3651  }
3652 
3653  if ( max == 0.0f ) {
3654  // matrix is not invertible
3655  return false;
3656  }
3657 
3658  pivot[c] = true;
3659 
3660  // swap rows such that entry (c,c) has the pivot entry
3661  if ( r != c ) {
3662  SwapRows( r, c );
3663  }
3664 
3665  // keep track of the row permutation
3666  rowIndex[i] = r;
3667  columnIndex[i] = c;
3668 
3669  // scale the row to make the pivot entry equal to 1
3670  d = 1.0f / (*this)[c][c];
3671  (*this)[c][c] = 1.0f;
3672  for ( k = 0; k < numRows; k++ ) {
3673  (*this)[c][k] *= d;
3674  }
3675 
3676  // zero out the pivot column entries in the other rows
3677  for ( j = 0; j < numRows; j++ ) {
3678  if ( j != c ) {
3679  d = (*this)[j][c];
3680  (*this)[j][c] = 0.0f;
3681  for ( k = 0; k < numRows; k++ ) {
3682  (*this)[j][k] -= (*this)[c][k] * d;
3683  }
3684  }
3685  }
3686  }
3687 
3688  // reorder rows to store the inverse of the original matrix
3689  for ( j = numRows - 1; j >= 0; j-- ) {
3690  if ( rowIndex[j] != columnIndex[j] ) {
3691  for ( k = 0; k < numRows; k++ ) {
3692  d = (*this)[k][rowIndex[j]];
3693  (*this)[k][rowIndex[j]] = (*this)[k][columnIndex[j]];
3694  (*this)[k][columnIndex[j]] = d;
3695  }
3696  }
3697  }
3698 
3699  return true;
3700 }
3701 
3702 /*
3703 ============
3704 idMatX::Inverse_UpdateRankOne
3705 
3706  Updates the in-place inverse using the Sherman-Morrison formula to obtain the inverse for the matrix: A + alpha * v * w'
3707 ============
3708 */
3709 bool idMatX::Inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha ) {
3710  int i, j;
3711  float beta, s;
3712  idVecX y, z;
3713 
3714  assert( numRows == numColumns );
3715  assert( v.GetSize() >= numColumns );
3716  assert( w.GetSize() >= numRows );
3717 
3718  y.SetData( numRows, VECX_ALLOCA( numRows ) );
3719  z.SetData( numRows, VECX_ALLOCA( numRows ) );
3720 
3721  Multiply( y, v );
3722  TransposeMultiply( z, w );
3723  beta = 1.0f + ( w * y );
3724 
3725  if ( beta == 0.0f ) {
3726  return false;
3727  }
3728 
3729  alpha /= beta;
3730 
3731  for ( i = 0; i < numRows; i++ ) {
3732  s = y[i] * alpha;
3733  for ( j = 0; j < numColumns; j++ ) {
3734  (*this)[i][j] -= s * z[j];
3735  }
3736  }
3737  return true;
3738 }
3739 
3740 /*
3741 ============
3742 idMatX::Inverse_UpdateRowColumn
3743 
3744  Updates the in-place inverse to obtain the inverse for the matrix:
3745 
3746  [ 0 a 0 ]
3747  A + [ d b e ]
3748  [ 0 c 0 ]
3749 
3750  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
3751 ============
3752 */
3753 bool idMatX::Inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r ) {
3754  idVecX s;
3755 
3756  assert( numRows == numColumns );
3757  assert( v.GetSize() >= numColumns );
3758  assert( w.GetSize() >= numRows );
3759  assert( r >= 0 && r < numRows && r < numColumns );
3760  assert( w[r] == 0.0f );
3761 
3762  s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
3763  s.Zero();
3764  s[r] = 1.0f;
3765 
3766  if ( !Inverse_UpdateRankOne( v, s, 1.0f ) ) {
3767  return false;
3768  }
3769  if ( !Inverse_UpdateRankOne( s, w, 1.0f ) ) {
3770  return false;
3771  }
3772  return true;
3773 }
3774 
3775 /*
3776 ============
3777 idMatX::Inverse_UpdateIncrement
3778 
3779  Updates the in-place inverse to obtain the inverse for the matrix:
3780 
3781  [ A a ]
3782  [ c b ]
3783 
3784  where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
3785 ============
3786 */
3787 bool idMatX::Inverse_UpdateIncrement( const idVecX &v, const idVecX &w ) {
3788  idVecX v2;
3789 
3790  assert( numRows == numColumns );
3791  assert( v.GetSize() >= numRows+1 );
3792  assert( w.GetSize() >= numColumns+1 );
3793 
3794  ChangeSize( numRows+1, numColumns+1, true );
3795  (*this)[numRows-1][numRows-1] = 1.0f;
3796 
3797  v2.SetData( numRows, VECX_ALLOCA( numRows ) );
3798  v2 = v;
3799  v2[numRows-1] -= 1.0f;
3800 
3801  return Inverse_UpdateRowColumn( v2, w, numRows-1 );
3802 }
3803 
3804 /*
3805 ============
3806 idMatX::Inverse_UpdateDecrement
3807 
3808  Updates the in-place inverse to obtain the inverse of the matrix with row r and column r removed.
3809  v and w should store the column and row of the original matrix respectively.
3810 ============
3811 */
3812 bool idMatX::Inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r ) {
3813  idVecX v1, w1;
3814 
3815  assert( numRows == numColumns );
3816  assert( v.GetSize() >= numRows );
3817  assert( w.GetSize() >= numColumns );
3818  assert( r >= 0 && r < numRows && r < numColumns );
3819 
3820  v1.SetData( numRows, VECX_ALLOCA( numRows ) );
3821  w1.SetData( numRows, VECX_ALLOCA( numRows ) );
3822 
3823  // update the row and column to identity
3824  v1 = -v;
3825  w1 = -w;
3826  v1[r] += 1.0f;
3827  w1[r] = 0.0f;
3828 
3829  if ( !Inverse_UpdateRowColumn( v1, w1, r ) ) {
3830  return false;
3831  }
3832 
3833  // physically remove the row and column
3834  Update_Decrement( r );
3835 
3836  return true;
3837 }
3838 
3839 /*
3840 ============
3841 idMatX::Inverse_Solve
3842 
3843  Solve Ax = b with A inverted
3844 ============
3845 */
3846 void idMatX::Inverse_Solve( idVecX &x, const idVecX &b ) const {
3847  Multiply( x, b );
3848 }
3849 
3850 /*
3851 ============
3852 idMatX::LU_Factor
3853 
3854  in-place factorization: LU
3855  L is a triangular matrix stored in the lower triangle.
3856  L has ones on the diagonal that are not stored.
3857  U is a triangular matrix stored in the upper triangle.
3858  If index != NULL partial pivoting is used for numerical stability.
3859  If index != NULL it must point to an array of numRow integers and is used to keep track of the row permutation.
3860  If det != NULL the determinant of the matrix is calculated and stored.
3861 ============
3862 */
3863 bool idMatX::LU_Factor( int *index, float *det ) {
3864  int i, j, k, newi, min;
3865  double s, t, d, w;
3866 
3867  // if partial pivoting should be used
3868  if ( index ) {
3869  for ( i = 0; i < numRows; i++ ) {
3870  index[i] = i;
3871  }
3872  }
3873 
3874  w = 1.0f;
3875  min = Min( numRows, numColumns );
3876  for ( i = 0; i < min; i++ ) {
3877 
3878  newi = i;
3879  s = idMath::Fabs( (*this)[i][i] );
3880 
3881  if ( index ) {
3882  // find the largest absolute pivot
3883  for ( j = i + 1; j < numRows; j++ ) {
3884  t = idMath::Fabs( (*this)[j][i] );
3885  if ( t > s ) {
3886  newi = j;
3887  s = t;
3888  }
3889  }
3890  }
3891 
3892  if ( s == 0.0f ) {
3893  return false;
3894  }
3895 
3896  if ( newi != i ) {
3897 
3898  w = -w;
3899 
3900  // swap index elements
3901  k = index[i];
3902  index[i] = index[newi];
3903  index[newi] = k;
3904 
3905  // swap rows
3906  for ( j = 0; j < numColumns; j++ ) {
3907  t = (*this)[newi][j];
3908  (*this)[newi][j] = (*this)[i][j];
3909  (*this)[i][j] = t;
3910  }
3911  }
3912 
3913  if ( i < numRows ) {
3914  d = 1.0f / (*this)[i][i];
3915  for ( j = i + 1; j < numRows; j++ ) {
3916  (*this)[j][i] *= d;
3917  }
3918  }
3919 
3920  if ( i < min-1 ) {
3921  for ( j = i + 1; j < numRows; j++ ) {
3922  d = (*this)[j][i];
3923  for ( k = i + 1; k < numColumns; k++ ) {
3924  (*this)[j][k] -= d * (*this)[i][k];
3925  }
3926  }
3927  }
3928  }
3929 
3930  if ( det ) {
3931  for ( i = 0; i < numRows; i++ ) {
3932  w *= (*this)[i][i];
3933  }
3934  *det = w;
3935  }
3936 
3937  return true;
3938 }
3939 
3940 /*
3941 ============
3942 idMatX::LU_UpdateRankOne
3943 
3944  Updates the in-place LU factorization to obtain the factors for the matrix: LU + alpha * v * w'
3945 ============
3946 */
3947 bool idMatX::LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index ) {
3948  int i, j, max;
3949  float *y, *z;
3950  double diag, beta, p0, p1, d;
3951 
3952  assert( v.GetSize() >= numColumns );
3953  assert( w.GetSize() >= numRows );
3954 
3955  y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
3956  z = (float *) _alloca16( w.GetSize() * sizeof( float ) );
3957 
3958  if ( index != NULL ) {
3959  for ( i = 0; i < numRows; i++ ) {
3960  y[i] = alpha * v[index[i]];
3961  }
3962  } else {
3963  for ( i = 0; i < numRows; i++ ) {
3964  y[i] = alpha * v[i];
3965  }
3966  }
3967 
3968  memcpy( z, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
3969 
3970  max = Min( numRows, numColumns );
3971  for ( i = 0; i < max; i++ ) {
3972  diag = (*this)[i][i];
3973 
3974  p0 = y[i];
3975  p1 = z[i];
3976  diag += p0 * p1;
3977 
3978  if ( diag == 0.0f ) {
3979  return false;
3980  }
3981 
3982  beta = p1 / diag;
3983 
3984  (*this)[i][i] = diag;
3985 
3986  for ( j = i+1; j < numColumns; j++ ) {
3987 
3988  d = (*this)[i][j];
3989 
3990  d += p0 * z[j];
3991  z[j] -= beta * d;
3992 
3993  (*this)[i][j] = d;
3994  }
3995 
3996  for ( j = i+1; j < numRows; j++ ) {
3997 
3998  d = (*this)[j][i];
3999 
4000  y[j] -= p0 * d;
4001  d += beta * y[j];
4002 
4003  (*this)[j][i] = d;
4004  }
4005  }
4006  return true;
4007 }
4008 
4009 /*
4010 ============
4011 idMatX::LU_UpdateRowColumn
4012 
4013  Updates the in-place LU factorization to obtain the factors for the matrix:
4014 
4015  [ 0 a 0 ]
4016  LU + [ d b e ]
4017  [ 0 c 0 ]
4018 
4019  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
4020 ============
4021 */
4022 bool idMatX::LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index ) {
4023 #if 0
4024 
4025  idVecX s;
4026 
4027  assert( v.GetSize() >= numColumns );
4028  assert( w.GetSize() >= numRows );
4029  assert( r >= 0 && r < numRows && r < numColumns );
4030  assert( w[r] == 0.0f );
4031 
4032  s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
4033  s.Zero();
4034  s[r] = 1.0f;
4035 
4036  if ( !LU_UpdateRankOne( v, s, 1.0f, index ) ) {
4037  return false;
4038  }
4039  if ( !LU_UpdateRankOne( s, w, 1.0f, index ) ) {
4040  return false;
4041  }
4042  return true;
4043 
4044 #else
4045 
4046  int i, j, min, max, rp;
4047  float *y0, *y1, *z0, *z1;
4048  double diag, beta0, beta1, p0, p1, q0, q1, d;
4049 
4050  assert( v.GetSize() >= numColumns );
4051  assert( w.GetSize() >= numRows );
4052  assert( r >= 0 && r < numColumns && r < numRows );
4053  assert( w[r] == 0.0f );
4054 
4055  y0 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
4056  z0 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
4057  y1 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
4058  z1 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
4059 
4060  if ( index != NULL ) {
4061  for ( i = 0; i < numRows; i++ ) {
4062  y0[i] = v[index[i]];
4063  }
4064  rp = r;
4065  for ( i = 0; i < numRows; i++ ) {
4066  if ( index[i] == r ) {
4067  rp = i;
4068  break;
4069  }
4070  }
4071  } else {
4072  memcpy( y0, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
4073  rp = r;
4074  }
4075 
4076  memset( y1, 0, v.GetSize() * sizeof( float ) );
4077  y1[rp] = 1.0f;
4078 
4079  memset( z0, 0, w.GetSize() * sizeof( float ) );
4080  z0[r] = 1.0f;
4081 
4082  memcpy( z1, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
4083 
4084  // update the beginning of the to be updated row and column
4085  min = Min( r, rp );
4086  for ( i = 0; i < min; i++ ) {
4087  p0 = y0[i];
4088  beta1 = z1[i] / (*this)[i][i];
4089 
4090  (*this)[i][r] += p0;
4091  for ( j = i+1; j < numColumns; j++ ) {
4092  z1[j] -= beta1 * (*this)[i][j];
4093  }
4094  for ( j = i+1; j < numRows; j++ ) {
4095  y0[j] -= p0 * (*this)[j][i];
4096  }
4097  (*this)[rp][i] += beta1;
4098  }
4099 
4100  // update the lower right corner starting at r,r
4101  max = Min( numRows, numColumns );
4102  for ( i = min; i < max; i++ ) {
4103  diag = (*this)[i][i];
4104 
4105  p0 = y0[i];
4106  p1 = z0[i];
4107  diag += p0 * p1;
4108 
4109  if ( diag == 0.0f ) {
4110  return false;
4111  }
4112 
4113  beta0 = p1 / diag;
4114 
4115  q0 = y1[i];
4116  q1 = z1[i];
4117  diag += q0 * q1;
4118 
4119  if ( diag == 0.0f ) {
4120  return false;
4121  }
4122 
4123  beta1 = q1 / diag;
4124 
4125  (*this)[i][i] = diag;
4126 
4127  for ( j = i+1; j < numColumns; j++ ) {
4128 
4129  d = (*this)[i][j];
4130 
4131  d += p0 * z0[j];
4132  z0[j] -= beta0 * d;
4133 
4134  d += q0 * z1[j];
4135  z1[j] -= beta1 * d;
4136 
4137  (*this)[i][j] = d;
4138  }
4139 
4140  for ( j = i+1; j < numRows; j++ ) {
4141 
4142  d = (*this)[j][i];
4143 
4144  y0[j] -= p0 * d;
4145  d += beta0 * y0[j];
4146 
4147  y1[j] -= q0 * d;
4148  d += beta1 * y1[j];
4149 
4150  (*this)[j][i] = d;
4151  }
4152  }
4153  return true;
4154 
4155 #endif
4156 }
4157 
4158 /*
4159 ============
4160 idMatX::LU_UpdateIncrement
4161 
4162  Updates the in-place LU factorization to obtain the factors for the matrix:
4163 
4164  [ A a ]
4165  [ c b ]
4166 
4167  where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
4168 ============
4169 */
4170 bool idMatX::LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index ) {
4171  int i, j;
4172  float sum;
4173 
4174  assert( numRows == numColumns );
4175  assert( v.GetSize() >= numRows+1 );
4176  assert( w.GetSize() >= numColumns+1 );
4177 
4178  ChangeSize( numRows+1, numColumns+1, true );
4179 
4180  // add row to L
4181  for ( i = 0; i < numRows - 1; i++ ) {
4182  sum = w[i];
4183  for ( j = 0; j < i; j++ ) {
4184  sum -= (*this)[numRows - 1][j] * (*this)[j][i];
4185  }
4186  (*this)[numRows - 1 ][i] = sum / (*this)[i][i];
4187  }
4188 
4189  // add row to the permutation index
4190  if ( index != NULL ) {
4191  index[numRows - 1] = numRows - 1;
4192  }
4193 
4194  // add column to U
4195  for ( i = 0; i < numRows; i++ ) {
4196  if ( index != NULL ) {
4197  sum = v[index[i]];
4198  } else {
4199  sum = v[i];
4200  }
4201  for ( j = 0; j < i; j++ ) {
4202  sum -= (*this)[i][j] * (*this)[j][numRows - 1];
4203  }
4204  (*this)[i][numRows - 1] = sum;
4205  }
4206 
4207  return true;
4208 }
4209 
4210 /*
4211 ============
4212 idMatX::LU_UpdateDecrement
4213 
4214  Updates the in-place LU factorization to obtain the factors for the matrix with row r and column r removed.
4215  v and w should store the column and row of the original matrix respectively.
4216  If index != NULL then u should store row index[r] of the original matrix. If index == NULL then u = w.
4217 ============
4218 */
4219 bool idMatX::LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index ) {
4220  int i, p;
4221  idVecX v1, w1;
4222 
4223  assert( numRows == numColumns );
4224  assert( v.GetSize() >= numColumns );
4225  assert( w.GetSize() >= numRows );
4226  assert( r >= 0 && r < numRows && r < numColumns );
4227 
4228  v1.SetData( numRows, VECX_ALLOCA( numRows ) );
4229  w1.SetData( numRows, VECX_ALLOCA( numRows ) );
4230 
4231  if ( index != NULL ) {
4232 
4233  // find the pivot row
4234  for ( p = i = 0; i < numRows; i++ ) {
4235  if ( index[i] == r ) {
4236  p = i;
4237  break;
4238  }
4239  }
4240 
4241  // update the row and column to identity
4242  v1 = -v;
4243  w1 = -u;
4244 
4245  if ( p != r ) {
4246  idSwap( v1[index[r]], v1[index[p]] );
4247  idSwap( index[r], index[p] );
4248  }
4249 
4250  v1[r] += 1.0f;
4251  w1[r] = 0.0f;
4252 
4253  if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
4254  return false;
4255  }
4256 
4257  if ( p != r ) {
4258 
4259  if ( idMath::Fabs( u[p] ) < 1e-4f ) {
4260  // NOTE: an additional row interchange is required for numerical stability
4261  }
4262 
4263  // move row index[r] of the original matrix to row index[p] of the original matrix
4264  v1.Zero();
4265  v1[index[p]] = 1.0f;
4266  w1 = u - w;
4267 
4268  if ( !LU_UpdateRankOne( v1, w1, 1.0f, index ) ) {
4269  return false;
4270  }
4271  }
4272 
4273  // remove the row from the permutation index
4274  for ( i = r; i < numRows - 1; i++ ) {
4275  index[i] = index[i+1];
4276  }
4277  for ( i = 0; i < numRows - 1; i++ ) {
4278  if ( index[i] > r ) {
4279  index[i]--;
4280  }
4281  }
4282 
4283  } else {
4284 
4285  v1 = -v;
4286  w1 = -w;
4287  v1[r] += 1.0f;
4288  w1[r] = 0.0f;
4289 
4290  if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
4291  return false;
4292  }
4293  }
4294 
4295  // physically remove the row and column
4296  Update_Decrement( r );
4297 
4298  return true;
4299 }
4300 
4301 /*
4302 ============
4303 idMatX::LU_Solve
4304 
4305  Solve Ax = b with A factored in-place as: LU
4306 ============
4307 */
4308 void idMatX::LU_Solve( idVecX &x, const idVecX &b, const int *index ) const {
4309  int i, j;
4310  double sum;
4311 
4312  assert( x.GetSize() == numColumns && b.GetSize() == numRows );
4313 
4314  // solve L
4315  for ( i = 0; i < numRows; i++ ) {
4316  if ( index != NULL ) {
4317  sum = b[index[i]];
4318  } else {
4319  sum = b[i];
4320  }
4321  for ( j = 0; j < i; j++ ) {
4322  sum -= (*this)[i][j] * x[j];
4323  }
4324  x[i] = sum;
4325  }
4326 
4327  // solve U
4328  for ( i = numRows - 1; i >= 0; i-- ) {
4329  sum = x[i];
4330  for ( j = i + 1; j < numRows; j++ ) {
4331  sum -= (*this)[i][j] * x[j];
4332  }
4333  x[i] = sum / (*this)[i][i];
4334  }
4335 }
4336 
4337 /*
4338 ============
4339 idMatX::LU_Inverse
4340 
4341  Calculates the inverse of the matrix which is factored in-place as LU
4342 ============
4343 */
4344 void idMatX::LU_Inverse( idMatX &inv, const int *index ) const {
4345  int i, j;
4346  idVecX x, b;
4347 
4348  assert( numRows == numColumns );
4349 
4350  x.SetData( numRows, VECX_ALLOCA( numRows ) );
4351  b.SetData( numRows, VECX_ALLOCA( numRows ) );
4352  b.Zero();
4353  inv.SetSize( numRows, numColumns );
4354 
4355  for ( i = 0; i < numRows; i++ ) {
4356 
4357  b[i] = 1.0f;
4358  LU_Solve( x, b, index );
4359  for ( j = 0; j < numRows; j++ ) {
4360  inv[j][i] = x[j];
4361  }
4362  b[i] = 0.0f;
4363  }
4364 }
4365 
4366 /*
4367 ============
4368 idMatX::LU_UnpackFactors
4369 
4370  Unpacks the in-place LU factorization.
4371 ============
4372 */
4373 void idMatX::LU_UnpackFactors( idMatX &L, idMatX &U ) const {
4374  int i, j;
4375 
4376  L.Zero( numRows, numColumns );
4377  U.Zero( numRows, numColumns );
4378  for ( i = 0; i < numRows; i++ ) {
4379  for ( j = 0; j < i; j++ ) {
4380  L[i][j] = (*this)[i][j];
4381  }
4382  L[i][i] = 1.0f;
4383  for ( j = i; j < numColumns; j++ ) {
4384  U[i][j] = (*this)[i][j];
4385  }
4386  }
4387 }
4388 
4389 /*
4390 ============
4391 idMatX::LU_MultiplyFactors
4392 
4393  Multiplies the factors of the in-place LU factorization to form the original matrix.
4394 ============
4395 */
4396 void idMatX::LU_MultiplyFactors( idMatX &m, const int *index ) const {
4397  int r, rp, i, j;
4398  double sum;
4399 
4400  m.SetSize( numRows, numColumns );
4401 
4402  for ( r = 0; r < numRows; r++ ) {
4403 
4404  if ( index != NULL ) {
4405  rp = index[r];
4406  } else {
4407  rp = r;
4408  }
4409 
4410  // calculate row of matrix
4411  for ( i = 0; i < numColumns; i++ ) {
4412  if ( i >= r ) {
4413  sum = (*this)[r][i];
4414  } else {
4415  sum = 0.0f;
4416  }
4417  for ( j = 0; j <= i && j < r; j++ ) {
4418  sum += (*this)[r][j] * (*this)[j][i];
4419  }
4420  m[rp][i] = sum;
4421  }
4422  }
4423 }
4424 
4425 /*
4426 ============
4427 idMatX::QR_Factor
4428 
4429  in-place factorization: QR
4430  Q is an orthogonal matrix represented as a product of Householder matrices stored in the lower triangle and c.
4431  R is a triangular matrix stored in the upper triangle except for the diagonal elements which are stored in d.
4432  The initial matrix has to be square.
4433 ============
4434 */
4435 bool idMatX::QR_Factor( idVecX &c, idVecX &d ) {
4436  int i, j, k;
4437  double scale, s, t, sum;
4438  bool singular = false;
4439 
4440  assert( numRows == numColumns );
4441  assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
4442 
4443  for ( k = 0; k < numRows-1; k++ ) {
4444 
4445  scale = 0.0f;
4446  for ( i = k; i < numRows; i++ ) {
4447  s = idMath::Fabs( (*this)[i][k] );
4448  if ( s > scale ) {
4449  scale = s;
4450  }
4451  }
4452  if ( scale == 0.0f ) {
4453  singular = true;
4454  c[k] = d[k] = 0.0f;
4455  } else {
4456 
4457  s = 1.0f / scale;
4458  for ( i = k; i < numRows; i++ ) {
4459  (*this)[i][k] *= s;
4460  }
4461 
4462  sum = 0.0f;
4463  for ( i = k; i < numRows; i++ ) {
4464  s = (*this)[i][k];
4465  sum += s * s;
4466  }
4467 
4468  s = idMath::Sqrt( sum );
4469  if ( (*this)[k][k] < 0.0f ) {
4470  s = -s;
4471  }
4472  (*this)[k][k] += s;
4473  c[k] = s * (*this)[k][k];
4474  d[k] = -scale * s;
4475 
4476  for ( j = k+1; j < numRows; j++ ) {
4477 
4478  sum = 0.0f;
4479  for ( i = k; i < numRows; i++ ) {
4480  sum += (*this)[i][k] * (*this)[i][j];
4481  }
4482  t = sum / c[k];
4483  for ( i = k; i < numRows; i++ ) {
4484  (*this)[i][j] -= t * (*this)[i][k];
4485  }
4486  }
4487  }
4488  }
4489  d[numRows-1] = (*this)[ (numRows-1) ][ (numRows-1) ];
4490  if ( d[numRows-1] == 0.0f ) {
4491  singular = true;
4492  }
4493 
4494  return !singular;
4495 }
4496 
4497 /*
4498 ============
4499 idMatX::QR_Rotate
4500 
4501  Performs a Jacobi rotation on the rows i and i+1 of the unpacked QR factors.
4502 ============
4503 */
4504 void idMatX::QR_Rotate( idMatX &R, int i, float a, float b ) {
4505  int j;
4506  float f, c, s, w, y;
4507 
4508  if ( a == 0.0f ) {
4509  c = 0.0f;
4510  s = ( b >= 0.0f ) ? 1.0f : -1.0f;
4511  } else if ( idMath::Fabs( a ) > idMath::Fabs( b ) ) {
4512  f = b / a;
4513  c = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
4514  if ( a < 0.0f ) {
4515  c = -c;
4516  }
4517  s = f * c;
4518  } else {
4519  f = a / b;
4520  s = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
4521  if ( b < 0.0f ) {
4522  s = -s;
4523  }
4524  c = f * s;
4525  }
4526  for ( j = i; j < numRows; j++ ) {
4527  y = R[i][j];
4528  w = R[i+1][j];
4529  R[i][j] = c * y - s * w;
4530  R[i+1][j] = s * y + c * w;
4531  }
4532  for ( j = 0; j < numRows; j++ ) {
4533  y = (*this)[j][i];
4534  w = (*this)[j][i+1];
4535  (*this)[j][i] = c * y - s * w;
4536  (*this)[j][i+1] = s * y + c * w;
4537  }
4538 }
4539 
4540 /*
4541 ============
4542 idMatX::QR_UpdateRankOne
4543 
4544  Updates the unpacked QR factorization to obtain the factors for the matrix: QR + alpha * v * w'
4545 ============
4546 */
4547 bool idMatX::QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha ) {
4548  int i, k;
4549  float f;
4550  idVecX u;
4551 
4552  assert( v.GetSize() >= numColumns );
4553  assert( w.GetSize() >= numRows );
4554 
4555  u.SetData( v.GetSize(), VECX_ALLOCA( v.GetSize() ) );
4556  TransposeMultiply( u, v );
4557  u *= alpha;
4558 
4559  for ( k = v.GetSize()-1; k > 0; k-- ) {
4560  if ( u[k] != 0.0f ) {
4561  break;
4562  }
4563  }
4564  for ( i = k-1; i >= 0; i-- ) {
4565  QR_Rotate( R, i, u[i], -u[i+1] );
4566  if ( u[i] == 0.0f ) {
4567  u[i] = idMath::Fabs( u[i+1] );
4568  } else if ( idMath::Fabs( u[i] ) > idMath::Fabs( u[i+1] ) ) {
4569  f = u[i+1] / u[i];
4570  u[i] = idMath::Fabs( u[i] ) * idMath::Sqrt( 1.0f + f * f );
4571  } else {
4572  f = u[i] / u[i+1];
4573  u[i] = idMath::Fabs( u[i+1] ) * idMath::Sqrt( 1.0f + f * f );
4574  }
4575  }
4576  for ( i = 0; i < v.GetSize(); i++ ) {
4577  R[0][i] += u[0] * w[i];
4578  }
4579  for ( i = 0; i < k; i++ ) {
4580  QR_Rotate( R, i, -R[i][i], R[i+1][i] );
4581  }
4582  return true;
4583 }
4584 
4585 /*
4586 ============
4587 idMatX::QR_UpdateRowColumn
4588 
4589  Updates the unpacked QR factorization to obtain the factors for the matrix:
4590 
4591  [ 0 a 0 ]
4592  QR + [ d b e ]
4593  [ 0 c 0 ]
4594 
4595  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
4596 ============
4597 */
4598 bool idMatX::QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
4599  idVecX s;
4600 
4601  assert( v.GetSize() >= numColumns );
4602  assert( w.GetSize() >= numRows );
4603  assert( r >= 0 && r < numRows && r < numColumns );
4604  assert( w[r] == 0.0f );
4605 
4606  s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
4607  s.Zero();
4608  s[r] = 1.0f;
4609 
4610  if ( !QR_UpdateRankOne( R, v, s, 1.0f ) ) {
4611  return false;
4612  }
4613  if ( !QR_UpdateRankOne( R, s, w, 1.0f ) ) {
4614  return false;
4615  }
4616  return true;
4617 }
4618 
4619 /*
4620 ============
4621 idMatX::QR_UpdateIncrement
4622 
4623  Updates the unpacked QR factorization to obtain the factors for the matrix:
4624 
4625  [ A a ]
4626  [ c b ]
4627 
4628  where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
4629 ============
4630 */
4631 bool idMatX::QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w ) {
4632  idVecX v2;
4633 
4634  assert( numRows == numColumns );
4635  assert( v.GetSize() >= numRows+1 );
4636  assert( w.GetSize() >= numColumns+1 );
4637 
4638  ChangeSize( numRows+1, numColumns+1, true );
4639  (*this)[numRows-1][numRows-1] = 1.0f;
4640 
4641  R.ChangeSize( R.numRows+1, R.numColumns+1, true );
4642  R[R.numRows-1][R.numRows-1] = 1.0f;
4643 
4644  v2.SetData( numRows, VECX_ALLOCA( numRows ) );
4645  v2 = v;
4646  v2[numRows-1] -= 1.0f;
4647 
4648  return QR_UpdateRowColumn( R, v2, w, numRows-1 );
4649 }
4650 
4651 /*
4652 ============
4653 idMatX::QR_UpdateDecrement
4654 
4655  Updates the unpacked QR factorization to obtain the factors for the matrix with row r and column r removed.
4656  v and w should store the column and row of the original matrix respectively.
4657 ============
4658 */
4659 bool idMatX::QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
4660  idVecX v1, w1;
4661 
4662  assert( numRows == numColumns );
4663  assert( v.GetSize() >= numRows );
4664  assert( w.GetSize() >= numColumns );
4665  assert( r >= 0 && r < numRows && r < numColumns );
4666 
4667  v1.SetData( numRows, VECX_ALLOCA( numRows ) );
4668  w1.SetData( numRows, VECX_ALLOCA( numRows ) );
4669 
4670  // update the row and column to identity
4671  v1 = -v;
4672  w1 = -w;
4673  v1[r] += 1.0f;
4674  w1[r] = 0.0f;
4675 
4676  if ( !QR_UpdateRowColumn( R, v1, w1, r ) ) {
4677  return false;
4678  }
4679 
4680  // physically remove the row and column
4681  Update_Decrement( r );
4682  R.Update_Decrement( r );
4683 
4684  return true;
4685 }
4686 
4687 /*
4688 ============
4689 idMatX::QR_Solve
4690 
4691  Solve Ax = b with A factored in-place as: QR
4692 ============
4693 */
4694 void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const {
4695  int i, j;
4696  double sum, t;
4697 
4698  assert( numRows == numColumns );
4699  assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
4700  assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
4701 
4702  for ( i = 0; i < numRows; i++ ) {
4703  x[i] = b[i];
4704  }
4705 
4706  // multiply b with transpose of Q
4707  for ( i = 0; i < numRows-1; i++ ) {
4708 
4709  sum = 0.0f;
4710  for ( j = i; j < numRows; j++ ) {
4711  sum += (*this)[j][i] * x[j];
4712  }
4713  t = sum / c[i];
4714  for ( j = i; j < numRows; j++ ) {
4715  x[j] -= t * (*this)[j][i];
4716  }
4717  }
4718 
4719  // backsubstitution with R
4720  for ( i = numRows-1; i >= 0; i-- ) {
4721 
4722  sum = x[i];
4723  for ( j = i + 1; j < numRows; j++ ) {
4724  sum -= (*this)[i][j] * x[j];
4725  }
4726  x[i] = sum / d[i];
4727  }
4728 }
4729 
4730 /*
4731 ============
4732 idMatX::QR_Solve
4733 
4734  Solve Ax = b with A factored as: QR
4735 ============
4736 */
4737 void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const {
4738  int i, j;
4739  double sum;
4740 
4741  assert( numRows == numColumns );
4742 
4743  // multiply b with transpose of Q
4744  TransposeMultiply( x, b );
4745 
4746  // backsubstitution with R
4747  for ( i = numRows-1; i >= 0; i-- ) {
4748 
4749  sum = x[i];
4750  for ( j = i + 1; j < numRows; j++ ) {
4751  sum -= R[i][j] * x[j];
4752  }
4753  x[i] = sum / R[i][i];
4754  }
4755 }
4756 
4757 /*
4758 ============
4759 idMatX::QR_Inverse
4760 
4761  Calculates the inverse of the matrix which is factored in-place as: QR
4762 ============
4763 */
4764 void idMatX::QR_Inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const {
4765  int i, j;
4766  idVecX x, b;
4767 
4768  assert( numRows == numColumns );
4769 
4770  x.SetData( numRows, VECX_ALLOCA( numRows ) );
4771  b.SetData( numRows, VECX_ALLOCA( numRows ) );
4772  b.Zero();
4773  inv.SetSize( numRows, numColumns );
4774 
4775  for ( i = 0; i < numRows; i++ ) {
4776 
4777  b[i] = 1.0f;
4778  QR_Solve( x, b, c, d );
4779  for ( j = 0; j < numRows; j++ ) {
4780  inv[j][i] = x[j];
4781  }
4782  b[i] = 0.0f;
4783  }
4784 }
4785 
4786 /*
4787 ============
4788 idMatX::QR_UnpackFactors
4789 
4790  Unpacks the in-place QR factorization.
4791 ============
4792 */
4793 void idMatX::QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const {
4794  int i, j, k;
4795  double sum;
4796 
4797  Q.Identity( numRows, numColumns );
4798  for ( i = 0; i < numColumns-1; i++ ) {
4799  if ( c[i] == 0.0f ) {
4800  continue;
4801  }
4802  for ( j = 0; j < numRows; j++ ) {
4803  sum = 0.0f;
4804  for ( k = i; k < numColumns; k++ ) {
4805  sum += (*this)[k][i] * Q[j][k];
4806  }
4807  sum /= c[i];
4808  for ( k = i; k < numColumns; k++ ) {
4809  Q[j][k] -= sum * (*this)[k][i];
4810  }
4811  }
4812  }
4813 
4814  R.Zero( numRows, numColumns );
4815  for ( i = 0; i < numRows; i++ ) {
4816  R[i][i] = d[i];
4817  for ( j = i+1; j < numColumns; j++ ) {
4818  R[i][j] = (*this)[i][j];
4819  }
4820  }
4821 }
4822 
4823 /*
4824 ============
4825 idMatX::QR_MultiplyFactors
4826 
4827  Multiplies the factors of the in-place QR factorization to form the original matrix.
4828 ============
4829 */
4830 void idMatX::QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const {
4831  int i, j, k;
4832  double sum;
4833  idMatX Q;
4834 
4835  Q.Identity( numRows, numColumns );
4836  for ( i = 0; i < numColumns-1; i++ ) {
4837  if ( c[i] == 0.0f ) {
4838  continue;
4839  }
4840  for ( j = 0; j < numRows; j++ ) {
4841  sum = 0.0f;
4842  for ( k = i; k < numColumns; k++ ) {
4843  sum += (*this)[k][i] * Q[j][k];
4844  }
4845  sum /= c[i];
4846  for ( k = i; k < numColumns; k++ ) {
4847  Q[j][k] -= sum * (*this)[k][i];
4848  }
4849  }
4850  }
4851 
4852  for ( i = 0; i < numRows; i++ ) {
4853  for ( j = 0; j < numColumns; j++ ) {
4854  sum = Q[i][j] * d[i];
4855  for ( k = 0; k < i; k++ ) {
4856  sum += Q[i][k] * (*this)[j][k];
4857  }
4858  m[i][j] = sum;
4859  }
4860  }
4861 }
4862 
4863 /*
4864 ============
4865 idMatX::Pythag
4866 
4867  Computes (a^2 + b^2)^1/2 without underflow or overflow.
4868 ============
4869 */
4870 float idMatX::Pythag( float a, float b ) const {
4871  double at, bt, ct;
4872 
4873  at = idMath::Fabs( a );
4874  bt = idMath::Fabs( b );
4875  if ( at > bt ) {
4876  ct = bt / at;
4877  return at * idMath::Sqrt( 1.0f + ct * ct );
4878  } else {
4879  if ( bt ) {
4880  ct = at / bt;
4881  return bt * idMath::Sqrt( 1.0f + ct * ct );
4882  } else {
4883  return 0.0f;
4884  }
4885  }
4886 }
4887 
4888 /*
4889 ============
4890 idMatX::SVD_BiDiag
4891 ============
4892 */
4893 void idMatX::SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm ) {
4894  int i, j, k, l;
4895  double f, h, r, g, s, scale;
4896 
4897  anorm = 0.0f;
4898  g = s = scale = 0.0f;
4899  for ( i = 0; i < numColumns; i++ ) {
4900  l = i + 1;
4901  rv1[i] = scale * g;
4902  g = s = scale = 0.0f;
4903  if ( i < numRows ) {
4904  for ( k = i; k < numRows; k++ ) {
4905  scale += idMath::Fabs( (*this)[k][i] );
4906  }
4907  if ( scale ) {
4908  for ( k = i; k < numRows; k++ ) {
4909  (*this)[k][i] /= scale;
4910  s += (*this)[k][i] * (*this)[k][i];
4911  }
4912  f = (*this)[i][i];
4913  g = idMath::Sqrt( s );
4914  if ( f >= 0.0f ) {
4915  g = -g;
4916  }
4917  h = f * g - s;
4918  (*this)[i][i] = f - g;
4919  if ( i != (numColumns-1) ) {
4920  for ( j = l; j < numColumns; j++ ) {
4921  for ( s = 0.0f, k = i; k < numRows; k++ ) {
4922  s += (*this)[k][i] * (*this)[k][j];
4923  }
4924  f = s / h;
4925  for ( k = i; k < numRows; k++ ) {
4926  (*this)[k][j] += f * (*this)[k][i];
4927  }
4928  }
4929  }
4930  for ( k = i; k < numRows; k++ ) {
4931  (*this)[k][i] *= scale;
4932  }
4933  }
4934  }
4935  w[i] = scale * g;
4936  g = s = scale = 0.0f;
4937  if ( i < numRows && i != (numColumns-1) ) {
4938  for ( k = l; k < numColumns; k++ ) {
4939  scale += idMath::Fabs( (*this)[i][k] );
4940  }
4941  if ( scale ) {
4942  for ( k = l; k < numColumns; k++ ) {
4943  (*this)[i][k] /= scale;
4944  s += (*this)[i][k] * (*this)[i][k];
4945  }
4946  f = (*this)[i][l];
4947  g = idMath::Sqrt( s );
4948  if ( f >= 0.0f ) {
4949  g = -g;
4950  }
4951  h = 1.0f / ( f * g - s );
4952  (*this)[i][l] = f - g;
4953  for ( k = l; k < numColumns; k++ ) {
4954  rv1[k] = (*this)[i][k] * h;
4955  }
4956  if ( i != (numRows-1) ) {
4957  for ( j = l; j < numRows; j++ ) {
4958  for ( s = 0.0f, k = l; k < numColumns; k++ ) {
4959  s += (*this)[j][k] * (*this)[i][k];
4960  }
4961  for ( k = l; k < numColumns; k++ ) {
4962  (*this)[j][k] += s * rv1[k];
4963  }
4964  }
4965  }
4966  for ( k = l; k < numColumns; k++ ) {
4967  (*this)[i][k] *= scale;
4968  }
4969  }
4970  }
4971  r = idMath::Fabs( w[i] ) + idMath::Fabs( rv1[i] );
4972  if ( r > anorm ) {
4973  anorm = r;
4974  }
4975  }
4976 }
4977 
4978 /*
4979 ============
4980 idMatX::SVD_InitialWV
4981 ============
4982 */
4983 void idMatX::SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 ) {
4984  int i, j, k, l;
4985  double f, g, s;
4986 
4987  g = 0.0f;
4988  for ( i = (numColumns-1); i >= 0; i-- ) {
4989  l = i + 1;
4990  if ( i < ( numColumns - 1 ) ) {
4991  if ( g ) {
4992  for ( j = l; j < numColumns; j++ ) {
4993  V[j][i] = ((*this)[i][j] / (*this)[i][l]) / g;
4994  }
4995  // double division to reduce underflow
4996  for ( j = l; j < numColumns; j++ ) {
4997  for ( s = 0.0f, k = l; k < numColumns; k++ ) {
4998  s += (*this)[i][k] * V[k][j];
4999  }
5000  for ( k = l; k < numColumns; k++ ) {
5001  V[k][j] += s * V[k][i];
5002  }
5003  }
5004  }
5005  for ( j = l; j < numColumns; j++ ) {
5006  V[i][j] = V[j][i] = 0.0f;
5007  }
5008  }
5009  V[i][i] = 1.0f;
5010  g = rv1[i];
5011  }
5012  for ( i = numColumns - 1 ; i >= 0; i-- ) {
5013  l = i + 1;
5014  g = w[i];
5015  if ( i < (numColumns-1) ) {
5016  for ( j = l; j < numColumns; j++ ) {
5017  (*this)[i][j] = 0.0f;
5018  }
5019  }
5020  if ( g ) {
5021  g = 1.0f / g;
5022  if ( i != (numColumns-1) ) {
5023  for ( j = l; j < numColumns; j++ ) {
5024  for ( s = 0.0f, k = l; k < numRows; k++ ) {
5025  s += (*this)[k][i] * (*this)[k][j];
5026  }
5027  f = (s / (*this)[i][i]) * g;
5028  for ( k = i; k < numRows; k++ ) {
5029  (*this)[k][j] += f * (*this)[k][i];
5030  }
5031  }
5032  }
5033  for ( j = i; j < numRows; j++ ) {
5034  (*this)[j][i] *= g;
5035  }
5036  }
5037  else {
5038  for ( j = i; j < numRows; j++ ) {
5039  (*this)[j][i] = 0.0f;
5040  }
5041  }
5042  (*this)[i][i] += 1.0f;
5043  }
5044 }
5045 
5046 /*
5047 ============
5048 idMatX::SVD_Factor
5049 
5050  in-place factorization: U * Diag(w) * V.Transpose()
5051  known as the Singular Value Decomposition.
5052  U is a column-orthogonal matrix which overwrites the original matrix.
5053  w is a diagonal matrix with all elements >= 0 which are the singular values.
5054  V is the transpose of an orthogonal matrix.
5055 ============
5056 */
5057 bool idMatX::SVD_Factor( idVecX &w, idMatX &V ) {
5058  int flag, i, its, j, jj, k, l, nm;
5059  double c, f, h, s, x, y, z, r, g = 0.0f;
5060  float anorm = 0.0f;
5061  idVecX rv1;
5062 
5063  if ( numRows < numColumns ) {
5064  return false;
5065  }
5066 
5067  rv1.SetData( numColumns, VECX_ALLOCA( numColumns ) );
5068  rv1.Zero();
5069  w.Zero( numColumns );
5070  V.Zero( numColumns, numColumns );
5071 
5072  SVD_BiDiag( w, rv1, anorm );
5073  SVD_InitialWV( w, V, rv1 );
5074 
5075  for ( k = numColumns - 1; k >= 0; k-- ) {
5076  for ( its = 1; its <= 30; its++ ) {
5077  flag = 1;
5078  nm = 0;
5079  for ( l = k; l >= 0; l-- ) {
5080  nm = l - 1;
5081  if ( ( idMath::Fabs( rv1[l] ) + anorm ) == anorm /* idMath::Fabs( rv1[l] ) < idMath::FLT_EPSILON */ ) {
5082  flag = 0;
5083  break;
5084  }
5085  if ( ( idMath::Fabs( w[nm] ) + anorm ) == anorm /* idMath::Fabs( w[nm] ) < idMath::FLT_EPSILON */ ) {
5086  break;
5087  }
5088  }
5089  if ( flag ) {
5090  c = 0.0f;
5091  s = 1.0f;
5092  for ( i = l; i <= k; i++ ) {
5093  f = s * rv1[i];
5094 
5095  if ( ( idMath::Fabs( f ) + anorm ) != anorm /* idMath::Fabs( f ) > idMath::FLT_EPSILON */ ) {
5096  g = w[i];
5097  h = Pythag( f, g );
5098  w[i] = h;
5099  h = 1.0f / h;
5100  c = g * h;
5101  s = -f * h;
5102  for ( j = 0; j < numRows; j++ ) {
5103  y = (*this)[j][nm];
5104  z = (*this)[j][i];
5105  (*this)[j][nm] = y * c + z * s;
5106  (*this)[j][i] = z * c - y * s;
5107  }
5108  }
5109  }
5110  }
5111  z = w[k];
5112  if ( l == k ) {
5113  if ( z < 0.0f ) {
5114  w[k] = -z;
5115  for ( j = 0; j < numColumns; j++ ) {
5116  V[j][k] = -V[j][k];
5117  }
5118  }
5119  break;
5120  }
5121  if ( its == 30 ) {
5122  return false; // no convergence
5123  }
5124  x = w[l];
5125  nm = k - 1;
5126  y = w[nm];
5127  g = rv1[nm];
5128  h = rv1[k];
5129  f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0f * h * y );
5130  g = Pythag( f, 1.0f );
5131  r = ( f >= 0.0f ? g : - g );
5132  f= ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + r ) ) - h ) ) / x;
5133  c = s = 1.0f;
5134  for ( j = l; j <= nm; j++ ) {
5135  i = j + 1;
5136  g = rv1[i];
5137  y = w[i];
5138  h = s * g;
5139  g = c * g;
5140  z = Pythag( f, h );
5141  rv1[j] = z;
5142  c = f / z;
5143  s = h / z;
5144  f = x * c + g * s;
5145  g = g * c - x * s;
5146  h = y * s;
5147  y = y * c;
5148  for ( jj = 0; jj < numColumns; jj++ ) {
5149  x = V[jj][j];
5150  z = V[jj][i];
5151  V[jj][j] = x * c + z * s;
5152  V[jj][i] = z * c - x * s;
5153  }
5154  z = Pythag( f, h );
5155  w[j] = z;
5156  if ( z ) {
5157  z = 1.0f / z;
5158  c = f * z;
5159  s = h * z;
5160  }
5161  f = ( c * g ) + ( s * y );
5162  x = ( c * y ) - ( s * g );
5163  for ( jj = 0; jj < numRows; jj++ ) {
5164  y = (*this)[jj][j];
5165  z = (*this)[jj][i];
5166  (*this)[jj][j] = y * c + z * s;
5167  (*this)[jj][i] = z * c - y * s;
5168  }
5169  }
5170  rv1[l] = 0.0f;
5171  rv1[k] = f;
5172  w[k] = x;
5173  }
5174  }
5175  return true;
5176 }
5177 
5178 /*
5179 ============
5180 idMatX::SVD_Solve
5181 
5182  Solve Ax = b with A factored as: U * Diag(w) * V.Transpose()
5183 ============
5184 */
5185 void idMatX::SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const {
5186  int i, j;
5187  double sum;
5188  idVecX tmp;
5189 
5190  assert( x.GetSize() >= numColumns );
5191  assert( b.GetSize() >= numColumns );
5192  assert( w.GetSize() == numColumns );
5193  assert( V.GetNumRows() == numColumns && V.GetNumColumns() == numColumns );
5194 
5195  tmp.SetData( numColumns, VECX_ALLOCA( numColumns ) );
5196 
5197  for ( i = 0; i < numColumns; i++ ) {
5198  sum = 0.0f;
5199  if ( w[i] >= idMath::FLT_EPSILON ) {
5200  for ( j = 0; j < numRows; j++ ) {
5201  sum += (*this)[j][i] * b[j];
5202  }
5203  sum /= w[i];
5204  }
5205  tmp[i] = sum;
5206  }
5207  for ( i = 0; i < numColumns; i++ ) {
5208  sum = 0.0f;
5209  for ( j = 0; j < numColumns; j++ ) {
5210  sum += V[i][j] * tmp[j];
5211  }
5212  x[i] = sum;
5213  }
5214 }
5215 
5216 /*
5217 ============
5218 idMatX::SVD_Inverse
5219 
5220  Calculates the inverse of the matrix which is factored in-place as: U * Diag(w) * V.Transpose()
5221 ============
5222 */
5223 void idMatX::SVD_Inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const {
5224  int i, j, k;
5225  double wi, sum;
5226  idMatX V2;
5227 
5228  assert( numRows == numColumns );
5229 
5230  V2 = V;
5231 
5232  // V * [diag(1/w[i])]
5233  for ( i = 0; i < numRows; i++ ) {
5234  wi = w[i];
5235  wi = ( wi < idMath::FLT_EPSILON ) ? 0.0f : 1.0f / wi;
5236  for ( j = 0; j < numColumns; j++ ) {
5237  V2[j][i] *= wi;
5238  }
5239  }
5240 
5241  // V * [diag(1/w[i])] * Ut
5242  for ( i = 0; i < numRows; i++ ) {
5243  for ( j = 0; j < numColumns; j++ ) {
5244  sum = V2[i][0] * (*this)[j][0];
5245  for ( k = 1; k < numColumns; k++ ) {
5246  sum += V2[i][k] * (*this)[j][k];
5247  }
5248  inv[i][j] = sum;
5249  }
5250  }
5251 }
5252 
5253 /*
5254 ============
5255 idMatX::SVD_MultiplyFactors
5256 
5257  Multiplies the factors of the in-place SVD factorization to form the original matrix.
5258 ============
5259 */
5260 void idMatX::SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const {
5261  int r, i, j;
5262  double sum;
5263 
5264  m.SetSize( numRows, V.GetNumRows() );
5265 
5266  for ( r = 0; r < numRows; r++ ) {
5267  // calculate row of matrix
5268  if ( w[r] >= idMath::FLT_EPSILON ) {
5269  for ( i = 0; i < V.GetNumRows(); i++ ) {
5270  sum = 0.0f;
5271  for ( j = 0; j < numColumns; j++ ) {
5272  sum += (*this)[r][j] * V[i][j];
5273  }
5274  m[r][i] = sum * w[r];
5275  }
5276  } else {
5277  for ( i = 0; i < V.GetNumRows(); i++ ) {
5278  m[r][i] = 0.0f;
5279  }
5280  }
5281  }
5282 }
5283 
5284 /*
5285 ============
5286 idMatX::Cholesky_Factor
5287 
5288  in-place Cholesky factorization: LL'
5289  L is a triangular matrix stored in the lower triangle.
5290  The upper triangle is not cleared.
5291  The initial matrix has to be symmetric positive definite.
5292 ============
5293 */
5294 bool idMatX::Cholesky_Factor( void ) {
5295  int i, j, k;
5296  float *invSqrt;
5297  double sum;
5298 
5299  assert( numRows == numColumns );
5300 
5301  invSqrt = (float *) _alloca16( numRows * sizeof( float ) );
5302 
5303  for ( i = 0; i < numRows; i++ ) {
5304 
5305  for ( j = 0; j < i; j++ ) {
5306 
5307  sum = (*this)[i][j];
5308  for ( k = 0; k < j; k++ ) {
5309  sum -= (*this)[i][k] * (*this)[j][k];
5310  }
5311  (*this)[i][j] = sum * invSqrt[j];
5312  }
5313 
5314  sum = (*this)[i][i];
5315  for ( k = 0; k < i; k++ ) {
5316  sum -= (*this)[i][k] * (*this)[i][k];
5317  }
5318 
5319  if ( sum <= 0.0f ) {
5320  return false;
5321  }
5322 
5323  invSqrt[i] = idMath::InvSqrt( sum );
5324  (*this)[i][i] = invSqrt[i] * sum;
5325  }
5326  return true;
5327 }
5328 
5329 /*
5330 ============
5331 idMatX::Cholesky_UpdateRankOne
5332 
5333  Updates the in-place Cholesky factorization to obtain the factors for the matrix: LL' + alpha * v * v'
5334  If offset > 0 only the lower right corner starting at (offset, offset) is updated.
5335 ============
5336 */
5337 bool idMatX::Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
5338  int i, j;
5339  float *y;
5340  double diag, invDiag, diagSqr, newDiag, newDiagSqr, beta, p, d;
5341 
5342  assert( numRows == numColumns );
5343  assert( v.GetSize() >= numRows );
5344  assert( offset >= 0 && offset < numRows );
5345 
5346  y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
5347  memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
5348 
5349  for ( i = offset; i < numColumns; i++ ) {
5350  p = y[i];
5351  diag = (*this)[i][i];
5352  invDiag = 1.0f / diag;
5353  diagSqr = diag * diag;
5354  newDiagSqr = diagSqr + alpha * p * p;
5355 
5356  if ( newDiagSqr <= 0.0f ) {
5357  return false;
5358  }
5359 
5360  (*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
5361 
5362  alpha /= newDiagSqr;
5363  beta = p * alpha;
5364  alpha *= diagSqr;
5365 
5366  for ( j = i+1; j < numRows; j++ ) {
5367 
5368  d = (*this)[j][i] * invDiag;
5369 
5370  y[j] -= p * d;
5371  d += beta * y[j];
5372 
5373  (*this)[j][i] = d * newDiag;
5374  }
5375  }
5376  return true;
5377 }
5378 
5379 /*
5380 ============
5381 idMatX::Cholesky_UpdateRowColumn
5382 
5383  Updates the in-place Cholesky factorization to obtain the factors for the matrix:
5384 
5385  [ 0 a 0 ]
5386  LL' + [ a b c ]
5387  [ 0 c 0 ]
5388 
5389  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
5390 ============
5391 */
5392 bool idMatX::Cholesky_UpdateRowColumn( const idVecX &v, int r ) {
5393  int i, j;
5394  double sum;
5395  float *original, *y;
5396  idVecX addSub;
5397 
5398  assert( numRows == numColumns );
5399  assert( v.GetSize() >= numRows );
5400  assert( r >= 0 && r < numRows );
5401 
5402  addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5403 
5404  if ( r == 0 ) {
5405 
5406  if ( numColumns == 1 ) {
5407  double v0 = v[0];
5408  sum = (*this)[0][0];
5409  sum = sum * sum;
5410  sum = sum + v0;
5411  if ( sum <= 0.0f ) {
5412  return false;
5413  }
5414  (*this)[0][0] = idMath::Sqrt( sum );
5415  return true;
5416  }
5417  for ( i = 0; i < numColumns; i++ ) {
5418  addSub[i] = v[i];
5419  }
5420 
5421  } else {
5422 
5423  original = (float *) _alloca16( numColumns * sizeof( float ) );
5424  y = (float *) _alloca16( numColumns * sizeof( float ) );
5425 
5426  // calculate original row/column of matrix
5427  for ( i = 0; i < numRows; i++ ) {
5428  sum = 0.0f;
5429  for ( j = 0; j <= i; j++ ) {
5430  sum += (*this)[r][j] * (*this)[i][j];
5431  }
5432  original[i] = sum;
5433  }
5434 
5435  // solve for y in L * y = original + v
5436  for ( i = 0; i < r; i++ ) {
5437  sum = original[i] + v[i];
5438  for ( j = 0; j < i; j++ ) {
5439  sum -= (*this)[r][j] * (*this)[i][j];
5440  }
5441  (*this)[r][i] = sum / (*this)[i][i];
5442  }
5443 
5444  // if the last row/column of the matrix is updated
5445  if ( r == numColumns - 1 ) {
5446  // only calculate new diagonal
5447  sum = original[r] + v[r];
5448  for ( j = 0; j < r; j++) {
5449  sum -= (*this)[r][j] * (*this)[r][j];
5450  }
5451  if ( sum <= 0.0f ) {
5452  return false;
5453  }
5454  (*this)[r][r] = idMath::Sqrt( sum );
5455  return true;
5456  }
5457 
5458  // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
5459  for ( i = r; i < numColumns; i++ ) {
5460  sum = 0.0f;
5461  for ( j = 0; j <= r; j++ ) {
5462  sum += (*this)[r][j] * (*this)[i][j];
5463  }
5464  addSub[i] = v[i] - ( sum - original[i] );
5465  }
5466  }
5467 
5468  // add row/column to the lower right sub matrix starting at (r, r)
5469 
5470 #if 0
5471 
5472  idVecX v1, v2;
5473  double d;
5474 
5475  v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5476  v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5477 
5478  d = idMath::SQRT_1OVER2;
5479  v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
5480  v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
5481  for ( i = r+1; i < numColumns; i++ ) {
5482  v1[i] = v2[i] = addSub[i] * d;
5483  }
5484 
5485  // update
5486  if ( !Cholesky_UpdateRankOne( v1, 1.0f, r ) ) {
5487  return false;
5488  }
5489  // downdate
5490  if ( !Cholesky_UpdateRankOne( v2, -1.0f, r ) ) {
5491  return false;
5492  }
5493 
5494 #else
5495 
5496  float *v1, *v2;
5497  double diag, invDiag, diagSqr, newDiag, newDiagSqr;
5498  double alpha1, alpha2, beta1, beta2, p1, p2, d;
5499 
5500  v1 = (float *) _alloca16( numColumns * sizeof( float ) );
5501  v2 = (float *) _alloca16( numColumns * sizeof( float ) );
5502 
5503  d = idMath::SQRT_1OVER2;
5504  v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
5505  v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
5506  for ( i = r+1; i < numColumns; i++ ) {
5507  v1[i] = v2[i] = addSub[i] * d;
5508  }
5509 
5510  alpha1 = 1.0f;
5511  alpha2 = -1.0f;
5512 
5513  // simultaneous update/downdate of the sub matrix starting at (r, r)
5514  for ( i = r; i < numColumns; i++ ) {
5515  p1 = v1[i];
5516  diag = (*this)[i][i];
5517  invDiag = 1.0f / diag;
5518  diagSqr = diag * diag;
5519  newDiagSqr = diagSqr + alpha1 * p1 * p1;
5520 
5521  if ( newDiagSqr <= 0.0f ) {
5522  return false;
5523  }
5524 
5525  alpha1 /= newDiagSqr;
5526  beta1 = p1 * alpha1;
5527  alpha1 *= diagSqr;
5528 
5529  p2 = v2[i];
5530  diagSqr = newDiagSqr;
5531  newDiagSqr = diagSqr + alpha2 * p2 * p2;
5532 
5533  if ( newDiagSqr <= 0.0f ) {
5534  return false;
5535  }
5536 
5537  (*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
5538 
5539  alpha2 /= newDiagSqr;
5540  beta2 = p2 * alpha2;
5541  alpha2 *= diagSqr;
5542 
5543  for ( j = i+1; j < numRows; j++ ) {
5544 
5545  d = (*this)[j][i] * invDiag;
5546 
5547  v1[j] -= p1 * d;
5548  d += beta1 * v1[j];
5549 
5550  v2[j] -= p2 * d;
5551  d += beta2 * v2[j];
5552 
5553  (*this)[j][i] = d * newDiag;
5554  }
5555  }
5556 
5557 #endif
5558 
5559  return true;
5560 }
5561 
5562 /*
5563 ============
5564 idMatX::Cholesky_UpdateIncrement
5565 
5566  Updates the in-place Cholesky factorization to obtain the factors for the matrix:
5567 
5568  [ A a ]
5569  [ a b ]
5570 
5571  where: a = v[0,numRows-1], b = v[numRows]
5572 ============
5573 */
5574 bool idMatX::Cholesky_UpdateIncrement( const idVecX &v ) {
5575  int i, j;
5576  float *x;
5577  double sum;
5578 
5579  assert( numRows == numColumns );
5580  assert( v.GetSize() >= numRows+1 );
5581 
5582  ChangeSize( numRows+1, numColumns+1, false );
5583 
5584  x = (float *) _alloca16( numRows * sizeof( float ) );
5585 
5586  // solve for x in L * x = v
5587  for ( i = 0; i < numRows - 1; i++ ) {
5588  sum = v[i];
5589  for ( j = 0; j < i; j++ ) {
5590  sum -= (*this)[i][j] * x[j];
5591  }
5592  x[i] = sum / (*this)[i][i];
5593  }
5594 
5595  // calculate new row of L and calculate the square of the diagonal entry
5596  sum = v[numRows - 1];
5597  for ( i = 0; i < numRows - 1; i++ ) {
5598  (*this)[numRows - 1][i] = x[i];
5599  sum -= x[i] * x[i];
5600  }
5601 
5602  if ( sum <= 0.0f ) {
5603  return false;
5604  }
5605 
5606  // store the diagonal entry
5607  (*this)[numRows - 1][numRows - 1] = idMath::Sqrt( sum );
5608 
5609  return true;
5610 }
5611 
5612 /*
5613 ============
5614 idMatX::Cholesky_UpdateDecrement
5615 
5616  Updates the in-place Cholesky factorization to obtain the factors for the matrix with row r and column r removed.
5617  v should store the row of the original matrix.
5618 ============
5619 */
5620 bool idMatX::Cholesky_UpdateDecrement( const idVecX &v, int r ) {
5621  idVecX v1;
5622 
5623  assert( numRows == numColumns );
5624  assert( v.GetSize() >= numRows );
5625  assert( r >= 0 && r < numRows );
5626 
5627  v1.SetData( numRows, VECX_ALLOCA( numRows ) );
5628 
5629  // update the row and column to identity
5630  v1 = -v;
5631  v1[r] += 1.0f;
5632 
5633  // NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
5634  // untouched when calling Cholesky_UpdateRowColumn in the if statement
5635 #if 0
5636  if ( !Cholesky_UpdateRowColumn( v1, r ) ) {
5637 #else
5638  bool ret = Cholesky_UpdateRowColumn( v1, r );
5639  if ( !ret ) {
5640 #endif
5641  return false;
5642  }
5643 
5644  // physically remove the row and column
5645  Update_Decrement( r );
5646 
5647  return true;
5648 }
5649 
5650 /*
5651 ============
5652 idMatX::Cholesky_Solve
5653 
5654  Solve Ax = b with A factored in-place as: LL'
5655 ============
5656 */
5657 void idMatX::Cholesky_Solve( idVecX &x, const idVecX &b ) const {
5658  int i, j;
5659  double sum;
5660 
5661  assert( numRows == numColumns );
5662  assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
5663 
5664  // solve L
5665  for ( i = 0; i < numRows; i++ ) {
5666  sum = b[i];
5667  for ( j = 0; j < i; j++ ) {
5668  sum -= (*this)[i][j] * x[j];
5669  }
5670  x[i] = sum / (*this)[i][i];
5671  }
5672 
5673  // solve Lt
5674  for ( i = numRows - 1; i >= 0; i-- ) {
5675  sum = x[i];
5676  for ( j = i + 1; j < numRows; j++ ) {
5677  sum -= (*this)[j][i] * x[j];
5678  }
5679  x[i] = sum / (*this)[i][i];
5680  }
5681 }
5682 
5683 /*
5684 ============
5685 idMatX::Cholesky_Inverse
5686 
5687  Calculates the inverse of the matrix which is factored in-place as: LL'
5688 ============
5689 */
5690 void idMatX::Cholesky_Inverse( idMatX &inv ) const {
5691  int i, j;
5692  idVecX x, b;
5693 
5694  assert( numRows == numColumns );
5695 
5696  x.SetData( numRows, VECX_ALLOCA( numRows ) );
5697  b.SetData( numRows, VECX_ALLOCA( numRows ) );
5698  b.Zero();
5699  inv.SetSize( numRows, numColumns );
5700 
5701  for ( i = 0; i < numRows; i++ ) {
5702 
5703  b[i] = 1.0f;
5704  Cholesky_Solve( x, b );
5705  for ( j = 0; j < numRows; j++ ) {
5706  inv[j][i] = x[j];
5707  }
5708  b[i] = 0.0f;
5709  }
5710 }
5711 
5712 /*
5713 ============
5714 idMatX::Cholesky_MultiplyFactors
5715 
5716  Multiplies the factors of the in-place Cholesky factorization to form the original matrix.
5717 ============
5718 */
5719 void idMatX::Cholesky_MultiplyFactors( idMatX &m ) const {
5720  int r, i, j;
5721  double sum;
5722 
5723  m.SetSize( numRows, numColumns );
5724 
5725  for ( r = 0; r < numRows; r++ ) {
5726 
5727  // calculate row of matrix
5728  for ( i = 0; i < numRows; i++ ) {
5729  sum = 0.0f;
5730  for ( j = 0; j <= i && j <= r; j++ ) {
5731  sum += (*this)[r][j] * (*this)[i][j];
5732  }
5733  m[r][i] = sum;
5734  }
5735  }
5736 }
5737 
5738 /*
5739 ============
5740 idMatX::LDLT_Factor
5741 
5742  in-place factorization: LDL'
5743  L is a triangular matrix stored in the lower triangle.
5744  L has ones on the diagonal that are not stored.
5745  D is a diagonal matrix stored on the diagonal.
5746  The upper triangle is not cleared.
5747  The initial matrix has to be symmetric.
5748 ============
5749 */
5750 bool idMatX::LDLT_Factor( void ) {
5751  int i, j, k;
5752  float *v;
5753  double d, sum;
5754 
5755  assert( numRows == numColumns );
5756 
5757  v = (float *) _alloca16( numRows * sizeof( float ) );
5758 
5759  for ( i = 0; i < numRows; i++ ) {
5760 
5761  sum = (*this)[i][i];
5762  for ( j = 0; j < i; j++ ) {
5763  d = (*this)[i][j];
5764  v[j] = (*this)[j][j] * d;
5765  sum -= v[j] * d;
5766  }
5767 
5768  if ( sum == 0.0f ) {
5769  return false;
5770  }
5771 
5772  (*this)[i][i] = sum;
5773  d = 1.0f / sum;
5774 
5775  for ( j = i + 1; j < numRows; j++ ) {
5776  sum = (*this)[j][i];
5777  for ( k = 0; k < i; k++ ) {
5778  sum -= (*this)[j][k] * v[k];
5779  }
5780  (*this)[j][i] = sum * d;
5781  }
5782  }
5783 
5784  return true;
5785 }
5786 
5787 /*
5788 ============
5789 idMatX::LDLT_UpdateRankOne
5790 
5791  Updates the in-place LDL' factorization to obtain the factors for the matrix: LDL' + alpha * v * v'
5792  If offset > 0 only the lower right corner starting at (offset, offset) is updated.
5793 ============
5794 */
5795 bool idMatX::LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
5796  int i, j;
5797  float *y;
5798  double diag, newDiag, beta, p, d;
5799 
5800  assert( numRows == numColumns );
5801  assert( v.GetSize() >= numRows );
5802  assert( offset >= 0 && offset < numRows );
5803 
5804  y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
5805  memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
5806 
5807  for ( i = offset; i < numColumns; i++ ) {
5808  p = y[i];
5809  diag = (*this)[i][i];
5810  (*this)[i][i] = newDiag = diag + alpha * p * p;
5811 
5812  if ( newDiag == 0.0f ) {
5813  return false;
5814  }
5815 
5816  alpha /= newDiag;
5817  beta = p * alpha;
5818  alpha *= diag;
5819 
5820  for ( j = i+1; j < numRows; j++ ) {
5821 
5822  d = (*this)[j][i];
5823 
5824  y[j] -= p * d;
5825  d += beta * y[j];
5826 
5827  (*this)[j][i] = d;
5828  }
5829  }
5830 
5831  return true;
5832 }
5833 
5834 /*
5835 ============
5836 idMatX::LDLT_UpdateRowColumn
5837 
5838  Updates the in-place LDL' factorization to obtain the factors for the matrix:
5839 
5840  [ 0 a 0 ]
5841  LDL' + [ a b c ]
5842  [ 0 c 0 ]
5843 
5844  where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
5845 ============
5846 */
5847 bool idMatX::LDLT_UpdateRowColumn( const idVecX &v, int r ) {
5848  int i, j;
5849  double sum;
5850  float *original, *y;
5851  idVecX addSub;
5852 
5853  assert( numRows == numColumns );
5854  assert( v.GetSize() >= numRows );
5855  assert( r >= 0 && r < numRows );
5856 
5857  addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5858 
5859  if ( r == 0 ) {
5860 
5861  if ( numColumns == 1 ) {
5862  (*this)[0][0] += v[0];
5863  return true;
5864  }
5865  for ( i = 0; i < numColumns; i++ ) {
5866  addSub[i] = v[i];
5867  }
5868 
5869  } else {
5870 
5871  original = (float *) _alloca16( numColumns * sizeof( float ) );
5872  y = (float *) _alloca16( numColumns * sizeof( float ) );
5873 
5874  // calculate original row/column of matrix
5875  for ( i = 0; i < r; i++ ) {
5876  y[i] = (*this)[r][i] * (*this)[i][i];
5877  }
5878  for ( i = 0; i < numColumns; i++ ) {
5879  if ( i < r ) {
5880  sum = (*this)[i][i] * (*this)[r][i];
5881  } else if ( i == r ) {
5882  sum = (*this)[r][r];
5883  } else {
5884  sum = (*this)[r][r] * (*this)[i][r];
5885  }
5886  for ( j = 0; j < i && j < r; j++ ) {
5887  sum += (*this)[i][j] * y[j];
5888  }
5889  original[i] = sum;
5890  }
5891 
5892  // solve for y in L * y = original + v
5893  for ( i = 0; i < r; i++ ) {
5894  sum = original[i] + v[i];
5895  for ( j = 0; j < i; j++ ) {
5896  sum -= (*this)[i][j] * y[j];
5897  }
5898  y[i] = sum;
5899  }
5900 
5901  // calculate new row of L
5902  for ( i = 0; i < r; i++ ) {
5903  (*this)[r][i] = y[i] / (*this)[i][i];
5904  }
5905 
5906  // if the last row/column of the matrix is updated
5907  if ( r == numColumns - 1 ) {
5908  // only calculate new diagonal
5909  sum = original[r] + v[r];
5910  for ( j = 0; j < r; j++ ) {
5911  sum -= (*this)[r][j] * y[j];
5912  }
5913  if ( sum == 0.0f ) {
5914  return false;
5915  }
5916  (*this)[r][r] = sum;
5917  return true;
5918  }
5919 
5920  // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
5921  for ( i = 0; i < r; i++ ) {
5922  y[i] = (*this)[r][i] * (*this)[i][i];
5923  }
5924  for ( i = r; i < numColumns; i++ ) {
5925  if ( i == r ) {
5926  sum = (*this)[r][r];
5927  } else {
5928  sum = (*this)[r][r] * (*this)[i][r];
5929  }
5930  for ( j = 0; j < r; j++ ) {
5931  sum += (*this)[i][j] * y[j];
5932  }
5933  addSub[i] = v[i] - ( sum - original[i] );
5934  }
5935  }
5936 
5937  // add row/column to the lower right sub matrix starting at (r, r)
5938 
5939 #if 0
5940 
5941  idVecX v1, v2;
5942  double d;
5943 
5944  v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5945  v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
5946 
5947  d = idMath::SQRT_1OVER2;
5948  v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
5949  v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
5950  for ( i = r+1; i < numColumns; i++ ) {
5951  v1[i] = v2[i] = addSub[i] * d;
5952  }
5953 
5954  // update
5955  if ( !LDLT_UpdateRankOne( v1, 1.0f, r ) ) {
5956  return false;
5957  }
5958  // downdate
5959  if ( !LDLT_UpdateRankOne( v2, -1.0f, r ) ) {
5960  return false;
5961  }
5962 
5963 #else
5964 
5965  float *v1, *v2;
5966  double d, diag, newDiag, p1, p2, alpha1, alpha2, beta1, beta2;
5967 
5968  v1 = (float *) _alloca16( numColumns * sizeof( float ) );
5969  v2 = (float *) _alloca16( numColumns * sizeof( float ) );
5970 
5971  d = idMath::SQRT_1OVER2;
5972  v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
5973  v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
5974  for ( i = r+1; i < numColumns; i++ ) {
5975  v1[i] = v2[i] = addSub[i] * d;
5976  }
5977 
5978  alpha1 = 1.0f;
5979  alpha2 = -1.0f;
5980 
5981  // simultaneous update/downdate of the sub matrix starting at (r, r)
5982  for ( i = r; i < numColumns; i++ ) {
5983 
5984  diag = (*this)[i][i];
5985  p1 = v1[i];
5986  newDiag = diag + alpha1 * p1 * p1;
5987 
5988  if ( newDiag == 0.0f ) {
5989  return false;
5990  }
5991 
5992  alpha1 /= newDiag;
5993  beta1 = p1 * alpha1;
5994  alpha1 *= diag;
5995 
5996  diag = newDiag;
5997  p2 = v2[i];
5998  newDiag = diag + alpha2 * p2 * p2;
5999 
6000  if ( newDiag == 0.0f ) {
6001  return false;
6002  }
6003 
6004  alpha2 /= newDiag;
6005  beta2 = p2 * alpha2;
6006  alpha2 *= diag;
6007 
6008  (*this)[i][i] = newDiag;
6009 
6010  for ( j = i+1; j < numRows; j++ ) {
6011 
6012  d = (*this)[j][i];
6013 
6014  v1[j] -= p1 * d;
6015  d += beta1 * v1[j];
6016 
6017  v2[j] -= p2 * d;
6018  d += beta2 * v2[j];
6019 
6020  (*this)[j][i] = d;
6021  }
6022  }
6023 
6024 #endif
6025 
6026  return true;
6027 }
6028 
6029 /*
6030 ============
6031 idMatX::LDLT_UpdateIncrement
6032 
6033  Updates the in-place LDL' factorization to obtain the factors for the matrix:
6034 
6035  [ A a ]
6036  [ a b ]
6037 
6038  where: a = v[0,numRows-1], b = v[numRows]
6039 ============
6040 */
6041 bool idMatX::LDLT_UpdateIncrement( const idVecX &v ) {
6042  int i, j;
6043  float *x;
6044  double sum, d;
6045 
6046  assert( numRows == numColumns );
6047  assert( v.GetSize() >= numRows+1 );
6048 
6049  ChangeSize( numRows+1, numColumns+1, false );
6050 
6051  x = (float *) _alloca16( numRows * sizeof( float ) );
6052 
6053  // solve for x in L * x = v
6054  for ( i = 0; i < numRows - 1; i++ ) {
6055  sum = v[i];
6056  for ( j = 0; j < i; j++ ) {
6057  sum -= (*this)[i][j] * x[j];
6058  }
6059  x[i] = sum;
6060  }
6061 
6062  // calculate new row of L and calculate the diagonal entry
6063  sum = v[numRows - 1];
6064  for ( i = 0; i < numRows - 1; i++ ) {
6065  (*this)[numRows - 1][i] = d = x[i] / (*this)[i][i];
6066  sum -= d * x[i];
6067  }
6068 
6069  if ( sum == 0.0f ) {
6070  return false;
6071  }
6072 
6073  // store the diagonal entry
6074  (*this)[numRows - 1][numRows - 1] = sum;
6075 
6076  return true;
6077 }
6078 
6079 /*
6080 ============
6081 idMatX::LDLT_UpdateDecrement
6082 
6083  Updates the in-place LDL' factorization to obtain the factors for the matrix with row r and column r removed.
6084  v should store the row of the original matrix.
6085 ============
6086 */
6087 bool idMatX::LDLT_UpdateDecrement( const idVecX &v, int r ) {
6088  idVecX v1;
6089 
6090  assert( numRows == numColumns );
6091  assert( v.GetSize() >= numRows );
6092  assert( r >= 0 && r < numRows );
6093 
6094  v1.SetData( numRows, VECX_ALLOCA( numRows ) );
6095 
6096  // update the row and column to identity
6097  v1 = -v;
6098  v1[r] += 1.0f;
6099 
6100  // NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
6101  // untouched when calling LDLT_UpdateRowColumn in the if statement
6102 #if 0
6103  if ( !LDLT_UpdateRowColumn( v1, r ) ) {
6104 #else
6105  bool ret = LDLT_UpdateRowColumn( v1, r );
6106  if ( !ret ) {
6107 #endif
6108  return false;
6109  }
6110 
6111  // physically remove the row and column
6112  Update_Decrement( r );
6113 
6114  return true;
6115 }
6116 
6117 /*
6118 ============
6119 idMatX::LDLT_Solve
6120 
6121  Solve Ax = b with A factored in-place as: LDL'
6122 ============
6123 */
6124 void idMatX::LDLT_Solve( idVecX &x, const idVecX &b ) const {
6125  int i, j;
6126  double sum;
6127 
6128  assert( numRows == numColumns );
6129  assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
6130 
6131  // solve L
6132  for ( i = 0; i < numRows; i++ ) {
6133  sum = b[i];
6134  for ( j = 0; j < i; j++ ) {
6135  sum -= (*this)[i][j] * x[j];
6136  }
6137  x[i] = sum;
6138  }
6139 
6140  // solve D
6141  for ( i = 0; i < numRows; i++ ) {
6142  x[i] /= (*this)[i][i];
6143  }
6144 
6145  // solve Lt
6146  for ( i = numRows - 2; i >= 0; i-- ) {
6147  sum = x[i];
6148  for ( j = i + 1; j < numRows; j++ ) {
6149  sum -= (*this)[j][i] * x[j];
6150  }
6151  x[i] = sum;
6152  }
6153 }
6154 
6155 /*
6156 ============
6157 idMatX::LDLT_Inverse
6158 
6159  Calculates the inverse of the matrix which is factored in-place as: LDL'
6160 ============
6161 */
6162 void idMatX::LDLT_Inverse( idMatX &inv ) const {
6163  int i, j;
6164  idVecX x, b;
6165 
6166  assert( numRows == numColumns );
6167 
6168  x.SetData( numRows, VECX_ALLOCA( numRows ) );
6169  b.SetData( numRows, VECX_ALLOCA( numRows ) );
6170  b.Zero();
6171  inv.SetSize( numRows, numColumns );
6172 
6173  for ( i = 0; i < numRows; i++ ) {
6174 
6175  b[i] = 1.0f;
6176  LDLT_Solve( x, b );
6177  for ( j = 0; j < numRows; j++ ) {
6178  inv[j][i] = x[j];
6179  }
6180  b[i] = 0.0f;
6181  }
6182 }
6183 
6184 /*
6185 ============
6186 idMatX::LDLT_UnpackFactors
6187 
6188  Unpacks the in-place LDL' factorization.
6189 ============
6190 */
6191 void idMatX::LDLT_UnpackFactors( idMatX &L, idMatX &D ) const {
6192  int i, j;
6193 
6194  L.Zero( numRows, numColumns );
6195  D.Zero( numRows, numColumns );
6196  for ( i = 0; i < numRows; i++ ) {
6197  for ( j = 0; j < i; j++ ) {
6198  L[i][j] = (*this)[i][j];
6199  }
6200  L[i][i] = 1.0f;
6201  D[i][i] = (*this)[i][i];
6202  }
6203 }
6204 
6205 /*
6206 ============
6207 idMatX::LDLT_MultiplyFactors
6208 
6209  Multiplies the factors of the in-place LDL' factorization to form the original matrix.
6210 ============
6211 */
6212 void idMatX::LDLT_MultiplyFactors( idMatX &m ) const {
6213  int r, i, j;
6214  float *v;
6215  double sum;
6216 
6217  v = (float *) _alloca16( numRows * sizeof( float ) );
6218  m.SetSize( numRows, numColumns );
6219 
6220  for ( r = 0; r < numRows; r++ ) {
6221 
6222  // calculate row of matrix
6223  for ( i = 0; i < r; i++ ) {
6224  v[i] = (*this)[r][i] * (*this)[i][i];
6225  }
6226  for ( i = 0; i < numColumns; i++ ) {
6227  if ( i < r ) {
6228  sum = (*this)[i][i] * (*this)[r][i];
6229  } else if ( i == r ) {
6230  sum = (*this)[r][r];
6231  } else {
6232  sum = (*this)[r][r] * (*this)[i][r];
6233  }
6234  for ( j = 0; j < i && j < r; j++ ) {
6235  sum += (*this)[i][j] * v[j];
6236  }
6237  m[r][i] = sum;
6238  }
6239  }
6240 }
6241 
6242 /*
6243 ============
6244 idMatX::TriDiagonal_ClearTriangles
6245 ============
6246 */
6247 void idMatX::TriDiagonal_ClearTriangles( void ) {
6248  int i, j;
6249 
6250  assert( numRows == numColumns );
6251  for ( i = 0; i < numRows-2; i++ ) {
6252  for ( j = i+2; j < numColumns; j++ ) {
6253  (*this)[i][j] = 0.0f;
6254  (*this)[j][i] = 0.0f;
6255  }
6256  }
6257 }
6258 
6259 /*
6260 ============
6261 idMatX::TriDiagonal_Solve
6262 
6263  Solve Ax = b with A being tridiagonal.
6264 ============
6265 */
6266 bool idMatX::TriDiagonal_Solve( idVecX &x, const idVecX &b ) const {
6267  int i;
6268  float d;
6269  idVecX tmp;
6270 
6271  assert( numRows == numColumns );
6272  assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
6273 
6274  tmp.SetData( numRows, VECX_ALLOCA( numRows ) );
6275 
6276  d = (*this)[0][0];
6277  if ( d == 0.0f ) {
6278  return false;
6279  }
6280  d = 1.0f / d;
6281  x[0] = b[0] * d;
6282  for ( i = 1; i < numRows; i++ ) {
6283  tmp[i] = (*this)[i-1][i] * d;
6284  d = (*this)[i][i] - (*this)[i][i-1] * tmp[i];
6285  if ( d == 0.0f ) {
6286  return false;
6287  }
6288  d = 1.0f / d;
6289  x[i] = ( b[i] - (*this)[i][i-1] * x[i-1] ) * d;
6290  }
6291  for ( i = numRows - 2; i >= 0; i-- ) {
6292  x[i] -= tmp[i+1] * x[i+1];
6293  }
6294  return true;
6295 }
6296 
6297 /*
6298 ============
6299 idMatX::TriDiagonal_Inverse
6300 
6301  Calculates the inverse of a tri-diagonal matrix.
6302 ============
6303 */
6304 void idMatX::TriDiagonal_Inverse( idMatX &inv ) const {
6305  int i, j;
6306  idVecX x, b;
6307 
6308  assert( numRows == numColumns );
6309 
6310  x.SetData( numRows, VECX_ALLOCA( numRows ) );
6311  b.SetData( numRows, VECX_ALLOCA( numRows ) );
6312  b.Zero();
6313  inv.SetSize( numRows, numColumns );
6314 
6315  for ( i = 0; i < numRows; i++ ) {
6316 
6317  b[i] = 1.0f;
6318  TriDiagonal_Solve( x, b );
6319  for ( j = 0; j < numRows; j++ ) {
6320  inv[j][i] = x[j];
6321  }
6322  b[i] = 0.0f;
6323  }
6324 }
6325 
6326 /*
6327 ============
6328 idMatX::HouseholderReduction
6329 
6330  Householder reduction to symmetric tri-diagonal form.
6331  The original matrix is replaced by an orthogonal matrix effecting the accumulated householder transformations.
6332  The diagonal elements of the diagonal matrix are stored in diag.
6333  The off-diagonal elements of the diagonal matrix are stored in subd.
6334  The initial matrix has to be symmetric.
6335 ============
6336 */
6337 void idMatX::HouseholderReduction( idVecX &diag, idVecX &subd ) {
6338  int i0, i1, i2, i3;
6339  float h, f, g, invH, halfFdivH, scale, invScale, sum;
6340 
6341  assert( numRows == numColumns );
6342 
6343  diag.SetSize( numRows );
6344  subd.SetSize( numRows );
6345 
6346  for ( i0 = numRows-1, i3 = numRows-2; i0 >= 1; i0--, i3-- ) {
6347  h = 0.0f;
6348  scale = 0.0f;
6349 
6350  if ( i3 > 0 ) {
6351  for ( i2 = 0; i2 <= i3; i2++ ) {
6352  scale += idMath::Fabs( (*this)[i0][i2] );
6353  }
6354  if ( scale == 0 ) {
6355  subd[i0] = (*this)[i0][i3];
6356  } else {
6357  invScale = 1.0f / scale;
6358  for (i2 = 0; i2 <= i3; i2++)
6359  {
6360  (*this)[i0][i2] *= invScale;
6361  h += (*this)[i0][i2] * (*this)[i0][i2];
6362  }
6363  f = (*this)[i0][i3];
6364  g = idMath::Sqrt( h );
6365  if ( f > 0.0f ) {
6366  g = -g;
6367  }
6368  subd[i0] = scale * g;
6369  h -= f * g;
6370  (*this)[i0][i3] = f - g;
6371  f = 0.0f;
6372  invH = 1.0f / h;
6373  for (i1 = 0; i1 <= i3; i1++) {
6374  (*this)[i1][i0] = (*this)[i0][i1] * invH;
6375  g = 0.0f;
6376  for (i2 = 0; i2 <= i1; i2++) {
6377  g += (*this)[i1][i2] * (*this)[i0][i2];
6378  }
6379  for (i2 = i1+1; i2 <= i3; i2++) {
6380  g += (*this)[i2][i1] * (*this)[i0][i2];
6381  }
6382  subd[i1] = g * invH;
6383  f += subd[i1] * (*this)[i0][i1];
6384  }
6385  halfFdivH = 0.5f * f * invH;
6386  for ( i1 = 0; i1 <= i3; i1++ ) {
6387  f = (*this)[i0][i1];
6388  g = subd[i1] - halfFdivH * f;
6389  subd[i1] = g;
6390  for ( i2 = 0; i2 <= i1; i2++ ) {
6391  (*this)[i1][i2] -= f * subd[i2] + g * (*this)[i0][i2];
6392  }
6393  }
6394  }
6395  } else {
6396  subd[i0] = (*this)[i0][i3];
6397  }
6398 
6399  diag[i0] = h;
6400  }
6401 
6402  diag[0] = 0.0f;
6403  subd[0] = 0.0f;
6404  for ( i0 = 0, i3 = -1; i0 <= numRows-1; i0++, i3++ ) {
6405  if ( diag[i0] ) {
6406  for ( i1 = 0; i1 <= i3; i1++ ) {
6407  sum = 0.0f;
6408  for (i2 = 0; i2 <= i3; i2++) {
6409  sum += (*this)[i0][i2] * (*this)[i2][i1];
6410  }
6411  for ( i2 = 0; i2 <= i3; i2++ ) {
6412  (*this)[i2][i1] -= sum * (*this)[i2][i0];
6413  }
6414  }
6415  }
6416  diag[i0] = (*this)[i0][i0];
6417  (*this)[i0][i0] = 1.0f;
6418  for ( i1 = 0; i1 <= i3; i1++ ) {
6419  (*this)[i1][i0] = 0.0f;
6420  (*this)[i0][i1] = 0.0f;
6421  }
6422  }
6423 
6424  // re-order
6425  for ( i0 = 1, i3 = 0; i0 < numRows; i0++, i3++ ) {
6426  subd[i3] = subd[i0];
6427  }
6428  subd[numRows-1] = 0.0f;
6429 }
6430 
6431 /*
6432 ============
6433 idMatX::QL
6434 
6435  QL algorithm with implicit shifts to determine the eigenvalues and eigenvectors of a symmetric tri-diagonal matrix.
6436  diag contains the diagonal elements of the symmetric tri-diagonal matrix on input and is overwritten with the eigenvalues.
6437  subd contains the off-diagonal elements of the symmetric tri-diagonal matrix and is destroyed.
6438  This matrix has to be either the identity matrix to determine the eigenvectors for a symmetric tri-diagonal matrix,
6439  or the matrix returned by the Householder reduction to determine the eigenvalues for the original symmetric matrix.
6440 ============
6441 */
6442 bool idMatX::QL( idVecX &diag, idVecX &subd ) {
6443  const int maxIter = 32;
6444  int i0, i1, i2, i3;
6445  float a, b, f, g, r, p, s, c;
6446 
6447  assert( numRows == numColumns );
6448 
6449  for ( i0 = 0; i0 < numRows; i0++ ) {
6450  for ( i1 = 0; i1 < maxIter; i1++ ) {
6451  for ( i2 = i0; i2 <= numRows - 2; i2++ ) {
6452  a = idMath::Fabs( diag[i2] ) + idMath::Fabs( diag[i2+1] );
6453  if ( idMath::Fabs( subd[i2] ) + a == a ) {
6454  break;
6455  }
6456  }
6457  if ( i2 == i0 ) {
6458  break;
6459  }
6460 
6461  g = ( diag[i0+1] - diag[i0] ) / ( 2.0f * subd[i0] );
6462  r = idMath::Sqrt( g * g + 1.0f );
6463  if ( g < 0.0f ) {
6464  g = diag[i2] - diag[i0] + subd[i0] / ( g - r );
6465  } else {
6466  g = diag[i2] - diag[i0] + subd[i0] / ( g + r );
6467  }
6468  s = 1.0f;
6469  c = 1.0f;
6470  p = 0.0f;
6471  for ( i3 = i2 - 1; i3 >= i0; i3-- ) {
6472  f = s * subd[i3];
6473  b = c * subd[i3];
6474  if ( idMath::Fabs( f ) >= idMath::Fabs( g ) ) {
6475  c = g / f;
6476  r = idMath::Sqrt( c * c + 1.0f );
6477  subd[i3+1] = f * r;
6478  s = 1.0f / r;
6479  c *= s;
6480  } else {
6481  s = f / g;
6482  r = idMath::Sqrt( s * s + 1.0f );
6483  subd[i3+1] = g * r;
6484  c = 1.0f / r;
6485  s *= c;
6486  }
6487  g = diag[i3+1] - p;
6488  r = ( diag[i3] - g ) * s + 2.0f * b * c;
6489  p = s * r;
6490  diag[i3+1] = g + p;
6491  g = c * r - b;
6492 
6493  for ( int i4 = 0; i4 < numRows; i4++ ) {
6494  f = (*this)[i4][i3+1];
6495  (*this)[i4][i3+1] = s * (*this)[i4][i3] + c * f;
6496  (*this)[i4][i3] = c * (*this)[i4][i3] - s * f;
6497  }
6498  }
6499  diag[i0] -= p;
6500  subd[i0] = g;
6501  subd[i2] = 0.0f;
6502  }
6503  if ( i1 == maxIter ) {
6504  return false;
6505  }
6506  }
6507  return true;
6508 }
6509 
6510 /*
6511 ============
6512 idMatX::Eigen_SolveSymmetricTriDiagonal
6513 
6514  Determine eigen values and eigen vectors for a symmetric tri-diagonal matrix.
6515  The eigen values are stored in 'eigenValues'.
6516  Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
6517  The initial matrix has to be symmetric tri-diagonal.
6518 ============
6519 */
6520 bool idMatX::Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues ) {
6521  int i;
6522  idVecX subd;
6523 
6524  assert( numRows == numColumns );
6525 
6526  subd.SetData( numRows, VECX_ALLOCA( numRows ) );
6527  eigenValues.SetSize( numRows );
6528 
6529  for ( i = 0; i < numRows-1; i++ ) {
6530  eigenValues[i] = (*this)[i][i];
6531  subd[i] = (*this)[i+1][i];
6532  }
6533  eigenValues[numRows-1] = (*this)[numRows-1][numRows-1];
6534 
6535  Identity();
6536 
6537  return QL( eigenValues, subd );
6538 }
6539 
6540 /*
6541 ============
6542 idMatX::Eigen_SolveSymmetric
6543 
6544  Determine eigen values and eigen vectors for a symmetric matrix.
6545  The eigen values are stored in 'eigenValues'.
6546  Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
6547  The initial matrix has to be symmetric.
6548 ============
6549 */
6550 bool idMatX::Eigen_SolveSymmetric( idVecX &eigenValues ) {
6551  idVecX subd;
6552 
6553  assert( numRows == numColumns );
6554 
6555  subd.SetData( numRows, VECX_ALLOCA( numRows ) );
6556  eigenValues.SetSize( numRows );
6557 
6558  HouseholderReduction( eigenValues, subd );
6559  return QL( eigenValues, subd );
6560 }
6561 
6562 /*
6563 ============
6564 idMatX::HessenbergReduction
6565 
6566  Reduction to Hessenberg form.
6567 ============
6568 */
6569 void idMatX::HessenbergReduction( idMatX &H ) {
6570  int i, j, m;
6571  int low = 0;
6572  int high = numRows - 1;
6573  float scale, f, g, h;
6574  idVecX v;
6575 
6576  v.SetData( numRows, VECX_ALLOCA( numRows ) );
6577 
6578  for ( m = low + 1; m <= high - 1; m++ ) {
6579 
6580  scale = 0.0f;
6581  for ( i = m; i <= high; i++ ) {
6582  scale = scale + idMath::Fabs( H[i][m-1] );
6583  }
6584  if ( scale != 0.0f ) {
6585 
6586  // compute Householder transformation.
6587  h = 0.0f;
6588  for ( i = high; i >= m; i-- ) {
6589  v[i] = H[i][m-1] / scale;
6590  h += v[i] * v[i];
6591  }
6592  g = idMath::Sqrt( h );
6593  if ( v[m] > 0.0f ) {
6594  g = -g;
6595  }
6596  h = h - v[m] * g;
6597  v[m] = v[m] - g;
6598 
6599  // apply Householder similarity transformation
6600  // H = (I-u*u'/h)*H*(I-u*u')/h)
6601  for ( j = m; j < numRows; j++) {
6602  f = 0.0f;
6603  for ( i = high; i >= m; i-- ) {
6604  f += v[i] * H[i][j];
6605  }
6606  f = f / h;
6607  for ( i = m; i <= high; i++ ) {
6608  H[i][j] -= f * v[i];
6609  }
6610  }
6611 
6612  for ( i = 0; i <= high; i++ ) {
6613  f = 0.0f;
6614  for ( j = high; j >= m; j-- ) {
6615  f += v[j] * H[i][j];
6616  }
6617  f = f / h;
6618  for ( j = m; j <= high; j++ ) {
6619  H[i][j] -= f * v[j];
6620  }
6621  }
6622  v[m] = scale * v[m];
6623  H[m][m-1] = scale * g;
6624  }
6625  }
6626 
6627  // accumulate transformations
6628  Identity();
6629  for ( int m = high - 1; m >= low + 1; m-- ) {
6630  if ( H[m][m-1] != 0.0f ) {
6631  for ( i = m + 1; i <= high; i++ ) {
6632  v[i] = H[i][m-1];
6633  }
6634  for ( j = m; j <= high; j++ ) {
6635  g = 0.0f;
6636  for ( i = m; i <= high; i++ ) {
6637  g += v[i] * (*this)[i][j];
6638  }
6639  // float division to avoid possible underflow
6640  g = ( g / v[m] ) / H[m][m-1];
6641  for ( i = m; i <= high; i++ ) {
6642  (*this)[i][j] += g * v[i];
6643  }
6644  }
6645  }
6646  }
6647 }
6648 
6649 /*
6650 ============
6651 idMatX::ComplexDivision
6652 
6653  Complex scalar division.
6654 ============
6655 */
6656 void idMatX::ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi ) {
6657  float r, d;
6658  if ( idMath::Fabs( yr ) > idMath::Fabs( yi ) ) {
6659  r = yi / yr;
6660  d = yr + r * yi;
6661  cdivr = ( xr + r * xi ) / d;
6662  cdivi = ( xi - r * xr ) / d;
6663  } else {
6664  r = yr / yi;
6665  d = yi + r * yr;
6666  cdivr = ( r * xr + xi ) / d;
6667  cdivi = ( r * xi - xr ) / d;
6668  }
6669 }
6670 
6671 /*
6672 ============
6673 idMatX::HessenbergToRealSchur
6674 
6675  Reduction from Hessenberg to real Schur form.
6676 ============
6677 */
6678 bool idMatX::HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
6679  int i, j, k;
6680  int n = numRows - 1;
6681  int low = 0;
6682  int high = numRows - 1;
6683  float eps = 2e-16f, exshift = 0.0f;
6684  float p = 0.0f, q = 0.0f, r = 0.0f, s = 0.0f, z = 0.0f, t, w, x, y;
6685 
6686  // store roots isolated by balanc and compute matrix norm
6687  float norm = 0.0f;
6688  for ( i = 0; i < numRows; i++ ) {
6689  if ( i < low || i > high ) {
6690  realEigenValues[i] = H[i][i];
6691  imaginaryEigenValues[i] = 0.0f;
6692  }
6693  for ( j = Max( i - 1, 0 ); j < numRows; j++ ) {
6694  norm = norm + idMath::Fabs( H[i][j] );
6695  }
6696  }
6697 
6698  int iter = 0;
6699  while( n >= low ) {
6700 
6701  // look for single small sub-diagonal element
6702  int l = n;
6703  while ( l > low ) {
6704  s = idMath::Fabs( H[l-1][l-1] ) + idMath::Fabs( H[l][l] );
6705  if ( s == 0.0f ) {
6706  s = norm;
6707  }
6708  if ( idMath::Fabs( H[l][l-1] ) < eps * s ) {
6709  break;
6710  }
6711  l--;
6712  }
6713 
6714  // check for convergence
6715  if ( l == n ) { // one root found
6716  H[n][n] = H[n][n] + exshift;
6717  realEigenValues[n] = H[n][n];
6718  imaginaryEigenValues[n] = 0.0f;
6719  n--;
6720  iter = 0;
6721  } else if ( l == n-1 ) { // two roots found
6722  w = H[n][n-1] * H[n-1][n];
6723  p = ( H[n-1][n-1] - H[n][n] ) / 2.0f;
6724  q = p * p + w;
6725  z = idMath::Sqrt( idMath::Fabs( q ) );
6726  H[n][n] = H[n][n] + exshift;
6727  H[n-1][n-1] = H[n-1][n-1] + exshift;
6728  x = H[n][n];
6729 
6730  if ( q >= 0.0f ) { // real pair
6731  if ( p >= 0.0f ) {
6732  z = p + z;
6733  } else {
6734  z = p - z;
6735  }
6736  realEigenValues[n-1] = x + z;
6737  realEigenValues[n] = realEigenValues[n-1];
6738  if ( z != 0.0f ) {
6739  realEigenValues[n] = x - w / z;
6740  }
6741  imaginaryEigenValues[n-1] = 0.0f;
6742  imaginaryEigenValues[n] = 0.0f;
6743  x = H[n][n-1];
6744  s = idMath::Fabs( x ) + idMath::Fabs( z );
6745  p = x / s;
6746  q = z / s;
6747  r = idMath::Sqrt( p * p + q * q );
6748  p = p / r;
6749  q = q / r;
6750 
6751  // modify row
6752  for ( j = n-1; j < numRows; j++ ) {
6753  z = H[n-1][j];
6754  H[n-1][j] = q * z + p * H[n][j];
6755  H[n][j] = q * H[n][j] - p * z;
6756  }
6757 
6758  // modify column
6759  for ( i = 0; i <= n; i++ ) {
6760  z = H[i][n-1];
6761  H[i][n-1] = q * z + p * H[i][n];
6762  H[i][n] = q * H[i][n] - p * z;
6763  }
6764 
6765  // accumulate transformations
6766  for ( i = low; i <= high; i++ ) {
6767  z = (*this)[i][n-1];
6768  (*this)[i][n-1] = q * z + p * (*this)[i][n];
6769  (*this)[i][n] = q * (*this)[i][n] - p * z;
6770  }
6771  } else { // complex pair
6772  realEigenValues[n-1] = x + p;
6773  realEigenValues[n] = x + p;
6774  imaginaryEigenValues[n-1] = z;
6775  imaginaryEigenValues[n] = -z;
6776  }
6777  n = n - 2;
6778  iter = 0;
6779 
6780  } else { // no convergence yet
6781 
6782  // form shift
6783  x = H[n][n];
6784  y = 0.0f;
6785  w = 0.0f;
6786  if ( l < n ) {
6787  y = H[n-1][n-1];
6788  w = H[n][n-1] * H[n-1][n];
6789  }
6790 
6791  // Wilkinson's original ad hoc shift
6792  if ( iter == 10 ) {
6793  exshift += x;
6794  for ( i = low; i <= n; i++ ) {
6795  H[i][i] -= x;
6796  }
6797  s = idMath::Fabs( H[n][n-1] ) + idMath::Fabs( H[n-1][n-2] );
6798  x = y = 0.75f * s;
6799  w = -0.4375f * s * s;
6800  }
6801 
6802  // new ad hoc shift
6803  if ( iter == 30 ) {
6804  s = ( y - x ) / 2.0f;
6805  s = s * s + w;
6806  if ( s > 0 ) {
6807  s = idMath::Sqrt( s );
6808  if ( y < x ) {
6809  s = -s;
6810  }
6811  s = x - w / ( ( y - x ) / 2.0f + s );
6812  for ( i = low; i <= n; i++ ) {
6813  H[i][i] -= s;
6814  }
6815  exshift += s;
6816  x = y = w = 0.964f;
6817  }
6818  }
6819 
6820  iter = iter + 1;
6821 
6822  // look for two consecutive small sub-diagonal elements
6823  int m;
6824  for( m = n-2; m >= l; m-- ) {
6825  z = H[m][m];
6826  r = x - z;
6827  s = y - z;
6828  p = ( r * s - w ) / H[m+1][m] + H[m][m+1];
6829  q = H[m+1][m+1] - z - r - s;
6830  r = H[m+2][m+1];
6831  s = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
6832  p = p / s;
6833  q = q / s;
6834  r = r / s;
6835  if ( m == l ) {
6836  break;
6837  }
6838  if ( idMath::Fabs( H[m][m-1] ) * ( idMath::Fabs( q ) + idMath::Fabs( r ) ) <
6839  eps * ( idMath::Fabs( p ) * ( idMath::Fabs( H[m-1][m-1] ) + idMath::Fabs( z ) + idMath::Fabs( H[m+1][m+1] ) ) ) ) {
6840  break;
6841  }
6842  }
6843 
6844  for ( i = m+2; i <= n; i++ ) {
6845  H[i][i-2] = 0.0f;
6846  if ( i > m+2 ) {
6847  H[i][i-3] = 0.0f;
6848  }
6849  }
6850 
6851  // double QR step involving rows l:n and columns m:n
6852  for ( k = m; k <= n-1; k++ ) {
6853  bool notlast = ( k != n-1 );
6854  if ( k != m ) {
6855  p = H[k][k-1];
6856  q = H[k+1][k-1];
6857  r = ( notlast ? H[k+2][k-1] : 0.0f );
6858  x = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
6859  if ( x != 0.0f ) {
6860  p = p / x;
6861  q = q / x;
6862  r = r / x;
6863  }
6864  }
6865  if ( x == 0.0f ) {
6866  break;
6867  }
6868  s = idMath::Sqrt( p * p + q * q + r * r );
6869  if ( p < 0.0f ) {
6870  s = -s;
6871  }
6872  if ( s != 0.0f ) {
6873  if ( k != m ) {
6874  H[k][k-1] = -s * x;
6875  } else if ( l != m ) {
6876  H[k][k-1] = -H[k][k-1];
6877  }
6878  p = p + s;
6879  x = p / s;
6880  y = q / s;
6881  z = r / s;
6882  q = q / p;
6883  r = r / p;
6884 
6885  // modify row
6886  for ( j = k; j < numRows; j++ ) {
6887  p = H[k][j] + q * H[k+1][j];
6888  if ( notlast ) {
6889  p = p + r * H[k+2][j];
6890  H[k+2][j] = H[k+2][j] - p * z;
6891  }
6892  H[k][j] = H[k][j] - p * x;
6893  H[k+1][j] = H[k+1][j] - p * y;
6894  }
6895 
6896  // modify column
6897  for ( i = 0; i <= Min( n, k + 3 ); i++ ) {
6898  p = x * H[i][k] + y * H[i][k+1];
6899  if ( notlast ) {
6900  p = p + z * H[i][k+2];
6901  H[i][k+2] = H[i][k+2] - p * r;
6902  }
6903  H[i][k] = H[i][k] - p;
6904  H[i][k+1] = H[i][k+1] - p * q;
6905  }
6906 
6907  // accumulate transformations
6908  for ( i = low; i <= high; i++ ) {
6909  p = x * (*this)[i][k] + y * (*this)[i][k+1];
6910  if ( notlast ) {
6911  p = p + z * (*this)[i][k+2];
6912  (*this)[i][k+2] = (*this)[i][k+2] - p * r;
6913  }
6914  (*this)[i][k] = (*this)[i][k] - p;
6915  (*this)[i][k+1] = (*this)[i][k+1] - p * q;
6916  }
6917  }
6918  }
6919  }
6920  }
6921 
6922  // backsubstitute to find vectors of upper triangular form
6923  if ( norm == 0.0f ) {
6924  return false;
6925  }
6926 
6927  for ( n = numRows-1; n >= 0; n-- ) {
6928  p = realEigenValues[n];
6929  q = imaginaryEigenValues[n];
6930 
6931  if ( q == 0.0f ) { // real vector
6932  int l = n;
6933  H[n][n] = 1.0f;
6934  for ( i = n-1; i >= 0; i-- ) {
6935  w = H[i][i] - p;
6936  r = 0.0f;
6937  for ( j = l; j <= n; j++ ) {
6938  r = r + H[i][j] * H[j][n];
6939  }
6940  if ( imaginaryEigenValues[i] < 0.0f ) {
6941  z = w;
6942  s = r;
6943  } else {
6944  l = i;
6945  if ( imaginaryEigenValues[i] == 0.0f ) {
6946  if ( w != 0.0f ) {
6947  H[i][n] = -r / w;
6948  } else {
6949  H[i][n] = -r / ( eps * norm );
6950  }
6951  } else { // solve real equations
6952  x = H[i][i+1];
6953  y = H[i+1][i];
6954  q = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i];
6955  t = ( x * s - z * r ) / q;
6956  H[i][n] = t;
6957  if ( idMath::Fabs(x) > idMath::Fabs( z ) ) {
6958  H[i+1][n] = ( -r - w * t ) / x;
6959  } else {
6960  H[i+1][n] = ( -s - y * t ) / z;
6961  }
6962  }
6963 
6964  // overflow control
6965  t = idMath::Fabs(H[i][n]);
6966  if ( ( eps * t ) * t > 1 ) {
6967  for ( j = i; j <= n; j++ ) {
6968  H[j][n] = H[j][n] / t;
6969  }
6970  }
6971  }
6972  }
6973  } else if ( q < 0.0f ) { // complex vector
6974  int l = n-1;
6975 
6976  // last vector component imaginary so matrix is triangular
6977  if ( idMath::Fabs( H[n][n-1] ) > idMath::Fabs( H[n-1][n] ) ) {
6978  H[n-1][n-1] = q / H[n][n-1];
6979  H[n-1][n] = -( H[n][n] - p ) / H[n][n-1];
6980  } else {
6981  ComplexDivision( 0.0f, -H[n-1][n], H[n-1][n-1]-p, q, H[n-1][n-1], H[n-1][n] );
6982  }
6983  H[n][n-1] = 0.0f;
6984  H[n][n] = 1.0f;
6985  for ( i = n-2; i >= 0; i-- ) {
6986  float ra, sa, vr, vi;
6987  ra = 0.0f;
6988  sa = 0.0f;
6989  for ( j = l; j <= n; j++ ) {
6990  ra = ra + H[i][j] * H[j][n-1];
6991  sa = sa + H[i][j] * H[j][n];
6992  }
6993  w = H[i][i] - p;
6994 
6995  if ( imaginaryEigenValues[i] < 0.0f ) {
6996  z = w;
6997  r = ra;
6998  s = sa;
6999  } else {
7000  l = i;
7001  if ( imaginaryEigenValues[i] == 0.0f ) {
7002  ComplexDivision( -ra, -sa, w, q, H[i][n-1], H[i][n] );
7003  } else {
7004  // solve complex equations
7005  x = H[i][i+1];
7006  y = H[i+1][i];
7007  vr = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i] - q * q;
7008  vi = ( realEigenValues[i] - p ) * 2.0f * q;
7009  if ( vr == 0.0f && vi == 0.0f ) {
7010  vr = eps * norm * ( idMath::Fabs( w ) + idMath::Fabs( q ) + idMath::Fabs( x ) + idMath::Fabs( y ) + idMath::Fabs( z ) );
7011  }
7012  ComplexDivision( x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, H[i][n-1], H[i][n] );
7013  if ( idMath::Fabs( x ) > ( idMath::Fabs( z ) + idMath::Fabs( q ) ) ) {
7014  H[i+1][n-1] = ( -ra - w * H[i][n-1] + q * H[i][n] ) / x;
7015  H[i+1][n] = ( -sa - w * H[i][n] - q * H[i][n-1] ) / x;
7016  } else {
7017  ComplexDivision( -r - y * H[i][n-1], -s - y * H[i][n], z, q, H[i+1][n-1], H[i+1][n] );
7018  }
7019  }
7020 
7021  // overflow control
7022  t = Max( idMath::Fabs( H[i][n-1] ), idMath::Fabs( H[i][n] ) );
7023  if ( ( eps * t ) * t > 1 ) {
7024  for ( j = i; j <= n; j++ ) {
7025  H[j][n-1] = H[j][n-1] / t;
7026  H[j][n] = H[j][n] / t;
7027  }
7028  }
7029  }
7030  }
7031  }
7032  }
7033 
7034  // vectors of isolated roots
7035  for ( i = 0; i < numRows; i++ ) {
7036  if ( i < low || i > high ) {
7037  for ( j = i; j < numRows; j++ ) {
7038  (*this)[i][j] = H[i][j];
7039  }
7040  }
7041  }
7042 
7043  // back transformation to get eigenvectors of original matrix
7044  for ( j = numRows - 1; j >= low; j-- ) {
7045  for ( i = low; i <= high; i++ ) {
7046  z = 0.0f;
7047  for ( k = low; k <= Min( j, high ); k++ ) {
7048  z = z + (*this)[i][k] * H[k][j];
7049  }
7050  (*this)[i][j] = z;
7051  }
7052  }
7053 
7054  return true;
7055 }
7056 
7057 /*
7058 ============
7059 idMatX::Eigen_Solve
7060 
7061  Determine eigen values and eigen vectors for a square matrix.
7062  The eigen values are stored in 'realEigenValues' and 'imaginaryEigenValues'.
7063  Column i of the original matrix will store the eigen vector corresponding to the realEigenValues[i] and imaginaryEigenValues[i].
7064 ============
7065 */
7066 bool idMatX::Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
7067  idMatX H;
7068 
7069  assert( numRows == numColumns );
7070 
7071  realEigenValues.SetSize( numRows );
7072  imaginaryEigenValues.SetSize( numRows );
7073 
7074  H = *this;
7075 
7076  // reduce to Hessenberg form
7077  HessenbergReduction( H );
7078 
7079  // reduce Hessenberg to real Schur form
7080  return HessenbergToRealSchur( H, realEigenValues, imaginaryEigenValues );
7081 }
7082 
7083 /*
7084 ============
7085 idMatX::Eigen_SortIncreasing
7086 ============
7087 */
7088 void idMatX::Eigen_SortIncreasing( idVecX &eigenValues ) {
7089  int i, j, k;
7090  float min;
7091 
7092  for ( i = 0, j; i <= numRows - 2; i++ ) {
7093  j = i;
7094  min = eigenValues[j];
7095  for ( k = i + 1; k < numRows; k++ ) {
7096  if ( eigenValues[k] < min ) {
7097  j = k;
7098  min = eigenValues[j];
7099  }
7100  }
7101  if ( j != i ) {
7102  eigenValues.SwapElements( i, j );
7103  SwapColumns( i, j );
7104  }
7105  }
7106 }
7107 
7108 /*
7109 ============
7110 idMatX::Eigen_SortDecreasing
7111 ============
7112 */
7113 void idMatX::Eigen_SortDecreasing( idVecX &eigenValues ) {
7114  int i, j, k;
7115  float max;
7116 
7117  for ( i = 0, j; i <= numRows - 2; i++ ) {
7118  j = i;
7119  max = eigenValues[j];
7120  for ( k = i + 1; k < numRows; k++ ) {
7121  if ( eigenValues[k] > max ) {
7122  j = k;
7123  max = eigenValues[j];
7124  }
7125  }
7126  if ( j != i ) {
7127  eigenValues.SwapElements( i, j );
7128  SwapColumns( i, j );
7129  }
7130  }
7131 }
7132 
7133 /*
7134 ============
7135 idMatX::DeterminantGeneric
7136 ============
7137 */
7138 float idMatX::DeterminantGeneric( void ) const {
7139  int *index;
7140  float det;
7141  idMatX tmp;
7142 
7143  index = (int *) _alloca16( numRows * sizeof( int ) );
7144  tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
7145  tmp = *this;
7146 
7147  if ( !tmp.LU_Factor( index, &det ) ) {
7148  return 0.0f;
7149  }
7150 
7151  return det;
7152 }
7153 
7154 /*
7155 ============
7156 idMatX::InverseSelfGeneric
7157 ============
7158 */
7159 bool idMatX::InverseSelfGeneric( void ) {
7160  int i, j, *index;
7161  idMatX tmp;
7162  idVecX x, b;
7163 
7164  index = (int *) _alloca16( numRows * sizeof( int ) );
7165  tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
7166  tmp = *this;
7167 
7168  if ( !tmp.LU_Factor( index ) ) {
7169  return false;
7170  }
7171 
7172  x.SetData( numRows, VECX_ALLOCA( numRows ) );
7173  b.SetData( numRows, VECX_ALLOCA( numRows ) );
7174  b.Zero();
7175 
7176  for ( i = 0; i < numRows; i++ ) {
7177 
7178  b[i] = 1.0f;
7179  tmp.LU_Solve( x, b, index );
7180  for ( j = 0; j < numRows; j++ ) {
7181  (*this)[j][i] = x[j];
7182  }
7183  b[i] = 0.0f;
7184  }
7185  return true;
7186 }
7187 
7188 /*
7189 ============
7190 idMatX::Test
7191 ============
7192 */
7193 void idMatX::Test( void ) {
7194  idMatX original, m1, m2, m3, q1, q2, r1, r2;
7195  idVecX v, w, u, c, d;
7196  int offset, size, *index1, *index2;
7197 
7198  size = 6;
7199  original.Random( size, size, 0 );
7200  original = original * original.Transpose();
7201 
7202  index1 = (int *) _alloca16( ( size + 1 ) * sizeof( index1[0] ) );
7203  index2 = (int *) _alloca16( ( size + 1 ) * sizeof( index2[0] ) );
7204 
7205  /*
7206  idMatX::LowerTriangularInverse
7207  */
7208 
7209  m1 = original;
7210  m1.ClearUpperTriangle();
7211  m2 = m1;
7212 
7213  m2.InverseSelf();
7214  m1.LowerTriangularInverse();
7215 
7216  if ( !m1.Compare( m2, 1e-4f ) ) {
7217  idLib::common->Warning( "idMatX::LowerTriangularInverse failed" );
7218  }
7219 
7220  /*
7221  idMatX::UpperTriangularInverse
7222  */
7223 
7224  m1 = original;
7225  m1.ClearLowerTriangle();
7226  m2 = m1;
7227 
7228  m2.InverseSelf();
7229  m1.UpperTriangularInverse();
7230 
7231  if ( !m1.Compare( m2, 1e-4f ) ) {
7232  idLib::common->Warning( "idMatX::UpperTriangularInverse failed" );
7233  }
7234 
7235  /*
7236  idMatX::Inverse_GaussJordan
7237  */
7238 
7239  m1 = original;
7240 
7241  m1.Inverse_GaussJordan();
7242  m1 *= original;
7243 
7244  if ( !m1.IsIdentity( 1e-4f ) ) {
7245  idLib::common->Warning( "idMatX::Inverse_GaussJordan failed" );
7246  }
7247 
7248  /*
7249  idMatX::Inverse_UpdateRankOne
7250  */
7251 
7252  m1 = original;
7253  m2 = original;
7254 
7255  w.Random( size, 1 );
7256  v.Random( size, 2 );
7257 
7258  // invert m1
7259  m1.Inverse_GaussJordan();
7260 
7261  // modify and invert m2
7262  m2.Update_RankOne( v, w, 1.0f );
7263  if ( !m2.Inverse_GaussJordan() ) {
7264  assert( 0 );
7265  }
7266 
7267  // update inverse of m1
7268  m1.Inverse_UpdateRankOne( v, w, 1.0f );
7269 
7270  if ( !m1.Compare( m2, 1e-4f ) ) {
7271  idLib::common->Warning( "idMatX::Inverse_UpdateRankOne failed" );
7272  }
7273 
7274  /*
7275  idMatX::Inverse_UpdateRowColumn
7276  */
7277 
7278  for ( offset = 0; offset < size; offset++ ) {
7279  m1 = original;
7280  m2 = original;
7281 
7282  v.Random( size, 1 );
7283  w.Random( size, 2 );
7284  w[offset] = 0.0f;
7285 
7286  // invert m1
7287  m1.Inverse_GaussJordan();
7288 
7289  // modify and invert m2
7290  m2.Update_RowColumn( v, w, offset );
7291  if ( !m2.Inverse_GaussJordan() ) {
7292  assert( 0 );
7293  }
7294 
7295  // update inverse of m1
7296  m1.Inverse_UpdateRowColumn( v, w, offset );
7297 
7298  if ( !m1.Compare( m2, 1e-3f ) ) {
7299  idLib::common->Warning( "idMatX::Inverse_UpdateRowColumn failed" );
7300  }
7301  }
7302 
7303  /*
7304  idMatX::Inverse_UpdateIncrement
7305  */
7306 
7307  m1 = original;
7308  m2 = original;
7309 
7310  v.Random( size + 1, 1 );
7311  w.Random( size + 1, 2 );
7312  w[size] = 0.0f;
7313 
7314  // invert m1
7315  m1.Inverse_GaussJordan();
7316 
7317  // modify and invert m2
7318  m2.Update_Increment( v, w );
7319  if ( !m2.Inverse_GaussJordan() ) {
7320  assert( 0 );
7321  }
7322 
7323  // update inverse of m1
7324  m1.Inverse_UpdateIncrement( v, w );
7325 
7326  if ( !m1.Compare( m2, 1e-4f ) ) {
7327  idLib::common->Warning( "idMatX::Inverse_UpdateIncrement failed" );
7328  }
7329 
7330  /*
7331  idMatX::Inverse_UpdateDecrement
7332  */
7333 
7334  for ( offset = 0; offset < size; offset++ ) {
7335  m1 = original;
7336  m2 = original;
7337 
7338  v.SetSize( 6 );
7339  w.SetSize( 6 );
7340  for ( int i = 0; i < size; i++ ) {
7341  v[i] = original[i][offset];
7342  w[i] = original[offset][i];
7343  }
7344 
7345  // invert m1
7346  m1.Inverse_GaussJordan();
7347 
7348  // modify and invert m2
7349  m2.Update_Decrement( offset );
7350  if ( !m2.Inverse_GaussJordan() ) {
7351  assert( 0 );
7352  }
7353 
7354  // update inverse of m1
7355  m1.Inverse_UpdateDecrement( v, w, offset );
7356 
7357  if ( !m1.Compare( m2, 1e-3f ) ) {
7358  idLib::common->Warning( "idMatX::Inverse_UpdateDecrement failed" );
7359  }
7360  }
7361 
7362  /*
7363  idMatX::LU_Factor
7364  */
7365 
7366  m1 = original;
7367 
7368  m1.LU_Factor( NULL ); // no pivoting
7369  m1.LU_UnpackFactors( m2, m3 );
7370  m1 = m2 * m3;
7371 
7372  if ( !original.Compare( m1, 1e-4f ) ) {
7373  idLib::common->Warning( "idMatX::LU_Factor failed" );
7374  }
7375 
7376  /*
7377  idMatX::LU_UpdateRankOne
7378  */
7379 
7380  m1 = original;
7381  m2 = original;
7382 
7383  w.Random( size, 1 );
7384  v.Random( size, 2 );
7385 
7386  // factor m1
7387  m1.LU_Factor( index1 );
7388 
7389  // modify and factor m2
7390  m2.Update_RankOne( v, w, 1.0f );
7391  if ( !m2.LU_Factor( index2 ) ) {
7392  assert( 0 );
7393  }
7394  m2.LU_MultiplyFactors( m3, index2 );
7395  m2 = m3;
7396 
7397  // update factored m1
7398  m1.LU_UpdateRankOne( v, w, 1.0f, index1 );
7399  m1.LU_MultiplyFactors( m3, index1 );
7400  m1 = m3;
7401 
7402  if ( !m1.Compare( m2, 1e-4f ) ) {
7403  idLib::common->Warning( "idMatX::LU_UpdateRankOne failed" );
7404  }
7405 
7406  /*
7407  idMatX::LU_UpdateRowColumn
7408  */
7409 
7410  for ( offset = 0; offset < size; offset++ ) {
7411  m1 = original;
7412  m2 = original;
7413 
7414  v.Random( size, 1 );
7415  w.Random( size, 2 );
7416  w[offset] = 0.0f;
7417 
7418  // factor m1
7419  m1.LU_Factor( index1 );
7420 
7421  // modify and factor m2
7422  m2.Update_RowColumn( v, w, offset );
7423  if ( !m2.LU_Factor( index2 ) ) {
7424  assert( 0 );
7425  }
7426  m2.LU_MultiplyFactors( m3, index2 );
7427  m2 = m3;
7428 
7429  // update m1
7430  m1.LU_UpdateRowColumn( v, w, offset, index1 );
7431  m1.LU_MultiplyFactors( m3, index1 );
7432  m1 = m3;
7433 
7434  if ( !m1.Compare( m2, 1e-3f ) ) {
7435  idLib::common->Warning( "idMatX::LU_UpdateRowColumn failed" );
7436  }
7437  }
7438 
7439  /*
7440  idMatX::LU_UpdateIncrement
7441  */
7442 
7443  m1 = original;
7444  m2 = original;
7445 
7446  v.Random( size + 1, 1 );
7447  w.Random( size + 1, 2 );
7448  w[size] = 0.0f;
7449 
7450  // factor m1
7451  m1.LU_Factor( index1 );
7452 
7453  // modify and factor m2
7454  m2.Update_Increment( v, w );
7455  if ( !m2.LU_Factor( index2 ) ) {
7456  assert( 0 );
7457  }
7458  m2.LU_MultiplyFactors( m3, index2 );
7459  m2 = m3;
7460 
7461  // update factored m1
7462  m1.LU_UpdateIncrement( v, w, index1 );
7463  m1.LU_MultiplyFactors( m3, index1 );
7464  m1 = m3;
7465 
7466  if ( !m1.Compare( m2, 1e-4f ) ) {
7467  idLib::common->Warning( "idMatX::LU_UpdateIncrement failed" );
7468  }
7469 
7470  /*
7471  idMatX::LU_UpdateDecrement
7472  */
7473 
7474  for ( offset = 0; offset < size; offset++ ) {
7475  m1 = original;
7476  m2 = original;
7477 
7478  v.SetSize( 6 );
7479  w.SetSize( 6 );
7480  for ( int i = 0; i < size; i++ ) {
7481  v[i] = original[i][offset];
7482  w[i] = original[offset][i];
7483  }
7484 
7485  // factor m1
7486  m1.LU_Factor( index1 );
7487 
7488  // modify and factor m2
7489  m2.Update_Decrement( offset );
7490  if ( !m2.LU_Factor( index2 ) ) {
7491  assert( 0 );
7492  }
7493  m2.LU_MultiplyFactors( m3, index2 );
7494  m2 = m3;
7495 
7496  u.SetSize( 6 );
7497  for ( int i = 0; i < size; i++ ) {
7498  u[i] = original[index1[offset]][i];
7499  }
7500 
7501  // update factors of m1
7502  m1.LU_UpdateDecrement( v, w, u, offset, index1 );
7503  m1.LU_MultiplyFactors( m3, index1 );
7504  m1 = m3;
7505 
7506  if ( !m1.Compare( m2, 1e-3f ) ) {
7507  idLib::common->Warning( "idMatX::LU_UpdateDecrement failed" );
7508  }
7509  }
7510 
7511  /*
7512  idMatX::LU_Inverse
7513  */
7514 
7515  m2 = original;
7516 
7517  m2.LU_Factor( NULL );
7518  m2.LU_Inverse( m1, NULL );
7519  m1 *= original;
7520 
7521  if ( !m1.IsIdentity( 1e-4f ) ) {
7522  idLib::common->Warning( "idMatX::LU_Inverse failed" );
7523  }
7524 
7525  /*
7526  idMatX::QR_Factor
7527  */
7528 
7529  c.SetSize( size );
7530  d.SetSize( size );
7531 
7532  m1 = original;
7533 
7534  m1.QR_Factor( c, d );
7535  m1.QR_UnpackFactors( q1, r1, c, d );
7536  m1 = q1 * r1;
7537 
7538  if ( !original.Compare( m1, 1e-4f ) ) {
7539  idLib::common->Warning( "idMatX::QR_Factor failed" );
7540  }
7541 
7542  /*
7543  idMatX::QR_UpdateRankOne
7544  */
7545 
7546  c.SetSize( size );
7547  d.SetSize( size );
7548 
7549  m1 = original;
7550  m2 = original;
7551 
7552  w.Random( size, 0 );
7553  v = w;
7554 
7555  // factor m1
7556  m1.QR_Factor( c, d );
7557  m1.QR_UnpackFactors( q1, r1, c, d );
7558 
7559  // modify and factor m2
7560  m2.Update_RankOne( v, w, 1.0f );
7561  if ( !m2.QR_Factor( c, d ) ) {
7562  assert( 0 );
7563  }
7564  m2.QR_UnpackFactors( q2, r2, c, d );
7565  m2 = q2 * r2;
7566 
7567  // update factored m1
7568  q1.QR_UpdateRankOne( r1, v, w, 1.0f );
7569  m1 = q1 * r1;
7570 
7571  if ( !m1.Compare( m2, 1e-4f ) ) {
7572  idLib::common->Warning( "idMatX::QR_UpdateRankOne failed" );
7573  }
7574 
7575  /*
7576  idMatX::QR_UpdateRowColumn
7577  */
7578 
7579  for ( offset = 0; offset < size; offset++ ) {
7580  c.SetSize( size );
7581  d.SetSize( size );
7582 
7583  m1 = original;
7584  m2 = original;
7585 
7586  v.Random( size, 1 );
7587  w.Random( size, 2 );
7588  w[offset] = 0.0f;
7589 
7590  // factor m1
7591  m1.QR_Factor( c, d );
7592  m1.QR_UnpackFactors( q1, r1, c, d );
7593 
7594  // modify and factor m2
7595  m2.Update_RowColumn( v, w, offset );
7596  if ( !m2.QR_Factor( c, d ) ) {
7597  assert( 0 );
7598  }
7599  m2.QR_UnpackFactors( q2, r2, c, d );
7600  m2 = q2 * r2;
7601 
7602  // update m1
7603  q1.QR_UpdateRowColumn( r1, v, w, offset );
7604  m1 = q1 * r1;
7605 
7606  if ( !m1.Compare( m2, 1e-3f ) ) {
7607  idLib::common->Warning( "idMatX::QR_UpdateRowColumn failed" );
7608  }
7609  }
7610 
7611  /*
7612  idMatX::QR_UpdateIncrement
7613  */
7614 
7615  c.SetSize( size+1 );
7616  d.SetSize( size+1 );
7617 
7618  m1 = original;
7619  m2 = original;
7620 
7621  v.Random( size + 1, 1 );
7622  w.Random( size + 1, 2 );
7623  w[size] = 0.0f;
7624 
7625  // factor m1
7626  m1.QR_Factor( c, d );
7627  m1.QR_UnpackFactors( q1, r1, c, d );
7628 
7629  // modify and factor m2
7630  m2.Update_Increment( v, w );
7631  if ( !m2.QR_Factor( c, d ) ) {
7632  assert( 0 );
7633  }
7634  m2.QR_UnpackFactors( q2, r2, c, d );
7635  m2 = q2 * r2;
7636 
7637  // update factored m1
7638  q1.QR_UpdateIncrement( r1, v, w );
7639  m1 = q1 * r1;
7640 
7641  if ( !m1.Compare( m2, 1e-4f ) ) {
7642  idLib::common->Warning( "idMatX::QR_UpdateIncrement failed" );
7643  }
7644 
7645  /*
7646  idMatX::QR_UpdateDecrement
7647  */
7648 
7649  for ( offset = 0; offset < size; offset++ ) {
7650  c.SetSize( size+1 );
7651  d.SetSize( size+1 );
7652 
7653  m1 = original;
7654  m2 = original;
7655 
7656  v.SetSize( 6 );
7657  w.SetSize( 6 );
7658  for ( int i = 0; i < size; i++ ) {
7659  v[i] = original[i][offset];
7660  w[i] = original[offset][i];
7661  }
7662 
7663  // factor m1
7664  m1.QR_Factor( c, d );
7665  m1.QR_UnpackFactors( q1, r1, c, d );
7666 
7667  // modify and factor m2
7668  m2.Update_Decrement( offset );
7669  if ( !m2.QR_Factor( c, d ) ) {
7670  assert( 0 );
7671  }
7672  m2.QR_UnpackFactors( q2, r2, c, d );
7673  m2 = q2 * r2;
7674 
7675  // update factors of m1
7676  q1.QR_UpdateDecrement( r1, v, w, offset );
7677  m1 = q1 * r1;
7678 
7679  if ( !m1.Compare( m2, 1e-3f ) ) {
7680  idLib::common->Warning( "idMatX::QR_UpdateDecrement failed" );
7681  }
7682  }
7683 
7684  /*
7685  idMatX::QR_Inverse
7686  */
7687 
7688  m2 = original;
7689 
7690  m2.QR_Factor( c, d );
7691  m2.QR_Inverse( m1, c, d );
7692  m1 *= original;
7693 
7694  if ( !m1.IsIdentity( 1e-4f ) ) {
7695  idLib::common->Warning( "idMatX::QR_Inverse failed" );
7696  }
7697 
7698  /*
7699  idMatX::SVD_Factor
7700  */
7701 
7702  m1 = original;
7703  m3.Zero( size, size );
7704  w.Zero( size );
7705 
7706  m1.SVD_Factor( w, m3 );
7707  m2.Diag( w );
7708  m3.TransposeSelf();
7709  m1 = m1 * m2 * m3;
7710 
7711  if ( !original.Compare( m1, 1e-4f ) ) {
7712  idLib::common->Warning( "idMatX::SVD_Factor failed" );
7713  }
7714 
7715  /*
7716  idMatX::SVD_Inverse
7717  */
7718 
7719  m2 = original;
7720 
7721  m2.SVD_Factor( w, m3 );
7722  m2.SVD_Inverse( m1, w, m3 );
7723  m1 *= original;
7724 
7725  if ( !m1.IsIdentity( 1e-4f ) ) {
7726  idLib::common->Warning( "idMatX::SVD_Inverse failed" );
7727  }
7728 
7729  /*
7730  idMatX::Cholesky_Factor
7731  */
7732 
7733  m1 = original;
7734 
7735  m1.Cholesky_Factor();
7736  m1.Cholesky_MultiplyFactors( m2 );
7737 
7738  if ( !original.Compare( m2, 1e-4f ) ) {
7739  idLib::common->Warning( "idMatX::Cholesky_Factor failed" );
7740  }
7741 
7742  /*
7743  idMatX::Cholesky_UpdateRankOne
7744  */
7745 
7746  m1 = original;
7747  m2 = original;
7748 
7749  w.Random( size, 0 );
7750 
7751  // factor m1
7752  m1.Cholesky_Factor();
7753  m1.ClearUpperTriangle();
7754 
7755  // modify and factor m2
7756  m2.Update_RankOneSymmetric( w, 1.0f );
7757  if ( !m2.Cholesky_Factor() ) {
7758  assert( 0 );
7759  }
7760  m2.ClearUpperTriangle();
7761 
7762  // update factored m1
7763  m1.Cholesky_UpdateRankOne( w, 1.0f, 0 );
7764 
7765  if ( !m1.Compare( m2, 1e-4f ) ) {
7766  idLib::common->Warning( "idMatX::Cholesky_UpdateRankOne failed" );
7767  }
7768 
7769  /*
7770  idMatX::Cholesky_UpdateRowColumn
7771  */
7772 
7773  for ( offset = 0; offset < size; offset++ ) {
7774  m1 = original;
7775  m2 = original;
7776 
7777  // factor m1
7778  m1.Cholesky_Factor();
7779  m1.ClearUpperTriangle();
7780 
7781  int pdtable[] = { 1, 0, 1, 0, 0, 0 };
7782  w.Random( size, pdtable[offset] );
7783  w *= 0.1f;
7784 
7785  // modify and factor m2
7786  m2.Update_RowColumnSymmetric( w, offset );
7787  if ( !m2.Cholesky_Factor() ) {
7788  assert( 0 );
7789  }
7790  m2.ClearUpperTriangle();
7791 
7792  // update m1
7793  m1.Cholesky_UpdateRowColumn( w, offset );
7794 
7795  if ( !m1.Compare( m2, 1e-3f ) ) {
7796  idLib::common->Warning( "idMatX::Cholesky_UpdateRowColumn failed" );
7797  }
7798  }
7799 
7800  /*
7801  idMatX::Cholesky_UpdateIncrement
7802  */
7803 
7804  m1.Random( size + 1, size + 1, 0 );
7805  m3 = m1 * m1.Transpose();
7806 
7807  m1.SquareSubMatrix( m3, size );
7808  m2 = m1;
7809 
7810  w.SetSize( size + 1 );
7811  for ( int i = 0; i < size + 1; i++ ) {
7812  w[i] = m3[size][i];
7813  }
7814 
7815  // factor m1
7816  m1.Cholesky_Factor();
7817 
7818  // modify and factor m2
7819  m2.Update_IncrementSymmetric( w );
7820  if ( !m2.Cholesky_Factor() ) {
7821  assert( 0 );
7822  }
7823 
7824  // update factored m1
7825  m1.Cholesky_UpdateIncrement( w );
7826 
7827  m1.ClearUpperTriangle();
7828  m2.ClearUpperTriangle();
7829 
7830  if ( !m1.Compare( m2, 1e-4f ) ) {
7831  idLib::common->Warning( "idMatX::Cholesky_UpdateIncrement failed" );
7832  }
7833 
7834  /*
7835  idMatX::Cholesky_UpdateDecrement
7836  */
7837 
7838  for ( offset = 0; offset < size; offset += size - 1 ) {
7839  m1 = original;
7840  m2 = original;
7841 
7842  v.SetSize( 6 );
7843  for ( int i = 0; i < size; i++ ) {
7844  v[i] = original[i][offset];
7845  }
7846 
7847  // factor m1
7848  m1.Cholesky_Factor();
7849 
7850  // modify and factor m2
7851  m2.Update_Decrement( offset );
7852  if ( !m2.Cholesky_Factor() ) {
7853  assert( 0 );
7854  }
7855 
7856  // update factors of m1
7857  m1.Cholesky_UpdateDecrement( v, offset );
7858 
7859  if ( !m1.Compare( m2, 1e-3f ) ) {
7860  idLib::common->Warning( "idMatX::Cholesky_UpdateDecrement failed" );
7861  }
7862  }
7863 
7864  /*
7865  idMatX::Cholesky_Inverse
7866  */
7867 
7868  m2 = original;
7869 
7870  m2.Cholesky_Factor();
7871  m2.Cholesky_Inverse( m1 );
7872  m1 *= original;
7873 
7874  if ( !m1.IsIdentity( 1e-4f ) ) {
7875  idLib::common->Warning( "idMatX::Cholesky_Inverse failed" );
7876  }
7877 
7878  /*
7879  idMatX::LDLT_Factor
7880  */
7881 
7882  m1 = original;
7883 
7884  m1.LDLT_Factor();
7885  m1.LDLT_MultiplyFactors( m2 );
7886 
7887  if ( !original.Compare( m2, 1e-4f ) ) {
7888  idLib::common->Warning( "idMatX::LDLT_Factor failed" );
7889  }
7890 
7891  m1.LDLT_UnpackFactors( m2, m3 );
7892  m2 = m2 * m3 * m2.Transpose();
7893 
7894  if ( !original.Compare( m2, 1e-4f ) ) {
7895  idLib::common->Warning( "idMatX::LDLT_Factor failed" );
7896  }
7897 
7898  /*
7899  idMatX::LDLT_UpdateRankOne
7900  */
7901 
7902  m1 = original;
7903  m2 = original;
7904 
7905  w.Random( size, 0 );
7906 
7907  // factor m1
7908  m1.LDLT_Factor();
7909  m1.ClearUpperTriangle();
7910 
7911  // modify and factor m2
7912  m2.Update_RankOneSymmetric( w, 1.0f );
7913  if ( !m2.LDLT_Factor() ) {
7914  assert( 0 );
7915  }
7916  m2.ClearUpperTriangle();
7917 
7918  // update factored m1
7919  m1.LDLT_UpdateRankOne( w, 1.0f, 0 );
7920 
7921  if ( !m1.Compare( m2, 1e-4f ) ) {
7922  idLib::common->Warning( "idMatX::LDLT_UpdateRankOne failed" );
7923  }
7924 
7925  /*
7926  idMatX::LDLT_UpdateRowColumn
7927  */
7928 
7929  for ( offset = 0; offset < size; offset++ ) {
7930  m1 = original;
7931  m2 = original;
7932 
7933  w.Random( size, 0 );
7934 
7935  // factor m1
7936  m1.LDLT_Factor();
7937  m1.ClearUpperTriangle();
7938 
7939  // modify and factor m2
7940  m2.Update_RowColumnSymmetric( w, offset );
7941  if ( !m2.LDLT_Factor() ) {
7942  assert( 0 );
7943  }
7944  m2.ClearUpperTriangle();
7945 
7946  // update m1
7947  m1.LDLT_UpdateRowColumn( w, offset );
7948 
7949  if ( !m1.Compare( m2, 1e-3f ) ) {
7950  idLib::common->Warning( "idMatX::LDLT_UpdateRowColumn failed" );
7951  }
7952  }
7953 
7954  /*
7955  idMatX::LDLT_UpdateIncrement
7956  */
7957 
7958  m1.Random( size + 1, size + 1, 0 );
7959  m3 = m1 * m1.Transpose();
7960 
7961  m1.SquareSubMatrix( m3, size );
7962  m2 = m1;
7963 
7964  w.SetSize( size + 1 );
7965  for ( int i = 0; i < size + 1; i++ ) {
7966  w[i] = m3[size][i];
7967  }
7968 
7969  // factor m1
7970  m1.LDLT_Factor();
7971 
7972  // modify and factor m2
7973  m2.Update_IncrementSymmetric( w );
7974  if ( !m2.LDLT_Factor() ) {
7975  assert( 0 );
7976  }
7977 
7978  // update factored m1
7979  m1.LDLT_UpdateIncrement( w );
7980 
7981  m1.ClearUpperTriangle();
7982  m2.ClearUpperTriangle();
7983 
7984  if ( !m1.Compare( m2, 1e-4f ) ) {
7985  idLib::common->Warning( "idMatX::LDLT_UpdateIncrement failed" );
7986  }
7987 
7988  /*
7989  idMatX::LDLT_UpdateDecrement
7990  */
7991 
7992  for ( offset = 0; offset < size; offset++ ) {
7993  m1 = original;
7994  m2 = original;
7995 
7996  v.SetSize( 6 );
7997  for ( int i = 0; i < size; i++ ) {
7998  v[i] = original[i][offset];
7999  }
8000 
8001  // factor m1
8002  m1.LDLT_Factor();
8003 
8004  // modify and factor m2
8005  m2.Update_Decrement( offset );
8006  if ( !m2.LDLT_Factor() ) {
8007  assert( 0 );
8008  }
8009 
8010  // update factors of m1
8011  m1.LDLT_UpdateDecrement( v, offset );
8012 
8013  if ( !m1.Compare( m2, 1e-3f ) ) {
8014  idLib::common->Warning( "idMatX::LDLT_UpdateDecrement failed" );
8015  }
8016  }
8017 
8018  /*
8019  idMatX::LDLT_Inverse
8020  */
8021 
8022  m2 = original;
8023 
8024  m2.LDLT_Factor();
8025  m2.LDLT_Inverse( m1 );
8026  m1 *= original;
8027 
8028  if ( !m1.IsIdentity( 1e-4f ) ) {
8029  idLib::common->Warning( "idMatX::LDLT_Inverse failed" );
8030  }
8031 
8032  /*
8033  idMatX::Eigen_SolveSymmetricTriDiagonal
8034  */
8035 
8036  m3 = original;
8037  m3.TriDiagonal_ClearTriangles();
8038  m1 = m3;
8039 
8040  v.SetSize( size );
8041 
8042  m1.Eigen_SolveSymmetricTriDiagonal( v );
8043 
8044  m3.TransposeMultiply( m2, m1 );
8045 
8046  for ( int i = 0; i < size; i++ ) {
8047  for ( int j = 0; j < size; j++ ) {
8048  m1[i][j] *= v[j];
8049  }
8050  }
8051 
8052  if ( !m1.Compare( m2, 1e-4f ) ) {
8053  idLib::common->Warning( "idMatX::Eigen_SolveSymmetricTriDiagonal failed" );
8054  }
8055 
8056  /*
8057  idMatX::Eigen_SolveSymmetric
8058  */
8059 
8060  m3 = original;
8061  m1 = m3;
8062 
8063  v.SetSize( size );
8064 
8065  m1.Eigen_SolveSymmetric( v );
8066 
8067  m3.TransposeMultiply( m2, m1 );
8068 
8069  for ( int i = 0; i < size; i++ ) {
8070  for ( int j = 0; j < size; j++ ) {
8071  m1[i][j] *= v[j];
8072  }
8073  }
8074 
8075  if ( !m1.Compare( m2, 1e-4f ) ) {
8076  idLib::common->Warning( "idMatX::Eigen_SolveSymmetric failed" );
8077  }
8078 
8079  /*
8080  idMatX::Eigen_Solve
8081  */
8082 
8083  m3 = original;
8084  m1 = m3;
8085 
8086  v.SetSize( size );
8087  w.SetSize( size );
8088 
8089  m1.Eigen_Solve( v, w );
8090 
8091  m3.TransposeMultiply( m2, m1 );
8092 
8093  for ( int i = 0; i < size; i++ ) {
8094  for ( int j = 0; j < size; j++ ) {
8095  m1[i][j] *= v[j];
8096  }
8097  }
8098 
8099  if ( !m1.Compare( m2, 1e-4f ) ) {
8100  idLib::common->Warning( "idMatX::Eigen_Solve failed" );
8101  }
8102 }
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:2959
idMat3::ToCQuat
idCQuat ToCQuat(void) const
Definition: Matrix.cpp:233
g
GLubyte g
Definition: glext.h:4662
idMatX::TransposeMultiply
idVecX TransposeMultiply(const idVecX &vec) const
Definition: Matrix.h:2683
idMatX::ComplexDivision
void ComplexDivision(float xr, float xi, float yr, float yi, float &cdivr, float &cdivi)
Definition: Matrix.cpp:6656
idVecX::SetSize
void SetSize(int size)
Definition: Vector.h:1707
idMatX::Cholesky_Solve
void Cholesky_Solve(idVecX &x, const idVecX &b) const
Definition: Matrix.cpp:5657
idMatX::IsSymmetric
bool IsSymmetric(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.h:2512
min
#define min(a, b)
idMatX::Eigen_SolveSymmetric
bool Eigen_SolveSymmetric(idVecX &eigenValues)
Definition: Matrix.cpp:6550
idMatX::LU_Inverse
void LU_Inverse(idMatX &inv, const int *index) const
Definition: Matrix.cpp:4344
idVec3::Normalize
float Normalize(void)
Definition: Vector.h:646
idMatX::tempIndex
static int tempIndex
Definition: Matrix.h:1977
prepare.sp
tuple sp
Definition: prepare.py:20
idMat2::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:316
assert
assert(prefInfo.fullscreenBtn)
idMat6::mat
idVec6 mat[6]
Definition: Matrix.h:1500
mat3_identity
idMat3 mat3_identity(idVec3(1, 0, 0), idVec3(0, 1, 0), idVec3(0, 0, 1))
idCQuat
Definition: Quat.h:306
idVecX::GetSize
int GetSize(void) const
Definition: Vector.h:1467
idMatX::Cholesky_UpdateRowColumn
bool Cholesky_UpdateRowColumn(const idVecX &v, int r)
Definition: Matrix.cpp:5392
idMatX::IsSquare
bool IsSquare(void) const
Definition: Matrix.h:1844
mat4_zero
idMat4 mat4_zero(idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0))
idMatX::QR_Rotate
void QR_Rotate(idMatX &R, int i, float a, float b)
Definition: Matrix.cpp:4504
idMatX::Cholesky_UpdateIncrement
bool Cholesky_UpdateIncrement(const idVecX &v)
Definition: Matrix.cpp:5574
idMatX::QR_UpdateRankOne
bool QR_UpdateRankOne(idMatX &R, const idVecX &v, const idVecX &w, float alpha)
Definition: Matrix.cpp:4547
idMatX::DeterminantGeneric
float DeterminantGeneric(void) const
Definition: Matrix.cpp:7138
idMatX::QR_UpdateRowColumn
bool QR_UpdateRowColumn(idMatX &R, const idVecX &v, const idVecX &w, int r)
Definition: Matrix.cpp:4598
idMat4::InverseSelf
bool InverseSelf(void)
Definition: Matrix.cpp:689
idMatX::Update_RowColumn
void Update_RowColumn(const idVecX &v, const idVecX &w, int r)
Definition: Matrix.cpp:3500
v
const GLdouble * v
Definition: glext.h:2936
idMat3::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:610
idMat6::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:1761
idMatX::Inverse_UpdateIncrement
bool Inverse_UpdateIncrement(const idVecX &v, const idVecX &w)
Definition: Matrix.cpp:3787
idVecX::Zero
void Zero(void)
Definition: Vector.h:1767
idMatX::SVD_Factor
bool SVD_Factor(idVecX &w, idMatX &V)
Definition: Matrix.cpp:5057
idMatX::SwapRows
idMatX & SwapRows(int r1, int r2)
Definition: Matrix.h:2382
Max
ID_INLINE T Max(T x, T y)
Definition: Lib.h:158
idMatX::Inverse_UpdateRankOne
bool Inverse_UpdateRankOne(const idVecX &v, const idVecX &w, float alpha)
Definition: Matrix.cpp:3709
idMatX::Inverse_GaussJordan
bool Inverse_GaussJordan(void)
Definition: Matrix.cpp:3620
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:4804
idMat3::Transpose
idMat3 Transpose(void) const
Definition: Matrix.h:677
idMatX::LDLT_MultiplyFactors
void LDLT_MultiplyFactors(idMatX &m) const
Definition: Matrix.cpp:6212
idMatX::RemoveRow
idMatX & RemoveRow(int r)
Definition: Matrix.cpp:3001
mat6_zero
idMat6 mat6_zero(idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0))
idVec3::Set
void Set(const float x, const float y, const float z)
Definition: Vector.h:409
idQuat::w
float w
Definition: Quat.h:53
idMat3::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.cpp:378
y
GLenum GLint GLint y
Definition: glext.h:2849
n
GLenum GLsizei n
Definition: glext.h:3705
idMatX::LU_Factor
bool LU_Factor(int *index, float *det=NULL)
Definition: Matrix.cpp:3863
idMatX::TriDiagonal_Inverse
void TriDiagonal_Inverse(idMatX &inv) const
Definition: Matrix.cpp:6304
idMat5::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:1434
VECX_ALLOCA
#define VECX_ALLOCA(n)
Definition: Vector.h:1432
mat6_identity
idMat6 mat6_identity(idVec6(1, 0, 0, 0, 0, 0), idVec6(0, 1, 0, 0, 0, 0), idVec6(0, 0, 1, 0, 0, 0), idVec6(0, 0, 0, 1, 0, 0), idVec6(0, 0, 0, 0, 1, 0), idVec6(0, 0, 0, 0, 0, 1))
idMath::FLT_EPSILON
static const float FLT_EPSILON
Definition: Math.h:219
idMatX::HessenbergToRealSchur
bool HessenbergToRealSchur(idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues)
Definition: Matrix.cpp:6678
idMatX::GetDimension
int GetDimension(void) const
Definition: Matrix.h:2907
idMat3::InverseSelf
bool InverseSelf(void)
Definition: Matrix.cpp:333
idVecX::SwapElements
idVecX & SwapElements(int e1, int e2)
Definition: Vector.h:1829
idMat5::mat
idVec5 mat[5]
Definition: Matrix.h:1201
idVec3
Definition: Vector.h:316
float
case const float
Definition: Callbacks.cpp:62
idStr::FloatArrayToString
static const char * FloatArrayToString(const float *array, const int length, const int precision)
Definition: Str.cpp:418
R
Definition: eax4.h:1413
idMatX::LDLT_UnpackFactors
void LDLT_UnpackFactors(idMatX &L, idMatX &D) const
Definition: Matrix.cpp:6191
idMath::Sqrt
static float Sqrt(float x)
Definition: Math.h:302
idMatX::Eigen_SolveSymmetricTriDiagonal
bool Eigen_SolveSymmetricTriDiagonal(idVecX &eigenValues)
Definition: Matrix.cpp:6520
FLOAT_IS_NAN
#define FLOAT_IS_NAN(x)
Definition: Math.h:74
idMat5::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:1738
D
Definition: eax4.h:1412
idMat5::GetDimension
int GetDimension(void) const
Definition: Matrix.h:1430
idMatX::Update_RankOne
void Update_RankOne(const idVecX &v, const idVecX &w, float alpha)
Definition: Matrix.cpp:3450
idMatX::IsPositiveDefinite
bool IsPositiveDefinite(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3227
idMat4::Determinant
float Determinant(void) const
Definition: Matrix.cpp:665
idMatX::ClearUpperTriangle
void ClearUpperTriangle(void)
Definition: Matrix.h:2414
alpha
GLclampf GLclampf GLclampf alpha
Definition: glext.h:2843
idMatX::Transpose
idMatX Transpose(void) const
Definition: Matrix.h:2562
idMat4::Transpose
idMat4 Transpose(void) const
Definition: Matrix.cpp:629
s
GLdouble s
Definition: glext.h:2935
idMat4::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:1045
idMatX::Update_RowColumnSymmetric
void Update_RowColumnSymmetric(const idVecX &v, int r)
Definition: Matrix.cpp:3528
v0
GLfloat v0
Definition: glext.h:3606
idMatX::SVD_Solve
void SVD_Solve(idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V) const
Definition: Matrix.cpp:5185
idMatX::Cholesky_Inverse
void Cholesky_Inverse(idMatX &inv) const
Definition: Matrix.cpp:5690
idMatX::Identity
void Identity(void)
Definition: Matrix.h:2308
x
GLenum GLint x
Definition: glext.h:2849
process.i
int i
Definition: process.py:33
offset
GLintptr offset
Definition: glext.h:3113
idMatX::Cholesky_MultiplyFactors
void Cholesky_MultiplyFactors(idMatX &m) const
Definition: Matrix.cpp:5719
idMatX::LU_MultiplyFactors
void LU_MultiplyFactors(idMatX &m, const int *index) const
Definition: Matrix.cpp:4396
idMatX::LU_UpdateIncrement
bool LU_UpdateIncrement(const idVecX &v, const idVecX &w, int *index)
Definition: Matrix.cpp:4170
idMatX::IsSymmetricPositiveSemiDefinite
bool IsSymmetricPositiveSemiDefinite(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3366
idMatX::Update_Increment
void Update_Increment(const idVecX &v, const idVecX &w)
Definition: Matrix.cpp:3557
idMatX::RemoveRowColumn
idMatX & RemoveRowColumn(int r)
Definition: Matrix.cpp:3040
idMatX::LDLT_Factor
bool LDLT_Factor(void)
Definition: Matrix.cpp:5750
idMat4::TransposeSelf
idMat4 & TransposeSelf(void)
Definition: Matrix.cpp:646
idMatX::LDLT_UpdateIncrement
bool LDLT_UpdateIncrement(const idVecX &v)
Definition: Matrix.cpp:6041
idQuat::x
float x
Definition: Quat.h:50
prepare.l
list l
Definition: prepare.py:17
idMat3::ToAngularVelocity
idVec3 ToAngularVelocity(void) const
Definition: Matrix.cpp:309
idRotation::angle
float angle
Definition: Rotation.h:92
idMat5::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.cpp:1301
idRotation::vec
idVec3 vec
Definition: Rotation.h:91
idMatX::temp
static float temp[MATX_MAX_TEMP+4]
Definition: Matrix.h:1975
idRotation::axisValid
bool axisValid
Definition: Rotation.h:94
idMatX::RemoveColumn
idMatX & RemoveColumn(int r)
Definition: Matrix.cpp:3020
idQuat::y
float y
Definition: Quat.h:51
idMatX::LU_UpdateRankOne
bool LU_UpdateRankOne(const idVecX &v, const idVecX &w, float alpha, int *index)
Definition: Matrix.cpp:3947
idMatX::LDLT_UpdateRankOne
bool LDLT_UpdateRankOne(const idVecX &v, float alpha, int offset=0)
Definition: Matrix.cpp:5795
idMat2::InverseSelf
bool InverseSelf(void)
Definition: Matrix.cpp:47
idMath::M_RAD2DEG
static const float M_RAD2DEG
Definition: Math.h:215
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:3608
idMatX::Multiply
idVecX Multiply(const idVecX &vec) const
Definition: Matrix.h:2655
idRotation::origin
idVec3 origin
Definition: Rotation.h:90
idVecX::Random
void Random(int seed, float l=0.0f, float u=1.0f)
Definition: Vector.h:1784
idMat3::InertiaTranslate
idMat3 InertiaTranslate(const float mass, const idVec3 &centerOfMass, const idVec3 &translation) const
Definition: Matrix.cpp:537
idRotation::GetAngle
float GetAngle(void) const
Definition: Rotation.h:154
idMatX::GetNumColumns
int GetNumColumns(void) const
Definition: Matrix.h:1822
idVec2
Definition: Vector.h:52
idMatX::ClearLowerTriangle
void ClearLowerTriangle(void)
Definition: Matrix.h:2421
idMatX::Eigen_SortDecreasing
void Eigen_SortDecreasing(idVecX &eigenValues)
Definition: Matrix.cpp:7113
idMat6::Transpose
idMat6 Transpose(void) const
Definition: Matrix.cpp:1757
idMatX::TriDiagonal_ClearTriangles
void TriDiagonal_ClearTriangles(void)
Definition: Matrix.cpp:6247
idMatX::Update_Decrement
void Update_Decrement(int r)
Definition: Matrix.cpp:3609
index
GLuint index
Definition: glext.h:3476
c
const GLubyte * c
Definition: glext.h:4677
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3454
idVec4
Definition: Vector.h:808
idMatX::IsOrthonormal
bool IsOrthonormal(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3104
idMatX::LDLT_Solve
void LDLT_Solve(idVecX &x, const idVecX &b) const
Definition: Matrix.cpp:6124
idMatX::LU_UnpackFactors
void LU_UnpackFactors(idMatX &L, idMatX &U) const
Definition: Matrix.cpp:4373
H
#define H(x, y, z)
Definition: md5.c:105
mat5_zero
idMat5 mat5_zero(idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0))
idMatX::HouseholderReduction
void HouseholderReduction(idVecX &diag, idVecX &subd)
Definition: Matrix.cpp:6337
idMatX::SwapColumns
idMatX & SwapColumns(int r1, int r2)
Definition: Matrix.h:2393
idMatX::LDLT_Inverse
void LDLT_Inverse(idMatX &inv) const
Definition: Matrix.cpp:6162
idMath::Fabs
static float Fabs(float f)
Definition: Math.h:779
NULL
#define NULL
Definition: Lib.h:88
idMath::SQRT_1OVER2
static const float SQRT_1OVER2
Definition: Math.h:212
idMat5::Transpose
idMat5 Transpose(void) const
Definition: Matrix.cpp:1064
MATX_ALLOCA
#define MATX_ALLOCA(n)
Definition: Matrix.h:1783
idMatX::LDLT_UpdateDecrement
bool LDLT_UpdateDecrement(const idVecX &v, int r)
Definition: Matrix.cpp:6087
idMatX::IsSymmetricPositiveDefinite
bool IsSymmetricPositiveDefinite(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3277
idVecX::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Vector.h:1910
idRotation::axis
idMat3 axis
Definition: Rotation.h:93
idMatX::SVD_BiDiag
void SVD_BiDiag(idVecX &w, idVecX &rv1, float &anorm)
Definition: Matrix.cpp:4893
Mem_Alloc16
void * Mem_Alloc16(const int size)
Definition: Heap.cpp:1107
idMatX::QR_UpdateDecrement
bool QR_UpdateDecrement(idMatX &R, const idVecX &v, const idVecX &w, int r)
Definition: Matrix.cpp:4659
idMatX::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:3439
L
Definition: eax4.h:1413
idMatX::GetNumRows
int GetNumRows(void) const
Definition: Matrix.h:1821
idAngles::roll
float roll
Definition: Angles.h:55
idMatX::Random
void Random(int seed, float l=0.0f, float u=1.0f)
Definition: Matrix.h:2333
idMatX::alloced
int alloced
Definition: Matrix.h:1972
idMatX::SVD_MultiplyFactors
void SVD_MultiplyFactors(idMatX &m, const idVecX &w, const idMatX &V) const
Definition: Matrix.cpp:5260
idMatX::Zero
void Zero(void)
Definition: Matrix.h:2291
idMat6::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:2927
idMat4::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.cpp:775
idMatX::LU_UpdateRowColumn
bool LU_UpdateRowColumn(const idVecX &v, const idVecX &w, int r, int *index)
Definition: Matrix.cpp:4022
idMatX::Inverse_UpdateDecrement
bool Inverse_UpdateDecrement(const idVecX &v, const idVecX &w, int r)
Definition: Matrix.cpp:3812
idMatX::Update_IncrementSymmetric
void Update_IncrementSymmetric(const idVecX &v)
Definition: Matrix.cpp:3586
idMath::InvSqrt
static float InvSqrt(float x)
Definition: Math.h:268
idMatX::mat
float * mat
Definition: Matrix.h:1973
idAngles::pitch
float pitch
Definition: Angles.h:53
a
GLubyte GLubyte GLubyte a
Definition: glext.h:4662
idMatX::TriDiagonal_Solve
bool TriDiagonal_Solve(idVecX &x, const idVecX &b) const
Definition: Matrix.cpp:6266
DEG2RAD
#define DEG2RAD(a)
Definition: Math.h:56
idMatX::Inverse_UpdateRowColumn
bool Inverse_UpdateRowColumn(const idVecX &v, const idVecX &w, int r)
Definition: Matrix.cpp:3753
idVec3::FixDegenerateNormal
bool FixDegenerateNormal(void)
Definition: Vector.h:535
idSwap
ID_INLINE void idSwap(type &a, type &b)
Definition: List.h:77
idMatX::IsPositiveSemiDefinite
bool IsPositiveSemiDefinite(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3301
transpose
GLsizei GLboolean transpose
Definition: glext.h:3622
idMat2::ToString
const char * ToString(int precision=2) const
Definition: Matrix.cpp:128
v1
GLfloat GLfloat v1
Definition: glext.h:3607
idMat3::mat
idVec3 mat[3]
Definition: Matrix.h:408
idMatX::Cholesky_UpdateDecrement
bool Cholesky_UpdateDecrement(const idVecX &v, int r)
Definition: Matrix.cpp:5620
b
GLubyte GLubyte b
Definition: glext.h:4662
idRotation::GetVec
const idVec3 & GetVec(void) const
Definition: Rotation.h:150
idQuat
Definition: Quat.h:48
idMatX::InverseSelfGeneric
bool InverseSelfGeneric(void)
Definition: Matrix.cpp:7159
MATX_CLEAREND
#define MATX_CLEAREND()
Definition: Matrix.h:1782
idMatX::QR_MultiplyFactors
void QR_MultiplyFactors(idMatX &m, const idVecX &c, const idVecX &d) const
Definition: Matrix.cpp:4830
idMat5::InverseSelf
bool InverseSelf(void)
Definition: Matrix.cpp:1142
idQuat::z
float z
Definition: Quat.h:52
idMat6::GetDimension
int GetDimension(void) const
Definition: Matrix.h:1757
idMatX::Cholesky_Factor
bool Cholesky_Factor(void)
Definition: Matrix.cpp:5294
r
GLdouble GLdouble GLdouble r
Definition: glext.h:2951
mat2_identity
idMat2 mat2_identity(idVec2(1, 0), idVec2(0, 1))
idMatX::IsPMatrix
bool IsPMatrix(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3147
idMat4::GetDimension
int GetDimension(void) const
Definition: Matrix.h:1133
idMat3
Definition: Matrix.h:333
U
Definition: eax4.h:1412
idMatX::SetSize
void SetSize(int rows, int columns)
Definition: Matrix.h:2247
idMatX::Cholesky_UpdateRankOne
bool Cholesky_UpdateRankOne(const idVecX &v, float alpha, int offset=0)
Definition: Matrix.cpp:5337
idMatX::Update_RankOneSymmetric
void Update_RankOneSymmetric(const idVecX &v, float alpha)
Definition: Matrix.cpp:3472
idAngles::yaw
float yaw
Definition: Angles.h:54
idMat3::InertiaRotateSelf
idMat3 & InertiaRotateSelf(const idMat3 &rotation)
Definition: Matrix.cpp:599
MATX_MAX_TEMP
#define MATX_MAX_TEMP
Definition: Matrix.h:1780
idMatX::numColumns
int numColumns
Definition: Matrix.h:1971
idMat3::InertiaRotate
idMat3 InertiaRotate(const idMat3 &rotation) const
Definition: Matrix.cpp:589
idal.f
tuple f
Definition: idal.py:89
i2
GLint GLint i2
Definition: qgl.h:261
V
Definition: eax4.h:1413
idMat4::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:1137
idMatX::numRows
int numRows
Definition: Matrix.h:1970
idMatX::SVD_InitialWV
void SVD_InitialWV(idVecX &w, idMatX &V, idVecX &rv1)
Definition: Matrix.cpp:4983
idMatX::SquareSubMatrix
void SquareSubMatrix(const idMatX &m, int size)
Definition: Matrix.h:2428
idMat6::InverseSelf
bool InverseSelf(void)
Definition: Matrix.cpp:1869
idMat3::ToQuat
idQuat ToQuat(void) const
Definition: Matrix.cpp:182
idMatX::IsIdentity
bool IsIdentity(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.h:2467
norm
Definition: quaddefs.h:113
idMatX::LU_Solve
void LU_Solve(idVecX &x, const idVecX &b, const int *index) const
Definition: Matrix.cpp:4308
idMat2::GetDimension
int GetDimension(void) const
Definition: Matrix.h:312
RAD2DEG
#define RAD2DEG(a)
Definition: Math.h:57
size
GLsizeiptr size
Definition: glext.h:3112
y1
GLdouble y1
Definition: qgl.h:415
idMatX::SetData
void SetData(int rows, int columns, float *data)
Definition: Matrix.h:2278
Mem_Free16
void Mem_Free16(void *ptr)
Definition: Heap.cpp:1128
idMatX::QR_Solve
void QR_Solve(idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d) const
Definition: Matrix.cpp:4694
idMatX::QR_UpdateIncrement
bool QR_UpdateIncrement(idMatX &R, const idVecX &v, const idVecX &w)
Definition: Matrix.cpp:4631
idMat3::GetDimension
int GetDimension(void) const
Definition: Matrix.h:745
idMatX::HessenbergReduction
void HessenbergReduction(idMatX &H)
Definition: Matrix.cpp:6569
idMatX::QL
bool QL(idVecX &diag, idVecX &subd)
Definition: Matrix.cpp:6442
idVecX::SetData
void SetData(int length, float *data)
Definition: Vector.h:1756
idMatX::UpperTriangularInverse
bool UpperTriangularInverse(void)
Definition: Matrix.cpp:3412
i1
GLint i1
Definition: qgl.h:261
idMat3::ToRotation
idRotation ToRotation(void) const
Definition: Matrix.cpp:246
idMat2::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.cpp:74
mat5_identity
idMat5 mat5_identity(idVec5(1, 0, 0, 0, 0), idVec5(0, 1, 0, 0, 0), idVec5(0, 0, 1, 0, 0), idVec5(0, 0, 0, 1, 0), idVec5(0, 0, 0, 0, 1))
idMatX::Test
static void Test(void)
Definition: Matrix.cpp:7193
idMatX::SVD_Inverse
void SVD_Inverse(idMatX &inv, const idVecX &w, const idMatX &V) const
Definition: Matrix.cpp:5223
idMatX::QR_Factor
bool QR_Factor(idVecX &c, idVecX &d)
Definition: Matrix.cpp:4435
idMat5::TransposeSelf
idMat5 & TransposeSelf(void)
Definition: Matrix.cpp:1081
idMat3::ToAngles
idAngles ToAngles(void) const
Definition: Matrix.cpp:147
mat3_zero
idMat3 mat3_zero(idVec3(0, 0, 0), idVec3(0, 0, 0), idVec3(0, 0, 0))
mat4_identity
idMat4 mat4_identity(idVec4(1, 0, 0, 0), idVec4(0, 1, 0, 0), idVec4(0, 0, 1, 0), idVec4(0, 0, 0, 1))
idMatX::QR_Inverse
void QR_Inverse(idMatX &inv, const idVecX &c, const idVecX &d) const
Definition: Matrix.cpp:4764
idMat6::TransposeSelf
idMat6 & TransposeSelf(void)
Definition: Matrix.cpp:1774
idMatX::InverseSelf
bool InverseSelf(void)
Definition: Matrix.h:2592
j
GLint j
Definition: qgl.h:264
idMatX::Eigen_Solve
bool Eigen_Solve(idVecX &realEigenValues, idVecX &imaginaryEigenValues)
Definition: Matrix.cpp:7066
idMat4::mat
idVec4 mat[4]
Definition: Matrix.h:826
idMat6::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.cpp:2186
idMatX::tempPtr
static float * tempPtr
Definition: Matrix.h:1976
idMatX::LowerTriangularInverse
bool LowerTriangularInverse(void)
Definition: Matrix.cpp:3383
idMat5::Determinant
float Determinant(void) const
Definition: Matrix.cpp:1100
idMatX::LDLT_UpdateRowColumn
bool LDLT_UpdateRowColumn(const idVecX &v, int r)
Definition: Matrix.cpp:5847
idMatX::IsOrthogonal
bool IsOrthogonal(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3072
idMat2::mat
idVec2 mat[2]
Definition: Matrix.h:106
idMath::ACos
static float ACos(float a)
Definition: Math.h:544
max
#define max(x, y)
Definition: os.h:70
idMat2
Definition: Matrix.h:55
idMat3::InertiaTranslateSelf
idMat3 & InertiaTranslateSelf(const float mass, const idVec3 &centerOfMass, const idVec3 &translation)
Definition: Matrix.cpp:562
idMatX::Eigen_SortIncreasing
void Eigen_SortIncreasing(idVecX &eigenValues)
Definition: Matrix.cpp:7088
idMat3::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:749
p
GLfloat GLfloat p
Definition: glext.h:4674
idMatX::Inverse_Solve
void Inverse_Solve(idVecX &x, const idVecX &b) const
Definition: Matrix.cpp:3846
idMatX::QR_UnpackFactors
void QR_UnpackFactors(idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d) const
Definition: Matrix.cpp:4793
idMatX::ChangeSize
void ChangeSize(int rows, int columns, bool makeZero=false)
Definition: Matrix.cpp:2948
idMat6::Determinant
float Determinant(void) const
Definition: Matrix.cpp:1793
idMat4
Definition: Matrix.h:764
z
GLdouble GLdouble z
Definition: glext.h:3067
idMatX::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:2935
idVec3::Zero
void Zero(void)
Definition: Vector.h:415
idCommon::Warning
virtual void virtual void Warning(const char *fmt,...) id_attribute((format(printf
Min
ID_INLINE T Min(T x, T y)
Definition: Lib.h:159
idMatX::IsZMatrix
bool IsZMatrix(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.cpp:3202
idMatX::Pythag
float Pythag(float a, float b) const
Definition: Matrix.cpp:4870
mat2_zero
idMat2 mat2_zero(idVec2(0, 0), idVec2(0, 0))
idMatX::Diag
void Diag(const idVecX &v)
Definition: Matrix.h:2326
idMatX::LU_UpdateDecrement
bool LU_UpdateDecrement(const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index)
Definition: Matrix.cpp:4219
MATRIX_INVERSE_EPSILON
#define MATRIX_INVERSE_EPSILON
Definition: Matrix.h:40
idMatX::Compare
bool Compare(const idMatX &a) const
Definition: Matrix.h:2211
idLib::common
static class idCommon * common
Definition: Lib.h:53
idMat3::Determinant
float Determinant(void) const
Definition: Matrix.cpp:319
w1
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:4080
t
GLdouble GLdouble t
Definition: glext.h:2943
This page is maintained by Wladimir van der Laan. Generated on Mon Nov 17 2014 12:23:22 for doom3-gpl by doxygen 1.8.6.