Matrix.cpp (142302B)
1 /* 2 =========================================================================== 3 4 Doom 3 BFG Edition GPL Source Code 5 Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 6 7 This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code"). 8 9 Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>. 21 22 In addition, the Doom 3 BFG Edition 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 BFG Edition 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 #pragma hdrstop 30 #include "../precompiled.h" 31 32 //=============================================================== 33 // 34 // idMat2 35 // 36 //=============================================================== 37 38 idMat2 mat2_zero( idVec2( 0, 0 ), idVec2( 0, 0 ) ); 39 idMat2 mat2_identity( idVec2( 1, 0 ), idVec2( 0, 1 ) ); 40 41 /* 42 ============ 43 idMat2::InverseSelf 44 ============ 45 */ 46 bool idMat2::InverseSelf() { 47 // 2+4 = 6 multiplications 48 // 1 division 49 double det, invDet, a; 50 51 det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 52 53 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 54 return false; 55 } 56 57 invDet = 1.0f / det; 58 59 a = mat[0][0]; 60 mat[0][0] = mat[1][1] * invDet; 61 mat[0][1] = - mat[0][1] * invDet; 62 mat[1][0] = - mat[1][0] * invDet; 63 mat[1][1] = a * invDet; 64 65 return true; 66 } 67 68 /* 69 ============ 70 idMat2::InverseFastSelf 71 ============ 72 */ 73 bool idMat2::InverseFastSelf() { 74 #if 1 75 // 2+4 = 6 multiplications 76 // 1 division 77 double det, invDet, a; 78 79 det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 80 81 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 82 return false; 83 } 84 85 invDet = 1.0f / det; 86 87 a = mat[0][0]; 88 mat[0][0] = mat[1][1] * invDet; 89 mat[0][1] = - mat[0][1] * invDet; 90 mat[1][0] = - mat[1][0] * invDet; 91 mat[1][1] = a * invDet; 92 93 return true; 94 #else 95 // 2*4 = 8 multiplications 96 // 2 division 97 float *mat = reinterpret_cast<float *>(this); 98 double d, di; 99 float s; 100 101 di = mat[0]; 102 s = di; 103 mat[0*2+0] = d = 1.0f / di; 104 mat[0*2+1] *= d; 105 d = -d; 106 mat[1*2+0] *= d; 107 d = mat[1*2+0] * di; 108 mat[1*2+1] += mat[0*2+1] * d; 109 di = mat[1*2+1]; 110 s *= di; 111 mat[1*2+1] = d = 1.0f / di; 112 mat[1*2+0] *= d; 113 d = -d; 114 mat[0*2+1] *= d; 115 d = mat[0*2+1] * di; 116 mat[0*2+0] += mat[1*2+0] * d; 117 118 return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) ); 119 #endif 120 } 121 122 /* 123 ============= 124 idMat2::ToString 125 ============= 126 */ 127 const char *idMat2::ToString( int precision ) const { 128 return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision ); 129 } 130 131 132 //=============================================================== 133 // 134 // idMat3 135 // 136 //=============================================================== 137 138 idMat3 mat3_zero( idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ) ); 139 idMat3 mat3_identity( idVec3( 1, 0, 0 ), idVec3( 0, 1, 0 ), idVec3( 0, 0, 1 ) ); 140 141 /* 142 ======================== 143 idMat3::ToAngles 144 145 returns the pitch/yaw/roll each in the range [-180, 180] degrees 146 ======================== 147 */ 148 idAngles idMat3::ToAngles() const { 149 idAngles angles; 150 float s = idMath::Sqrt( mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1] ); 151 if ( s > idMath::FLT_EPSILON ) { 152 angles.pitch = RAD2DEG( - idMath::ATan( mat[0][2], s ) ); 153 angles.yaw = RAD2DEG( idMath::ATan( mat[0][1], mat[0][0] ) ); 154 angles.roll = RAD2DEG( idMath::ATan( mat[1][2], mat[2][2] ) ); 155 } else { 156 angles.pitch = mat[0][2] < 0.0f ? 90.0f : -90.0f; 157 angles.yaw = RAD2DEG( - idMath::ATan( mat[1][0], mat[1][1] ) ); 158 angles.roll = 0.0f; 159 } 160 return angles; 161 } 162 163 /* 164 ============ 165 idMat3::ToQuat 166 ============ 167 */ 168 idQuat idMat3::ToQuat() const { 169 idQuat q; 170 float trace; 171 float s; 172 float t; 173 int i; 174 int j; 175 int k; 176 177 static int next[ 3 ] = { 1, 2, 0 }; 178 179 trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ]; 180 181 if ( trace > 0.0f ) { 182 183 t = trace + 1.0f; 184 s = idMath::InvSqrt( t ) * 0.5f; 185 186 q[3] = s * t; 187 q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s; 188 q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s; 189 q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s; 190 191 } else { 192 193 i = 0; 194 if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) { 195 i = 1; 196 } 197 if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) { 198 i = 2; 199 } 200 j = next[ i ]; 201 k = next[ j ]; 202 203 t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f; 204 s = idMath::InvSqrt( t ) * 0.5f; 205 206 q[i] = s * t; 207 q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s; 208 q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s; 209 q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s; 210 } 211 return q; 212 } 213 214 /* 215 ============ 216 idMat3::ToCQuat 217 ============ 218 */ 219 idCQuat idMat3::ToCQuat() const { 220 idQuat q = ToQuat(); 221 if ( q.w < 0.0f ) { 222 return idCQuat( -q.x, -q.y, -q.z ); 223 } 224 return idCQuat( q.x, q.y, q.z ); 225 } 226 227 /* 228 ============ 229 idMat3::ToRotation 230 ============ 231 */ 232 idRotation idMat3::ToRotation() const { 233 idRotation r; 234 float trace; 235 float s; 236 float t; 237 int i; 238 int j; 239 int k; 240 static int next[ 3 ] = { 1, 2, 0 }; 241 242 trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ]; 243 if ( trace > 0.0f ) { 244 245 t = trace + 1.0f; 246 s = idMath::InvSqrt( t ) * 0.5f; 247 248 r.angle = s * t; 249 r.vec[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s; 250 r.vec[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s; 251 r.vec[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s; 252 253 } else { 254 255 i = 0; 256 if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) { 257 i = 1; 258 } 259 if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) { 260 i = 2; 261 } 262 j = next[ i ]; 263 k = next[ j ]; 264 265 t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f; 266 s = idMath::InvSqrt( t ) * 0.5f; 267 268 r.vec[i] = s * t; 269 r.angle = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s; 270 r.vec[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s; 271 r.vec[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s; 272 } 273 274 r.angle = idMath::ACos( r.angle ); 275 float lengthSqr = r.vec.LengthSqr(); 276 if ( ( idMath::Fabs( r.angle ) < 1e-10f ) || ( lengthSqr < 1e-10f ) ) { 277 r.vec.Set( 0.0f, 0.0f, 1.0f ); 278 r.angle = 0.0f; 279 } else { 280 r.vec *= idMath::InvSqrt( lengthSqr ); 281 r.angle *= 2.0f * idMath::M_RAD2DEG; 282 } 283 284 285 r.origin.Zero(); 286 r.axis = *this; 287 r.axisValid = true; 288 return r; 289 } 290 291 /* 292 ================= 293 idMat3::ToAngularVelocity 294 ================= 295 */ 296 idVec3 idMat3::ToAngularVelocity() const { 297 idRotation rotation = ToRotation(); 298 return rotation.GetVec() * DEG2RAD( rotation.GetAngle() ); 299 } 300 301 /* 302 ============ 303 idMat3::Determinant 304 ============ 305 */ 306 float idMat3::Determinant() const { 307 308 float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; 309 float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; 310 float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; 311 312 return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01; 313 } 314 315 /* 316 ============ 317 idMat3::InverseSelf 318 ============ 319 */ 320 bool idMat3::InverseSelf() { 321 // 18+3+9 = 30 multiplications 322 // 1 division 323 idMat3 inverse; 324 double det, invDet; 325 326 inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; 327 inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]; 328 inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; 329 330 det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0]; 331 332 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 333 return false; 334 } 335 336 invDet = 1.0f / det; 337 338 inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2]; 339 inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]; 340 inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]; 341 inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2]; 342 inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]; 343 inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 344 345 mat[0][0] = inverse[0][0] * invDet; 346 mat[0][1] = inverse[0][1] * invDet; 347 mat[0][2] = inverse[0][2] * invDet; 348 349 mat[1][0] = inverse[1][0] * invDet; 350 mat[1][1] = inverse[1][1] * invDet; 351 mat[1][2] = inverse[1][2] * invDet; 352 353 mat[2][0] = inverse[2][0] * invDet; 354 mat[2][1] = inverse[2][1] * invDet; 355 mat[2][2] = inverse[2][2] * invDet; 356 357 return true; 358 } 359 360 /* 361 ============ 362 idMat3::InverseFastSelf 363 ============ 364 */ 365 bool idMat3::InverseFastSelf() { 366 #if 1 367 // 18+3+9 = 30 multiplications 368 // 1 division 369 idMat3 inverse; 370 double det, invDet; 371 372 inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; 373 inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]; 374 inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; 375 376 det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0]; 377 378 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 379 return false; 380 } 381 382 invDet = 1.0f / det; 383 384 inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2]; 385 inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]; 386 inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]; 387 inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2]; 388 inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]; 389 inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 390 391 mat[0][0] = inverse[0][0] * invDet; 392 mat[0][1] = inverse[0][1] * invDet; 393 mat[0][2] = inverse[0][2] * invDet; 394 395 mat[1][0] = inverse[1][0] * invDet; 396 mat[1][1] = inverse[1][1] * invDet; 397 mat[1][2] = inverse[1][2] * invDet; 398 399 mat[2][0] = inverse[2][0] * invDet; 400 mat[2][1] = inverse[2][1] * invDet; 401 mat[2][2] = inverse[2][2] * invDet; 402 403 return true; 404 #elif 0 405 // 3*10 = 30 multiplications 406 // 3 divisions 407 float *mat = reinterpret_cast<float *>(this); 408 float s; 409 double d, di; 410 411 di = mat[0]; 412 s = di; 413 mat[0] = d = 1.0f / di; 414 mat[1] *= d; 415 mat[2] *= d; 416 d = -d; 417 mat[3] *= d; 418 mat[6] *= d; 419 d = mat[3] * di; 420 mat[4] += mat[1] * d; 421 mat[5] += mat[2] * d; 422 d = mat[6] * di; 423 mat[7] += mat[1] * d; 424 mat[8] += mat[2] * d; 425 di = mat[4]; 426 s *= di; 427 mat[4] = d = 1.0f / di; 428 mat[3] *= d; 429 mat[5] *= d; 430 d = -d; 431 mat[1] *= d; 432 mat[7] *= d; 433 d = mat[1] * di; 434 mat[0] += mat[3] * d; 435 mat[2] += mat[5] * d; 436 d = mat[7] * di; 437 mat[6] += mat[3] * d; 438 mat[8] += mat[5] * d; 439 di = mat[8]; 440 s *= di; 441 mat[8] = d = 1.0f / di; 442 mat[6] *= d; 443 mat[7] *= d; 444 d = -d; 445 mat[2] *= d; 446 mat[5] *= d; 447 d = mat[2] * di; 448 mat[0] += mat[6] * d; 449 mat[1] += mat[7] * d; 450 d = mat[5] * di; 451 mat[3] += mat[6] * d; 452 mat[4] += mat[7] * d; 453 454 return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) ); 455 #else 456 // 4*2+4*4 = 24 multiplications 457 // 2*1 = 2 divisions 458 idMat2 r0; 459 float r1[2], r2[2], r3; 460 float det, invDet; 461 float *mat = reinterpret_cast<float *>(this); 462 463 // r0 = m0.Inverse(); // 2x2 464 det = mat[0*3+0] * mat[1*3+1] - mat[0*3+1] * mat[1*3+0]; 465 466 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 467 return false; 468 } 469 470 invDet = 1.0f / det; 471 472 r0[0][0] = mat[1*3+1] * invDet; 473 r0[0][1] = - mat[0*3+1] * invDet; 474 r0[1][0] = - mat[1*3+0] * invDet; 475 r0[1][1] = mat[0*3+0] * invDet; 476 477 // r1 = r0 * m1; // 2x1 = 2x2 * 2x1 478 r1[0] = r0[0][0] * mat[0*3+2] + r0[0][1] * mat[1*3+2]; 479 r1[1] = r0[1][0] * mat[0*3+2] + r0[1][1] * mat[1*3+2]; 480 481 // r2 = m2 * r1; // 1x1 = 1x2 * 2x1 482 r2[0] = mat[2*3+0] * r1[0] + mat[2*3+1] * r1[1]; 483 484 // r3 = r2 - m3; // 1x1 = 1x1 - 1x1 485 r3 = r2[0] - mat[2*3+2]; 486 487 // r3.InverseSelf(); 488 if ( idMath::Fabs( r3 ) < MATRIX_INVERSE_EPSILON ) { 489 return false; 490 } 491 492 r3 = 1.0f / r3; 493 494 // r2 = m2 * r0; // 1x2 = 1x2 * 2x2 495 r2[0] = mat[2*3+0] * r0[0][0] + mat[2*3+1] * r0[1][0]; 496 r2[1] = mat[2*3+0] * r0[0][1] + mat[2*3+1] * r0[1][1]; 497 498 // m2 = r3 * r2; // 1x2 = 1x1 * 1x2 499 mat[2*3+0] = r3 * r2[0]; 500 mat[2*3+1] = r3 * r2[1]; 501 502 // m0 = r0 - r1 * m2; // 2x2 - 2x1 * 1x2 503 mat[0*3+0] = r0[0][0] - r1[0] * mat[2*3+0]; 504 mat[0*3+1] = r0[0][1] - r1[0] * mat[2*3+1]; 505 mat[1*3+0] = r0[1][0] - r1[1] * mat[2*3+0]; 506 mat[1*3+1] = r0[1][1] - r1[1] * mat[2*3+1]; 507 508 // m1 = r1 * r3; // 2x1 = 2x1 * 1x1 509 mat[0*3+2] = r1[0] * r3; 510 mat[1*3+2] = r1[1] * r3; 511 512 // m3 = -r3; 513 mat[2*3+2] = -r3; 514 515 return true; 516 #endif 517 } 518 519 /* 520 ============ 521 idMat3::InertiaTranslate 522 ============ 523 */ 524 idMat3 idMat3::InertiaTranslate( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) const { 525 idMat3 m; 526 idVec3 newCenter; 527 528 newCenter = centerOfMass + translation; 529 530 m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] ) 531 - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) ); 532 m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] ) 533 - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) ); 534 m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] ) 535 - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) ); 536 537 m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] ); 538 m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] ); 539 m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] ); 540 541 return (*this) + m; 542 } 543 544 /* 545 ============ 546 idMat3::InertiaTranslateSelf 547 ============ 548 */ 549 idMat3 &idMat3::InertiaTranslateSelf( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) { 550 idMat3 m; 551 idVec3 newCenter; 552 553 newCenter = centerOfMass + translation; 554 555 m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] ) 556 - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) ); 557 m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] ) 558 - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) ); 559 m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] ) 560 - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) ); 561 562 m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] ); 563 m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] ); 564 m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] ); 565 566 (*this) += m; 567 568 return (*this); 569 } 570 571 /* 572 ============ 573 idMat3::InertiaRotate 574 ============ 575 */ 576 idMat3 idMat3::InertiaRotate( const idMat3 &rotation ) const { 577 // NOTE: the rotation matrix is stored column-major 578 return rotation.Transpose() * (*this) * rotation; 579 } 580 581 /* 582 ============ 583 idMat3::InertiaRotateSelf 584 ============ 585 */ 586 idMat3 &idMat3::InertiaRotateSelf( const idMat3 &rotation ) { 587 // NOTE: the rotation matrix is stored column-major 588 *this = rotation.Transpose() * (*this) * rotation; 589 return *this; 590 } 591 592 /* 593 ============= 594 idMat3::ToString 595 ============= 596 */ 597 const char *idMat3::ToString( int precision ) const { 598 return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision ); 599 } 600 601 602 //=============================================================== 603 // 604 // idMat4 605 // 606 //=============================================================== 607 608 idMat4 mat4_zero( idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ) ); 609 idMat4 mat4_identity( idVec4( 1, 0, 0, 0 ), idVec4( 0, 1, 0, 0 ), idVec4( 0, 0, 1, 0 ), idVec4( 0, 0, 0, 1 ) ); 610 611 /* 612 ============ 613 idMat4::Transpose 614 ============ 615 */ 616 idMat4 idMat4::Transpose() const { 617 idMat4 transpose; 618 int i, j; 619 620 for( i = 0; i < 4; i++ ) { 621 for( j = 0; j < 4; j++ ) { 622 transpose[ i ][ j ] = mat[ j ][ i ]; 623 } 624 } 625 return transpose; 626 } 627 628 /* 629 ============ 630 idMat4::TransposeSelf 631 ============ 632 */ 633 idMat4 &idMat4::TransposeSelf() { 634 float temp; 635 int i, j; 636 637 for( i = 0; i < 4; i++ ) { 638 for( j = i + 1; j < 4; j++ ) { 639 temp = mat[ i ][ j ]; 640 mat[ i ][ j ] = mat[ j ][ i ]; 641 mat[ j ][ i ] = temp; 642 } 643 } 644 return *this; 645 } 646 647 /* 648 ============ 649 idMat4::Determinant 650 ============ 651 */ 652 float idMat4::Determinant() const { 653 654 // 2x2 sub-determinants 655 float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 656 float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]; 657 float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0]; 658 float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]; 659 float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1]; 660 float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2]; 661 662 // 3x3 sub-determinants 663 float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01; 664 float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01; 665 float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02; 666 float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12; 667 668 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] ); 669 } 670 671 /* 672 ============ 673 idMat4::InverseSelf 674 ============ 675 */ 676 bool idMat4::InverseSelf() { 677 // 84+4+16 = 104 multiplications 678 // 1 division 679 double det, invDet; 680 681 // 2x2 sub-determinants required to calculate 4x4 determinant 682 float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 683 float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]; 684 float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0]; 685 float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]; 686 float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1]; 687 float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2]; 688 689 // 3x3 sub-determinants required to calculate 4x4 determinant 690 float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01; 691 float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01; 692 float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02; 693 float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12; 694 695 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] ); 696 697 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 698 return false; 699 } 700 701 invDet = 1.0f / det; 702 703 // remaining 2x2 sub-determinants 704 float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0]; 705 float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0]; 706 float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0]; 707 float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1]; 708 float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1]; 709 float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2]; 710 711 float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; 712 float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0]; 713 float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; 714 float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1]; 715 float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; 716 float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2]; 717 718 // remaining 3x3 sub-determinants 719 float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01; 720 float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01; 721 float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02; 722 float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12; 723 724 float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01; 725 float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01; 726 float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02; 727 float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12; 728 729 float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01; 730 float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01; 731 float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02; 732 float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12; 733 734 mat[0][0] = - det3_213_123 * invDet; 735 mat[1][0] = + det3_213_023 * invDet; 736 mat[2][0] = - det3_213_013 * invDet; 737 mat[3][0] = + det3_213_012 * invDet; 738 739 mat[0][1] = + det3_203_123 * invDet; 740 mat[1][1] = - det3_203_023 * invDet; 741 mat[2][1] = + det3_203_013 * invDet; 742 mat[3][1] = - det3_203_012 * invDet; 743 744 mat[0][2] = + det3_301_123 * invDet; 745 mat[1][2] = - det3_301_023 * invDet; 746 mat[2][2] = + det3_301_013 * invDet; 747 mat[3][2] = - det3_301_012 * invDet; 748 749 mat[0][3] = - det3_201_123 * invDet; 750 mat[1][3] = + det3_201_023 * invDet; 751 mat[2][3] = - det3_201_013 * invDet; 752 mat[3][3] = + det3_201_012 * invDet; 753 754 return true; 755 } 756 757 /* 758 ============ 759 idMat4::InverseFastSelf 760 ============ 761 */ 762 bool idMat4::InverseFastSelf() { 763 #if 0 764 // 84+4+16 = 104 multiplications 765 // 1 division 766 double det, invDet; 767 768 // 2x2 sub-determinants required to calculate 4x4 determinant 769 float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; 770 float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]; 771 float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0]; 772 float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]; 773 float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1]; 774 float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2]; 775 776 // 3x3 sub-determinants required to calculate 4x4 determinant 777 float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01; 778 float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01; 779 float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02; 780 float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12; 781 782 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] ); 783 784 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 785 return false; 786 } 787 788 invDet = 1.0f / det; 789 790 // remaining 2x2 sub-determinants 791 float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0]; 792 float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0]; 793 float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0]; 794 float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1]; 795 float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1]; 796 float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2]; 797 798 float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; 799 float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0]; 800 float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; 801 float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1]; 802 float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; 803 float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2]; 804 805 // remaining 3x3 sub-determinants 806 float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01; 807 float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01; 808 float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02; 809 float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12; 810 811 float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01; 812 float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01; 813 float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02; 814 float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12; 815 816 float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01; 817 float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01; 818 float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02; 819 float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12; 820 821 mat[0][0] = - det3_213_123 * invDet; 822 mat[1][0] = + det3_213_023 * invDet; 823 mat[2][0] = - det3_213_013 * invDet; 824 mat[3][0] = + det3_213_012 * invDet; 825 826 mat[0][1] = + det3_203_123 * invDet; 827 mat[1][1] = - det3_203_023 * invDet; 828 mat[2][1] = + det3_203_013 * invDet; 829 mat[3][1] = - det3_203_012 * invDet; 830 831 mat[0][2] = + det3_301_123 * invDet; 832 mat[1][2] = - det3_301_023 * invDet; 833 mat[2][2] = + det3_301_013 * invDet; 834 mat[3][2] = - det3_301_012 * invDet; 835 836 mat[0][3] = - det3_201_123 * invDet; 837 mat[1][3] = + det3_201_023 * invDet; 838 mat[2][3] = - det3_201_013 * invDet; 839 mat[3][3] = + det3_201_012 * invDet; 840 841 return true; 842 #elif 0 843 // 4*18 = 72 multiplications 844 // 4 divisions 845 float *mat = reinterpret_cast<float *>(this); 846 float s; 847 double d, di; 848 849 di = mat[0]; 850 s = di; 851 mat[0] = d = 1.0f / di; 852 mat[1] *= d; 853 mat[2] *= d; 854 mat[3] *= d; 855 d = -d; 856 mat[4] *= d; 857 mat[8] *= d; 858 mat[12] *= d; 859 d = mat[4] * di; 860 mat[5] += mat[1] * d; 861 mat[6] += mat[2] * d; 862 mat[7] += mat[3] * d; 863 d = mat[8] * di; 864 mat[9] += mat[1] * d; 865 mat[10] += mat[2] * d; 866 mat[11] += mat[3] * d; 867 d = mat[12] * di; 868 mat[13] += mat[1] * d; 869 mat[14] += mat[2] * d; 870 mat[15] += mat[3] * d; 871 di = mat[5]; 872 s *= di; 873 mat[5] = d = 1.0f / di; 874 mat[4] *= d; 875 mat[6] *= d; 876 mat[7] *= d; 877 d = -d; 878 mat[1] *= d; 879 mat[9] *= d; 880 mat[13] *= d; 881 d = mat[1] * di; 882 mat[0] += mat[4] * d; 883 mat[2] += mat[6] * d; 884 mat[3] += mat[7] * d; 885 d = mat[9] * di; 886 mat[8] += mat[4] * d; 887 mat[10] += mat[6] * d; 888 mat[11] += mat[7] * d; 889 d = mat[13] * di; 890 mat[12] += mat[4] * d; 891 mat[14] += mat[6] * d; 892 mat[15] += mat[7] * d; 893 di = mat[10]; 894 s *= di; 895 mat[10] = d = 1.0f / di; 896 mat[8] *= d; 897 mat[9] *= d; 898 mat[11] *= d; 899 d = -d; 900 mat[2] *= d; 901 mat[6] *= d; 902 mat[14] *= d; 903 d = mat[2] * di; 904 mat[0] += mat[8] * d; 905 mat[1] += mat[9] * d; 906 mat[3] += mat[11] * d; 907 d = mat[6] * di; 908 mat[4] += mat[8] * d; 909 mat[5] += mat[9] * d; 910 mat[7] += mat[11] * d; 911 d = mat[14] * di; 912 mat[12] += mat[8] * d; 913 mat[13] += mat[9] * d; 914 mat[15] += mat[11] * d; 915 di = mat[15]; 916 s *= di; 917 mat[15] = d = 1.0f / di; 918 mat[12] *= d; 919 mat[13] *= d; 920 mat[14] *= d; 921 d = -d; 922 mat[3] *= d; 923 mat[7] *= d; 924 mat[11] *= d; 925 d = mat[3] * di; 926 mat[0] += mat[12] * d; 927 mat[1] += mat[13] * d; 928 mat[2] += mat[14] * d; 929 d = mat[7] * di; 930 mat[4] += mat[12] * d; 931 mat[5] += mat[13] * d; 932 mat[6] += mat[14] * d; 933 d = mat[11] * di; 934 mat[8] += mat[12] * d; 935 mat[9] += mat[13] * d; 936 mat[10] += mat[14] * d; 937 938 return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) ); 939 #else 940 // 6*8+2*6 = 60 multiplications 941 // 2*1 = 2 divisions 942 idMat2 r0, r1, r2, r3; 943 float a, det, invDet; 944 float *mat = reinterpret_cast<float *>(this); 945 946 // r0 = m0.Inverse(); 947 det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0]; 948 949 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 950 return false; 951 } 952 953 invDet = 1.0f / det; 954 955 r0[0][0] = mat[1*4+1] * invDet; 956 r0[0][1] = - mat[0*4+1] * invDet; 957 r0[1][0] = - mat[1*4+0] * invDet; 958 r0[1][1] = mat[0*4+0] * invDet; 959 960 // r1 = r0 * m1; 961 r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2]; 962 r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3]; 963 r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2]; 964 r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3]; 965 966 // r2 = m2 * r1; 967 r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0]; 968 r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1]; 969 r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0]; 970 r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1]; 971 972 // r3 = r2 - m3; 973 r3[0][0] = r2[0][0] - mat[2*4+2]; 974 r3[0][1] = r2[0][1] - mat[2*4+3]; 975 r3[1][0] = r2[1][0] - mat[3*4+2]; 976 r3[1][1] = r2[1][1] - mat[3*4+3]; 977 978 // r3.InverseSelf(); 979 det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0]; 980 981 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 982 return false; 983 } 984 985 invDet = 1.0f / det; 986 987 a = r3[0][0]; 988 r3[0][0] = r3[1][1] * invDet; 989 r3[0][1] = - r3[0][1] * invDet; 990 r3[1][0] = - r3[1][0] * invDet; 991 r3[1][1] = a * invDet; 992 993 // r2 = m2 * r0; 994 r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0]; 995 r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1]; 996 r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0]; 997 r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1]; 998 999 // m2 = r3 * r2; 1000 mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0]; 1001 mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1]; 1002 mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0]; 1003 mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1]; 1004 1005 // m0 = r0 - r1 * m2; 1006 mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0]; 1007 mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1]; 1008 mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0]; 1009 mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1]; 1010 1011 // m1 = r1 * r3; 1012 mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0]; 1013 mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1]; 1014 mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0]; 1015 mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1]; 1016 1017 // m3 = -r3; 1018 mat[2*4+2] = -r3[0][0]; 1019 mat[2*4+3] = -r3[0][1]; 1020 mat[3*4+2] = -r3[1][0]; 1021 mat[3*4+3] = -r3[1][1]; 1022 1023 return true; 1024 #endif 1025 } 1026 1027 /* 1028 ============= 1029 idMat4::ToString 1030 ============= 1031 */ 1032 const char *idMat4::ToString( int precision ) const { 1033 return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision ); 1034 } 1035 1036 1037 //=============================================================== 1038 // 1039 // idMat5 1040 // 1041 //=============================================================== 1042 1043 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 ) ); 1044 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 ) ); 1045 1046 /* 1047 ============ 1048 idMat5::Transpose 1049 ============ 1050 */ 1051 idMat5 idMat5::Transpose() const { 1052 idMat5 transpose; 1053 int i, j; 1054 1055 for( i = 0; i < 5; i++ ) { 1056 for( j = 0; j < 5; j++ ) { 1057 transpose[ i ][ j ] = mat[ j ][ i ]; 1058 } 1059 } 1060 return transpose; 1061 } 1062 1063 /* 1064 ============ 1065 idMat5::TransposeSelf 1066 ============ 1067 */ 1068 idMat5 &idMat5::TransposeSelf() { 1069 float temp; 1070 int i, j; 1071 1072 for( i = 0; i < 5; i++ ) { 1073 for( j = i + 1; j < 5; j++ ) { 1074 temp = mat[ i ][ j ]; 1075 mat[ i ][ j ] = mat[ j ][ i ]; 1076 mat[ j ][ i ] = temp; 1077 } 1078 } 1079 return *this; 1080 } 1081 1082 /* 1083 ============ 1084 idMat5::Determinant 1085 ============ 1086 */ 1087 float idMat5::Determinant() const { 1088 1089 // 2x2 sub-determinants required to calculate 5x5 determinant 1090 float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0]; 1091 float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0]; 1092 float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0]; 1093 float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0]; 1094 float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1]; 1095 float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1]; 1096 float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1]; 1097 float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2]; 1098 float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2]; 1099 float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3]; 1100 1101 // 3x3 sub-determinants required to calculate 5x5 determinant 1102 float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01; 1103 float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01; 1104 float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01; 1105 float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02; 1106 float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02; 1107 float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03; 1108 float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12; 1109 float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12; 1110 float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13; 1111 float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23; 1112 1113 // 4x4 sub-determinants required to calculate 5x5 determinant 1114 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; 1115 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; 1116 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; 1117 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; 1118 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; 1119 1120 // determinant of 5x5 matrix 1121 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; 1122 } 1123 1124 /* 1125 ============ 1126 idMat5::InverseSelf 1127 ============ 1128 */ 1129 bool idMat5::InverseSelf() { 1130 // 280+5+25 = 310 multiplications 1131 // 1 division 1132 double det, invDet; 1133 1134 // 2x2 sub-determinants required to calculate 5x5 determinant 1135 float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0]; 1136 float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0]; 1137 float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0]; 1138 float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0]; 1139 float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1]; 1140 float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1]; 1141 float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1]; 1142 float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2]; 1143 float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2]; 1144 float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3]; 1145 1146 // 3x3 sub-determinants required to calculate 5x5 determinant 1147 float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01; 1148 float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01; 1149 float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01; 1150 float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02; 1151 float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02; 1152 float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03; 1153 float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12; 1154 float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12; 1155 float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13; 1156 float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23; 1157 1158 // 4x4 sub-determinants required to calculate 5x5 determinant 1159 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; 1160 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; 1161 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; 1162 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; 1163 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; 1164 1165 // determinant of 5x5 matrix 1166 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; 1167 1168 if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 1169 return false; 1170 } 1171 1172 invDet = 1.0f / det; 1173 1174 // remaining 2x2 sub-determinants 1175 float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; 1176 float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; 1177 float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; 1178 float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0]; 1179 float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; 1180 float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; 1181 float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1]; 1182 float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; 1183 float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2]; 1184 float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3]; 1185 float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0]; 1186 float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0]; 1187 float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0]; 1188 float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0]; 1189 float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1]; 1190 float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1]; 1191 float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1]; 1192 float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2]; 1193 float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2]; 1194 float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3]; 1195 1196 // remaining 3x3 sub-determinants 1197 float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01; 1198 float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01; 1199 float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01; 1200 float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02; 1201 float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02; 1202 float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03; 1203 float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12; 1204 float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12; 1205 float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13; 1206 float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23; 1207 float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01; 1208 float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01; 1209 float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01; 1210 float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02; 1211 float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02; 1212 float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03; 1213 float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12; 1214 float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12; 1215 float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13; 1216 float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23; 1217 float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01; 1218 float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01; 1219 float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01; 1220 float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02; 1221 float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02; 1222 float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03; 1223 float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12; 1224 float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12; 1225 float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13; 1226 float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23; 1227 1228 // remaining 4x4 sub-determinants 1229 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; 1230 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; 1231 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; 1232 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; 1233 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; 1234 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; 1235 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; 1236 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; 1237 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; 1238 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; 1239 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; 1240 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; 1241 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; 1242 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; 1243 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; 1244 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; 1245 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; 1246 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; 1247 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; 1248 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; 1249 1250 mat[0][0] = det4_1234_1234 * invDet; 1251 mat[0][1] = -det4_0234_1234 * invDet; 1252 mat[0][2] = det4_0134_1234 * invDet; 1253 mat[0][3] = -det4_0124_1234 * invDet; 1254 mat[0][4] = det4_0123_1234 * invDet; 1255 1256 mat[1][0] = -det4_1234_0234 * invDet; 1257 mat[1][1] = det4_0234_0234 * invDet; 1258 mat[1][2] = -det4_0134_0234 * invDet; 1259 mat[1][3] = det4_0124_0234 * invDet; 1260 mat[1][4] = -det4_0123_0234 * invDet; 1261 1262 mat[2][0] = det4_1234_0134 * invDet; 1263 mat[2][1] = -det4_0234_0134 * invDet; 1264 mat[2][2] = det4_0134_0134 * invDet; 1265 mat[2][3] = -det4_0124_0134 * invDet; 1266 mat[2][4] = det4_0123_0134 * invDet; 1267 1268 mat[3][0] = -det4_1234_0124 * invDet; 1269 mat[3][1] = det4_0234_0124 * invDet; 1270 mat[3][2] = -det4_0134_0124 * invDet; 1271 mat[3][3] = det4_0124_0124 * invDet; 1272 mat[3][4] = -det4_0123_0124 * invDet; 1273 1274 mat[4][0] = det4_1234_0123 * invDet; 1275 mat[4][1] = -det4_0234_0123 * invDet; 1276 mat[4][2] = det4_0134_0123 * invDet; 1277 mat[4][3] = -det4_0124_0123 * invDet; 1278 mat[4][4] = det4_0123_0123 * invDet; 1279 1280 return true; 1281 } 1282 1283 /* 1284 ============ 1285 idMat5::InverseFastSelf 1286 ============ 1287 */ 1288 bool idMat5::InverseFastSelf() { 1289 #if 0 1290 // 280+5+25 = 310 multiplications 1291 // 1 division 1292 double det, invDet; 1293 1294 // 2x2 sub-determinants required to calculate 5x5 determinant 1295 float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0]; 1296 float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0]; 1297 float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0]; 1298 float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0]; 1299 float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1]; 1300 float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1]; 1301 float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1]; 1302 float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2]; 1303 float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2]; 1304 float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3]; 1305 1306 // 3x3 sub-determinants required to calculate 5x5 determinant 1307 float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01; 1308 float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01; 1309 float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01; 1310 float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02; 1311 float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02; 1312 float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03; 1313 float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12; 1314 float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12; 1315 float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13; 1316 float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23; 1317 1318 // 4x4 sub-determinants required to calculate 5x5 determinant 1319 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; 1320 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; 1321 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; 1322 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; 1323 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; 1324 1325 // determinant of 5x5 matrix 1326 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; 1327 1328 if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 1329 return false; 1330 } 1331 1332 invDet = 1.0f / det; 1333 1334 // remaining 2x2 sub-determinants 1335 float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; 1336 float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; 1337 float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; 1338 float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0]; 1339 float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; 1340 float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; 1341 float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1]; 1342 float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; 1343 float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2]; 1344 float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3]; 1345 float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0]; 1346 float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0]; 1347 float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0]; 1348 float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0]; 1349 float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1]; 1350 float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1]; 1351 float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1]; 1352 float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2]; 1353 float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2]; 1354 float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3]; 1355 1356 // remaining 3x3 sub-determinants 1357 float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01; 1358 float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01; 1359 float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01; 1360 float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02; 1361 float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02; 1362 float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03; 1363 float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12; 1364 float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12; 1365 float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13; 1366 float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23; 1367 float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01; 1368 float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01; 1369 float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01; 1370 float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02; 1371 float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02; 1372 float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03; 1373 float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12; 1374 float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12; 1375 float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13; 1376 float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23; 1377 float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01; 1378 float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01; 1379 float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01; 1380 float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02; 1381 float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02; 1382 float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03; 1383 float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12; 1384 float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12; 1385 float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13; 1386 float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23; 1387 1388 // remaining 4x4 sub-determinants 1389 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; 1390 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; 1391 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; 1392 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; 1393 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; 1394 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; 1395 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; 1396 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; 1397 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; 1398 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; 1399 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; 1400 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; 1401 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; 1402 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; 1403 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; 1404 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; 1405 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; 1406 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; 1407 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; 1408 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; 1409 1410 mat[0][0] = det4_1234_1234 * invDet; 1411 mat[0][1] = -det4_0234_1234 * invDet; 1412 mat[0][2] = det4_0134_1234 * invDet; 1413 mat[0][3] = -det4_0124_1234 * invDet; 1414 mat[0][4] = det4_0123_1234 * invDet; 1415 1416 mat[1][0] = -det4_1234_0234 * invDet; 1417 mat[1][1] = det4_0234_0234 * invDet; 1418 mat[1][2] = -det4_0134_0234 * invDet; 1419 mat[1][3] = det4_0124_0234 * invDet; 1420 mat[1][4] = -det4_0123_0234 * invDet; 1421 1422 mat[2][0] = det4_1234_0134 * invDet; 1423 mat[2][1] = -det4_0234_0134 * invDet; 1424 mat[2][2] = det4_0134_0134 * invDet; 1425 mat[2][3] = -det4_0124_0134 * invDet; 1426 mat[2][4] = det4_0123_0134 * invDet; 1427 1428 mat[3][0] = -det4_1234_0124 * invDet; 1429 mat[3][1] = det4_0234_0124 * invDet; 1430 mat[3][2] = -det4_0134_0124 * invDet; 1431 mat[3][3] = det4_0124_0124 * invDet; 1432 mat[3][4] = -det4_0123_0124 * invDet; 1433 1434 mat[4][0] = det4_1234_0123 * invDet; 1435 mat[4][1] = -det4_0234_0123 * invDet; 1436 mat[4][2] = det4_0134_0123 * invDet; 1437 mat[4][3] = -det4_0124_0123 * invDet; 1438 mat[4][4] = det4_0123_0123 * invDet; 1439 1440 return true; 1441 #elif 0 1442 // 5*28 = 140 multiplications 1443 // 5 divisions 1444 float *mat = reinterpret_cast<float *>(this); 1445 float s; 1446 double d, di; 1447 1448 di = mat[0]; 1449 s = di; 1450 mat[0] = d = 1.0f / di; 1451 mat[1] *= d; 1452 mat[2] *= d; 1453 mat[3] *= d; 1454 mat[4] *= d; 1455 d = -d; 1456 mat[5] *= d; 1457 mat[10] *= d; 1458 mat[15] *= d; 1459 mat[20] *= d; 1460 d = mat[5] * di; 1461 mat[6] += mat[1] * d; 1462 mat[7] += mat[2] * d; 1463 mat[8] += mat[3] * d; 1464 mat[9] += mat[4] * d; 1465 d = mat[10] * di; 1466 mat[11] += mat[1] * d; 1467 mat[12] += mat[2] * d; 1468 mat[13] += mat[3] * d; 1469 mat[14] += mat[4] * d; 1470 d = mat[15] * di; 1471 mat[16] += mat[1] * d; 1472 mat[17] += mat[2] * d; 1473 mat[18] += mat[3] * d; 1474 mat[19] += mat[4] * d; 1475 d = mat[20] * di; 1476 mat[21] += mat[1] * d; 1477 mat[22] += mat[2] * d; 1478 mat[23] += mat[3] * d; 1479 mat[24] += mat[4] * d; 1480 di = mat[6]; 1481 s *= di; 1482 mat[6] = d = 1.0f / di; 1483 mat[5] *= d; 1484 mat[7] *= d; 1485 mat[8] *= d; 1486 mat[9] *= d; 1487 d = -d; 1488 mat[1] *= d; 1489 mat[11] *= d; 1490 mat[16] *= d; 1491 mat[21] *= d; 1492 d = mat[1] * di; 1493 mat[0] += mat[5] * d; 1494 mat[2] += mat[7] * d; 1495 mat[3] += mat[8] * d; 1496 mat[4] += mat[9] * d; 1497 d = mat[11] * di; 1498 mat[10] += mat[5] * d; 1499 mat[12] += mat[7] * d; 1500 mat[13] += mat[8] * d; 1501 mat[14] += mat[9] * d; 1502 d = mat[16] * di; 1503 mat[15] += mat[5] * d; 1504 mat[17] += mat[7] * d; 1505 mat[18] += mat[8] * d; 1506 mat[19] += mat[9] * d; 1507 d = mat[21] * di; 1508 mat[20] += mat[5] * d; 1509 mat[22] += mat[7] * d; 1510 mat[23] += mat[8] * d; 1511 mat[24] += mat[9] * d; 1512 di = mat[12]; 1513 s *= di; 1514 mat[12] = d = 1.0f / di; 1515 mat[10] *= d; 1516 mat[11] *= d; 1517 mat[13] *= d; 1518 mat[14] *= d; 1519 d = -d; 1520 mat[2] *= d; 1521 mat[7] *= d; 1522 mat[17] *= d; 1523 mat[22] *= d; 1524 d = mat[2] * di; 1525 mat[0] += mat[10] * d; 1526 mat[1] += mat[11] * d; 1527 mat[3] += mat[13] * d; 1528 mat[4] += mat[14] * d; 1529 d = mat[7] * di; 1530 mat[5] += mat[10] * d; 1531 mat[6] += mat[11] * d; 1532 mat[8] += mat[13] * d; 1533 mat[9] += mat[14] * d; 1534 d = mat[17] * di; 1535 mat[15] += mat[10] * d; 1536 mat[16] += mat[11] * d; 1537 mat[18] += mat[13] * d; 1538 mat[19] += mat[14] * d; 1539 d = mat[22] * di; 1540 mat[20] += mat[10] * d; 1541 mat[21] += mat[11] * d; 1542 mat[23] += mat[13] * d; 1543 mat[24] += mat[14] * d; 1544 di = mat[18]; 1545 s *= di; 1546 mat[18] = d = 1.0f / di; 1547 mat[15] *= d; 1548 mat[16] *= d; 1549 mat[17] *= d; 1550 mat[19] *= d; 1551 d = -d; 1552 mat[3] *= d; 1553 mat[8] *= d; 1554 mat[13] *= d; 1555 mat[23] *= d; 1556 d = mat[3] * di; 1557 mat[0] += mat[15] * d; 1558 mat[1] += mat[16] * d; 1559 mat[2] += mat[17] * d; 1560 mat[4] += mat[19] * d; 1561 d = mat[8] * di; 1562 mat[5] += mat[15] * d; 1563 mat[6] += mat[16] * d; 1564 mat[7] += mat[17] * d; 1565 mat[9] += mat[19] * d; 1566 d = mat[13] * di; 1567 mat[10] += mat[15] * d; 1568 mat[11] += mat[16] * d; 1569 mat[12] += mat[17] * d; 1570 mat[14] += mat[19] * d; 1571 d = mat[23] * di; 1572 mat[20] += mat[15] * d; 1573 mat[21] += mat[16] * d; 1574 mat[22] += mat[17] * d; 1575 mat[24] += mat[19] * d; 1576 di = mat[24]; 1577 s *= di; 1578 mat[24] = d = 1.0f / di; 1579 mat[20] *= d; 1580 mat[21] *= d; 1581 mat[22] *= d; 1582 mat[23] *= d; 1583 d = -d; 1584 mat[4] *= d; 1585 mat[9] *= d; 1586 mat[14] *= d; 1587 mat[19] *= d; 1588 d = mat[4] * di; 1589 mat[0] += mat[20] * d; 1590 mat[1] += mat[21] * d; 1591 mat[2] += mat[22] * d; 1592 mat[3] += mat[23] * d; 1593 d = mat[9] * di; 1594 mat[5] += mat[20] * d; 1595 mat[6] += mat[21] * d; 1596 mat[7] += mat[22] * d; 1597 mat[8] += mat[23] * d; 1598 d = mat[14] * di; 1599 mat[10] += mat[20] * d; 1600 mat[11] += mat[21] * d; 1601 mat[12] += mat[22] * d; 1602 mat[13] += mat[23] * d; 1603 d = mat[19] * di; 1604 mat[15] += mat[20] * d; 1605 mat[16] += mat[21] * d; 1606 mat[17] += mat[22] * d; 1607 mat[18] += mat[23] * d; 1608 1609 return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) ); 1610 #else 1611 // 86+30+6 = 122 multiplications 1612 // 2*1 = 2 divisions 1613 idMat3 r0, r1, r2, r3; 1614 float c0, c1, c2, det, invDet; 1615 float *mat = reinterpret_cast<float *>(this); 1616 1617 // r0 = m0.Inverse(); // 3x3 1618 c0 = mat[1*5+1] * mat[2*5+2] - mat[1*5+2] * mat[2*5+1]; 1619 c1 = mat[1*5+2] * mat[2*5+0] - mat[1*5+0] * mat[2*5+2]; 1620 c2 = mat[1*5+0] * mat[2*5+1] - mat[1*5+1] * mat[2*5+0]; 1621 1622 det = mat[0*5+0] * c0 + mat[0*5+1] * c1 + mat[0*5+2] * c2; 1623 1624 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 1625 return false; 1626 } 1627 1628 invDet = 1.0f / det; 1629 1630 r0[0][0] = c0 * invDet; 1631 r0[0][1] = ( mat[0*5+2] * mat[2*5+1] - mat[0*5+1] * mat[2*5+2] ) * invDet; 1632 r0[0][2] = ( mat[0*5+1] * mat[1*5+2] - mat[0*5+2] * mat[1*5+1] ) * invDet; 1633 r0[1][0] = c1 * invDet; 1634 r0[1][1] = ( mat[0*5+0] * mat[2*5+2] - mat[0*5+2] * mat[2*5+0] ) * invDet; 1635 r0[1][2] = ( mat[0*5+2] * mat[1*5+0] - mat[0*5+0] * mat[1*5+2] ) * invDet; 1636 r0[2][0] = c2 * invDet; 1637 r0[2][1] = ( mat[0*5+1] * mat[2*5+0] - mat[0*5+0] * mat[2*5+1] ) * invDet; 1638 r0[2][2] = ( mat[0*5+0] * mat[1*5+1] - mat[0*5+1] * mat[1*5+0] ) * invDet; 1639 1640 // r1 = r0 * m1; // 3x2 = 3x3 * 3x2 1641 r1[0][0] = r0[0][0] * mat[0*5+3] + r0[0][1] * mat[1*5+3] + r0[0][2] * mat[2*5+3]; 1642 r1[0][1] = r0[0][0] * mat[0*5+4] + r0[0][1] * mat[1*5+4] + r0[0][2] * mat[2*5+4]; 1643 r1[1][0] = r0[1][0] * mat[0*5+3] + r0[1][1] * mat[1*5+3] + r0[1][2] * mat[2*5+3]; 1644 r1[1][1] = r0[1][0] * mat[0*5+4] + r0[1][1] * mat[1*5+4] + r0[1][2] * mat[2*5+4]; 1645 r1[2][0] = r0[2][0] * mat[0*5+3] + r0[2][1] * mat[1*5+3] + r0[2][2] * mat[2*5+3]; 1646 r1[2][1] = r0[2][0] * mat[0*5+4] + r0[2][1] * mat[1*5+4] + r0[2][2] * mat[2*5+4]; 1647 1648 // r2 = m2 * r1; // 2x2 = 2x3 * 3x2 1649 r2[0][0] = mat[3*5+0] * r1[0][0] + mat[3*5+1] * r1[1][0] + mat[3*5+2] * r1[2][0]; 1650 r2[0][1] = mat[3*5+0] * r1[0][1] + mat[3*5+1] * r1[1][1] + mat[3*5+2] * r1[2][1]; 1651 r2[1][0] = mat[4*5+0] * r1[0][0] + mat[4*5+1] * r1[1][0] + mat[4*5+2] * r1[2][0]; 1652 r2[1][1] = mat[4*5+0] * r1[0][1] + mat[4*5+1] * r1[1][1] + mat[4*5+2] * r1[2][1]; 1653 1654 // r3 = r2 - m3; // 2x2 = 2x2 - 2x2 1655 r3[0][0] = r2[0][0] - mat[3*5+3]; 1656 r3[0][1] = r2[0][1] - mat[3*5+4]; 1657 r3[1][0] = r2[1][0] - mat[4*5+3]; 1658 r3[1][1] = r2[1][1] - mat[4*5+4]; 1659 1660 // r3.InverseSelf(); // 2x2 1661 det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0]; 1662 1663 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 1664 return false; 1665 } 1666 1667 invDet = 1.0f / det; 1668 1669 c0 = r3[0][0]; 1670 r3[0][0] = r3[1][1] * invDet; 1671 r3[0][1] = - r3[0][1] * invDet; 1672 r3[1][0] = - r3[1][0] * invDet; 1673 r3[1][1] = c0 * invDet; 1674 1675 // r2 = m2 * r0; // 2x3 = 2x3 * 3x3 1676 r2[0][0] = mat[3*5+0] * r0[0][0] + mat[3*5+1] * r0[1][0] + mat[3*5+2] * r0[2][0]; 1677 r2[0][1] = mat[3*5+0] * r0[0][1] + mat[3*5+1] * r0[1][1] + mat[3*5+2] * r0[2][1]; 1678 r2[0][2] = mat[3*5+0] * r0[0][2] + mat[3*5+1] * r0[1][2] + mat[3*5+2] * r0[2][2]; 1679 r2[1][0] = mat[4*5+0] * r0[0][0] + mat[4*5+1] * r0[1][0] + mat[4*5+2] * r0[2][0]; 1680 r2[1][1] = mat[4*5+0] * r0[0][1] + mat[4*5+1] * r0[1][1] + mat[4*5+2] * r0[2][1]; 1681 r2[1][2] = mat[4*5+0] * r0[0][2] + mat[4*5+1] * r0[1][2] + mat[4*5+2] * r0[2][2]; 1682 1683 // m2 = r3 * r2; // 2x3 = 2x2 * 2x3 1684 mat[3*5+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0]; 1685 mat[3*5+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1]; 1686 mat[3*5+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2]; 1687 mat[4*5+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0]; 1688 mat[4*5+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1]; 1689 mat[4*5+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2]; 1690 1691 // m0 = r0 - r1 * m2; // 3x3 = 3x3 - 3x2 * 2x3 1692 mat[0*5+0] = r0[0][0] - r1[0][0] * mat[3*5+0] - r1[0][1] * mat[4*5+0]; 1693 mat[0*5+1] = r0[0][1] - r1[0][0] * mat[3*5+1] - r1[0][1] * mat[4*5+1]; 1694 mat[0*5+2] = r0[0][2] - r1[0][0] * mat[3*5+2] - r1[0][1] * mat[4*5+2]; 1695 mat[1*5+0] = r0[1][0] - r1[1][0] * mat[3*5+0] - r1[1][1] * mat[4*5+0]; 1696 mat[1*5+1] = r0[1][1] - r1[1][0] * mat[3*5+1] - r1[1][1] * mat[4*5+1]; 1697 mat[1*5+2] = r0[1][2] - r1[1][0] * mat[3*5+2] - r1[1][1] * mat[4*5+2]; 1698 mat[2*5+0] = r0[2][0] - r1[2][0] * mat[3*5+0] - r1[2][1] * mat[4*5+0]; 1699 mat[2*5+1] = r0[2][1] - r1[2][0] * mat[3*5+1] - r1[2][1] * mat[4*5+1]; 1700 mat[2*5+2] = r0[2][2] - r1[2][0] * mat[3*5+2] - r1[2][1] * mat[4*5+2]; 1701 1702 // m1 = r1 * r3; // 3x2 = 3x2 * 2x2 1703 mat[0*5+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0]; 1704 mat[0*5+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1]; 1705 mat[1*5+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0]; 1706 mat[1*5+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1]; 1707 mat[2*5+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0]; 1708 mat[2*5+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1]; 1709 1710 // m3 = -r3; // 2x2 = - 2x2 1711 mat[3*5+3] = -r3[0][0]; 1712 mat[3*5+4] = -r3[0][1]; 1713 mat[4*5+3] = -r3[1][0]; 1714 mat[4*5+4] = -r3[1][1]; 1715 1716 return true; 1717 #endif 1718 } 1719 1720 /* 1721 ============= 1722 idMat5::ToString 1723 ============= 1724 */ 1725 const char *idMat5::ToString( int precision ) const { 1726 return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision ); 1727 } 1728 1729 1730 //=============================================================== 1731 // 1732 // idMat6 1733 // 1734 //=============================================================== 1735 1736 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 ) ); 1737 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 ) ); 1738 1739 /* 1740 ============ 1741 idMat6::Transpose 1742 ============ 1743 */ 1744 idMat6 idMat6::Transpose() const { 1745 idMat6 transpose; 1746 int i, j; 1747 1748 for( i = 0; i < 6; i++ ) { 1749 for( j = 0; j < 6; j++ ) { 1750 transpose[ i ][ j ] = mat[ j ][ i ]; 1751 } 1752 } 1753 return transpose; 1754 } 1755 1756 /* 1757 ============ 1758 idMat6::TransposeSelf 1759 ============ 1760 */ 1761 idMat6 &idMat6::TransposeSelf() { 1762 float temp; 1763 int i, j; 1764 1765 for( i = 0; i < 6; i++ ) { 1766 for( j = i + 1; j < 6; j++ ) { 1767 temp = mat[ i ][ j ]; 1768 mat[ i ][ j ] = mat[ j ][ i ]; 1769 mat[ j ][ i ] = temp; 1770 } 1771 } 1772 return *this; 1773 } 1774 1775 /* 1776 ============ 1777 idMat6::Determinant 1778 ============ 1779 */ 1780 float idMat6::Determinant() const { 1781 1782 // 2x2 sub-determinants required to calculate 6x6 determinant 1783 float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0]; 1784 float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0]; 1785 float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0]; 1786 float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0]; 1787 float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0]; 1788 float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1]; 1789 float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1]; 1790 float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1]; 1791 float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1]; 1792 float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2]; 1793 float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2]; 1794 float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2]; 1795 float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3]; 1796 float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3]; 1797 float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4]; 1798 1799 // 3x3 sub-determinants required to calculate 6x6 determinant 1800 float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01; 1801 float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01; 1802 float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01; 1803 float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01; 1804 float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02; 1805 float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02; 1806 float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02; 1807 float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03; 1808 float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03; 1809 float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04; 1810 float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12; 1811 float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12; 1812 float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12; 1813 float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13; 1814 float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13; 1815 float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14; 1816 float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23; 1817 float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23; 1818 float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24; 1819 float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34; 1820 1821 // 4x4 sub-determinants required to calculate 6x6 determinant 1822 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; 1823 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; 1824 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; 1825 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; 1826 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; 1827 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; 1828 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; 1829 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; 1830 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; 1831 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; 1832 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; 1833 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; 1834 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; 1835 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; 1836 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; 1837 1838 // 5x5 sub-determinants required to calculate 6x6 determinant 1839 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; 1840 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; 1841 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; 1842 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; 1843 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; 1844 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; 1845 1846 // determinant of 6x6 matrix 1847 return mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 - 1848 mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234; 1849 } 1850 1851 /* 1852 ============ 1853 idMat6::InverseSelf 1854 ============ 1855 */ 1856 bool idMat6::InverseSelf() { 1857 // 810+6+36 = 852 multiplications 1858 // 1 division 1859 double det, invDet; 1860 1861 // 2x2 sub-determinants required to calculate 6x6 determinant 1862 float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0]; 1863 float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0]; 1864 float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0]; 1865 float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0]; 1866 float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0]; 1867 float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1]; 1868 float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1]; 1869 float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1]; 1870 float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1]; 1871 float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2]; 1872 float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2]; 1873 float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2]; 1874 float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3]; 1875 float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3]; 1876 float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4]; 1877 1878 // 3x3 sub-determinants required to calculate 6x6 determinant 1879 float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01; 1880 float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01; 1881 float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01; 1882 float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01; 1883 float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02; 1884 float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02; 1885 float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02; 1886 float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03; 1887 float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03; 1888 float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04; 1889 float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12; 1890 float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12; 1891 float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12; 1892 float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13; 1893 float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13; 1894 float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14; 1895 float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23; 1896 float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23; 1897 float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24; 1898 float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34; 1899 1900 // 4x4 sub-determinants required to calculate 6x6 determinant 1901 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; 1902 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; 1903 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; 1904 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; 1905 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; 1906 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; 1907 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; 1908 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; 1909 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; 1910 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; 1911 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; 1912 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; 1913 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; 1914 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; 1915 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; 1916 1917 // 5x5 sub-determinants required to calculate 6x6 determinant 1918 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; 1919 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; 1920 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; 1921 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; 1922 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; 1923 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; 1924 1925 // determinant of 6x6 matrix 1926 det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 - 1927 mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234; 1928 1929 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 1930 return false; 1931 } 1932 1933 invDet = 1.0f / det; 1934 1935 // remaining 2x2 sub-determinants 1936 float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0]; 1937 float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0]; 1938 float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0]; 1939 float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0]; 1940 float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0]; 1941 float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1]; 1942 float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1]; 1943 float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1]; 1944 float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1]; 1945 float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2]; 1946 float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2]; 1947 float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2]; 1948 float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3]; 1949 float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3]; 1950 float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4]; 1951 float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0]; 1952 float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0]; 1953 float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0]; 1954 float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0]; 1955 float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0]; 1956 float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1]; 1957 float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1]; 1958 float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1]; 1959 float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1]; 1960 float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2]; 1961 float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2]; 1962 float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2]; 1963 float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3]; 1964 float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3]; 1965 float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4]; 1966 1967 // remaining 3x3 sub-determinants 1968 float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01; 1969 float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01; 1970 float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01; 1971 float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01; 1972 float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02; 1973 float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02; 1974 float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02; 1975 float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03; 1976 float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03; 1977 float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04; 1978 float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12; 1979 float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12; 1980 float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12; 1981 float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13; 1982 float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13; 1983 float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14; 1984 float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23; 1985 float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23; 1986 float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24; 1987 float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34; 1988 float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01; 1989 float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01; 1990 float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01; 1991 float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01; 1992 float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02; 1993 float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02; 1994 float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02; 1995 float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03; 1996 float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03; 1997 float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04; 1998 float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12; 1999 float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12; 2000 float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12; 2001 float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13; 2002 float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13; 2003 float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14; 2004 float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23; 2005 float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23; 2006 float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24; 2007 float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34; 2008 float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01; 2009 float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01; 2010 float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01; 2011 float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01; 2012 float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02; 2013 float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02; 2014 float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02; 2015 float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03; 2016 float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03; 2017 float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04; 2018 float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12; 2019 float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12; 2020 float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12; 2021 float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13; 2022 float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13; 2023 float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14; 2024 float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23; 2025 float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23; 2026 float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24; 2027 float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34; 2028 2029 // remaining 4x4 sub-determinants 2030 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; 2031 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; 2032 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; 2033 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; 2034 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; 2035 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; 2036 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; 2037 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; 2038 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; 2039 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; 2040 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; 2041 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; 2042 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; 2043 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; 2044 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; 2045 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; 2046 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; 2047 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; 2048 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; 2049 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; 2050 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; 2051 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; 2052 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; 2053 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; 2054 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; 2055 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; 2056 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; 2057 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; 2058 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; 2059 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; 2060 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; 2061 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; 2062 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; 2063 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; 2064 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; 2065 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; 2066 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; 2067 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; 2068 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; 2069 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; 2070 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; 2071 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; 2072 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; 2073 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; 2074 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; 2075 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; 2076 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; 2077 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; 2078 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; 2079 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; 2080 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; 2081 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; 2082 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; 2083 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; 2084 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; 2085 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; 2086 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; 2087 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; 2088 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; 2089 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; 2090 2091 // remaining 5x5 sub-determinants 2092 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; 2093 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; 2094 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; 2095 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; 2096 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; 2097 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; 2098 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; 2099 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; 2100 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; 2101 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; 2102 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; 2103 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; 2104 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; 2105 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; 2106 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; 2107 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; 2108 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; 2109 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; 2110 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; 2111 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; 2112 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; 2113 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; 2114 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; 2115 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; 2116 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; 2117 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; 2118 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; 2119 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; 2120 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; 2121 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; 2122 2123 mat[0][0] = det5_12345_12345 * invDet; 2124 mat[0][1] = -det5_02345_12345 * invDet; 2125 mat[0][2] = det5_01345_12345 * invDet; 2126 mat[0][3] = -det5_01245_12345 * invDet; 2127 mat[0][4] = det5_01235_12345 * invDet; 2128 mat[0][5] = -det5_01234_12345 * invDet; 2129 2130 mat[1][0] = -det5_12345_02345 * invDet; 2131 mat[1][1] = det5_02345_02345 * invDet; 2132 mat[1][2] = -det5_01345_02345 * invDet; 2133 mat[1][3] = det5_01245_02345 * invDet; 2134 mat[1][4] = -det5_01235_02345 * invDet; 2135 mat[1][5] = det5_01234_02345 * invDet; 2136 2137 mat[2][0] = det5_12345_01345 * invDet; 2138 mat[2][1] = -det5_02345_01345 * invDet; 2139 mat[2][2] = det5_01345_01345 * invDet; 2140 mat[2][3] = -det5_01245_01345 * invDet; 2141 mat[2][4] = det5_01235_01345 * invDet; 2142 mat[2][5] = -det5_01234_01345 * invDet; 2143 2144 mat[3][0] = -det5_12345_01245 * invDet; 2145 mat[3][1] = det5_02345_01245 * invDet; 2146 mat[3][2] = -det5_01345_01245 * invDet; 2147 mat[3][3] = det5_01245_01245 * invDet; 2148 mat[3][4] = -det5_01235_01245 * invDet; 2149 mat[3][5] = det5_01234_01245 * invDet; 2150 2151 mat[4][0] = det5_12345_01235 * invDet; 2152 mat[4][1] = -det5_02345_01235 * invDet; 2153 mat[4][2] = det5_01345_01235 * invDet; 2154 mat[4][3] = -det5_01245_01235 * invDet; 2155 mat[4][4] = det5_01235_01235 * invDet; 2156 mat[4][5] = -det5_01234_01235 * invDet; 2157 2158 mat[5][0] = -det5_12345_01234 * invDet; 2159 mat[5][1] = det5_02345_01234 * invDet; 2160 mat[5][2] = -det5_01345_01234 * invDet; 2161 mat[5][3] = det5_01245_01234 * invDet; 2162 mat[5][4] = -det5_01235_01234 * invDet; 2163 mat[5][5] = det5_01234_01234 * invDet; 2164 2165 return true; 2166 } 2167 2168 /* 2169 ============ 2170 idMat6::InverseFastSelf 2171 ============ 2172 */ 2173 bool idMat6::InverseFastSelf() { 2174 #if 0 2175 // 810+6+36 = 852 multiplications 2176 // 1 division 2177 double det, invDet; 2178 2179 // 2x2 sub-determinants required to calculate 6x6 determinant 2180 float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0]; 2181 float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0]; 2182 float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0]; 2183 float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0]; 2184 float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0]; 2185 float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1]; 2186 float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1]; 2187 float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1]; 2188 float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1]; 2189 float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2]; 2190 float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2]; 2191 float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2]; 2192 float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3]; 2193 float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3]; 2194 float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4]; 2195 2196 // 3x3 sub-determinants required to calculate 6x6 determinant 2197 float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01; 2198 float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01; 2199 float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01; 2200 float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01; 2201 float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02; 2202 float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02; 2203 float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02; 2204 float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03; 2205 float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03; 2206 float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04; 2207 float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12; 2208 float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12; 2209 float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12; 2210 float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13; 2211 float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13; 2212 float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14; 2213 float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23; 2214 float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23; 2215 float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24; 2216 float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34; 2217 2218 // 4x4 sub-determinants required to calculate 6x6 determinant 2219 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; 2220 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; 2221 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; 2222 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; 2223 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; 2224 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; 2225 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; 2226 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; 2227 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; 2228 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; 2229 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; 2230 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; 2231 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; 2232 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; 2233 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; 2234 2235 // 5x5 sub-determinants required to calculate 6x6 determinant 2236 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; 2237 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; 2238 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; 2239 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; 2240 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; 2241 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; 2242 2243 // determinant of 6x6 matrix 2244 det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 - 2245 mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234; 2246 2247 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 2248 return false; 2249 } 2250 2251 invDet = 1.0f / det; 2252 2253 // remaining 2x2 sub-determinants 2254 float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0]; 2255 float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0]; 2256 float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0]; 2257 float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0]; 2258 float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0]; 2259 float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1]; 2260 float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1]; 2261 float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1]; 2262 float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1]; 2263 float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2]; 2264 float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2]; 2265 float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2]; 2266 float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3]; 2267 float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3]; 2268 float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4]; 2269 float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0]; 2270 float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0]; 2271 float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0]; 2272 float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0]; 2273 float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0]; 2274 float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1]; 2275 float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1]; 2276 float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1]; 2277 float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1]; 2278 float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2]; 2279 float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2]; 2280 float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2]; 2281 float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3]; 2282 float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3]; 2283 float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4]; 2284 2285 // remaining 3x3 sub-determinants 2286 float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01; 2287 float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01; 2288 float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01; 2289 float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01; 2290 float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02; 2291 float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02; 2292 float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02; 2293 float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03; 2294 float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03; 2295 float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04; 2296 float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12; 2297 float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12; 2298 float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12; 2299 float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13; 2300 float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13; 2301 float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14; 2302 float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23; 2303 float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23; 2304 float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24; 2305 float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34; 2306 float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01; 2307 float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01; 2308 float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01; 2309 float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01; 2310 float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02; 2311 float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02; 2312 float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02; 2313 float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03; 2314 float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03; 2315 float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04; 2316 float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12; 2317 float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12; 2318 float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12; 2319 float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13; 2320 float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13; 2321 float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14; 2322 float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23; 2323 float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23; 2324 float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24; 2325 float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34; 2326 float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01; 2327 float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01; 2328 float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01; 2329 float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01; 2330 float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02; 2331 float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02; 2332 float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02; 2333 float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03; 2334 float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03; 2335 float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04; 2336 float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12; 2337 float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12; 2338 float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12; 2339 float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13; 2340 float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13; 2341 float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14; 2342 float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23; 2343 float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23; 2344 float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24; 2345 float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34; 2346 2347 // remaining 4x4 sub-determinants 2348 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; 2349 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; 2350 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; 2351 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; 2352 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; 2353 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; 2354 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; 2355 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; 2356 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; 2357 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; 2358 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; 2359 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; 2360 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; 2361 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; 2362 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; 2363 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; 2364 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; 2365 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; 2366 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; 2367 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; 2368 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; 2369 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; 2370 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; 2371 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; 2372 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; 2373 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; 2374 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; 2375 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; 2376 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; 2377 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; 2378 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; 2379 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; 2380 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; 2381 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; 2382 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; 2383 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; 2384 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; 2385 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; 2386 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; 2387 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; 2388 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; 2389 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; 2390 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; 2391 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; 2392 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; 2393 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; 2394 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; 2395 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; 2396 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; 2397 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; 2398 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; 2399 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; 2400 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; 2401 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; 2402 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; 2403 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; 2404 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; 2405 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; 2406 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; 2407 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; 2408 2409 // remaining 5x5 sub-determinants 2410 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; 2411 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; 2412 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; 2413 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; 2414 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; 2415 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; 2416 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; 2417 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; 2418 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; 2419 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; 2420 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; 2421 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; 2422 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; 2423 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; 2424 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; 2425 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; 2426 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; 2427 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; 2428 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; 2429 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; 2430 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; 2431 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; 2432 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; 2433 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; 2434 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; 2435 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; 2436 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; 2437 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; 2438 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; 2439 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; 2440 2441 mat[0][0] = det5_12345_12345 * invDet; 2442 mat[0][1] = -det5_02345_12345 * invDet; 2443 mat[0][2] = det5_01345_12345 * invDet; 2444 mat[0][3] = -det5_01245_12345 * invDet; 2445 mat[0][4] = det5_01235_12345 * invDet; 2446 mat[0][5] = -det5_01234_12345 * invDet; 2447 2448 mat[1][0] = -det5_12345_02345 * invDet; 2449 mat[1][1] = det5_02345_02345 * invDet; 2450 mat[1][2] = -det5_01345_02345 * invDet; 2451 mat[1][3] = det5_01245_02345 * invDet; 2452 mat[1][4] = -det5_01235_02345 * invDet; 2453 mat[1][5] = det5_01234_02345 * invDet; 2454 2455 mat[2][0] = det5_12345_01345 * invDet; 2456 mat[2][1] = -det5_02345_01345 * invDet; 2457 mat[2][2] = det5_01345_01345 * invDet; 2458 mat[2][3] = -det5_01245_01345 * invDet; 2459 mat[2][4] = det5_01235_01345 * invDet; 2460 mat[2][5] = -det5_01234_01345 * invDet; 2461 2462 mat[3][0] = -det5_12345_01245 * invDet; 2463 mat[3][1] = det5_02345_01245 * invDet; 2464 mat[3][2] = -det5_01345_01245 * invDet; 2465 mat[3][3] = det5_01245_01245 * invDet; 2466 mat[3][4] = -det5_01235_01245 * invDet; 2467 mat[3][5] = det5_01234_01245 * invDet; 2468 2469 mat[4][0] = det5_12345_01235 * invDet; 2470 mat[4][1] = -det5_02345_01235 * invDet; 2471 mat[4][2] = det5_01345_01235 * invDet; 2472 mat[4][3] = -det5_01245_01235 * invDet; 2473 mat[4][4] = det5_01235_01235 * invDet; 2474 mat[4][5] = -det5_01234_01235 * invDet; 2475 2476 mat[5][0] = -det5_12345_01234 * invDet; 2477 mat[5][1] = det5_02345_01234 * invDet; 2478 mat[5][2] = -det5_01345_01234 * invDet; 2479 mat[5][3] = det5_01245_01234 * invDet; 2480 mat[5][4] = -det5_01235_01234 * invDet; 2481 mat[5][5] = det5_01234_01234 * invDet; 2482 2483 return true; 2484 #elif 0 2485 // 6*40 = 240 multiplications 2486 // 6 divisions 2487 float *mat = reinterpret_cast<float *>(this); 2488 float s; 2489 double d, di; 2490 2491 di = mat[0]; 2492 s = di; 2493 mat[0] = d = 1.0f / di; 2494 mat[1] *= d; 2495 mat[2] *= d; 2496 mat[3] *= d; 2497 mat[4] *= d; 2498 mat[5] *= d; 2499 d = -d; 2500 mat[6] *= d; 2501 mat[12] *= d; 2502 mat[18] *= d; 2503 mat[24] *= d; 2504 mat[30] *= d; 2505 d = mat[6] * di; 2506 mat[7] += mat[1] * d; 2507 mat[8] += mat[2] * d; 2508 mat[9] += mat[3] * d; 2509 mat[10] += mat[4] * d; 2510 mat[11] += mat[5] * d; 2511 d = mat[12] * di; 2512 mat[13] += mat[1] * d; 2513 mat[14] += mat[2] * d; 2514 mat[15] += mat[3] * d; 2515 mat[16] += mat[4] * d; 2516 mat[17] += mat[5] * d; 2517 d = mat[18] * di; 2518 mat[19] += mat[1] * d; 2519 mat[20] += mat[2] * d; 2520 mat[21] += mat[3] * d; 2521 mat[22] += mat[4] * d; 2522 mat[23] += mat[5] * d; 2523 d = mat[24] * di; 2524 mat[25] += mat[1] * d; 2525 mat[26] += mat[2] * d; 2526 mat[27] += mat[3] * d; 2527 mat[28] += mat[4] * d; 2528 mat[29] += mat[5] * d; 2529 d = mat[30] * di; 2530 mat[31] += mat[1] * d; 2531 mat[32] += mat[2] * d; 2532 mat[33] += mat[3] * d; 2533 mat[34] += mat[4] * d; 2534 mat[35] += mat[5] * d; 2535 di = mat[7]; 2536 s *= di; 2537 mat[7] = d = 1.0f / di; 2538 mat[6] *= d; 2539 mat[8] *= d; 2540 mat[9] *= d; 2541 mat[10] *= d; 2542 mat[11] *= d; 2543 d = -d; 2544 mat[1] *= d; 2545 mat[13] *= d; 2546 mat[19] *= d; 2547 mat[25] *= d; 2548 mat[31] *= d; 2549 d = mat[1] * di; 2550 mat[0] += mat[6] * d; 2551 mat[2] += mat[8] * d; 2552 mat[3] += mat[9] * d; 2553 mat[4] += mat[10] * d; 2554 mat[5] += mat[11] * d; 2555 d = mat[13] * di; 2556 mat[12] += mat[6] * d; 2557 mat[14] += mat[8] * d; 2558 mat[15] += mat[9] * d; 2559 mat[16] += mat[10] * d; 2560 mat[17] += mat[11] * d; 2561 d = mat[19] * di; 2562 mat[18] += mat[6] * d; 2563 mat[20] += mat[8] * d; 2564 mat[21] += mat[9] * d; 2565 mat[22] += mat[10] * d; 2566 mat[23] += mat[11] * d; 2567 d = mat[25] * di; 2568 mat[24] += mat[6] * d; 2569 mat[26] += mat[8] * d; 2570 mat[27] += mat[9] * d; 2571 mat[28] += mat[10] * d; 2572 mat[29] += mat[11] * d; 2573 d = mat[31] * di; 2574 mat[30] += mat[6] * d; 2575 mat[32] += mat[8] * d; 2576 mat[33] += mat[9] * d; 2577 mat[34] += mat[10] * d; 2578 mat[35] += mat[11] * d; 2579 di = mat[14]; 2580 s *= di; 2581 mat[14] = d = 1.0f / di; 2582 mat[12] *= d; 2583 mat[13] *= d; 2584 mat[15] *= d; 2585 mat[16] *= d; 2586 mat[17] *= d; 2587 d = -d; 2588 mat[2] *= d; 2589 mat[8] *= d; 2590 mat[20] *= d; 2591 mat[26] *= d; 2592 mat[32] *= d; 2593 d = mat[2] * di; 2594 mat[0] += mat[12] * d; 2595 mat[1] += mat[13] * d; 2596 mat[3] += mat[15] * d; 2597 mat[4] += mat[16] * d; 2598 mat[5] += mat[17] * d; 2599 d = mat[8] * di; 2600 mat[6] += mat[12] * d; 2601 mat[7] += mat[13] * d; 2602 mat[9] += mat[15] * d; 2603 mat[10] += mat[16] * d; 2604 mat[11] += mat[17] * d; 2605 d = mat[20] * di; 2606 mat[18] += mat[12] * d; 2607 mat[19] += mat[13] * d; 2608 mat[21] += mat[15] * d; 2609 mat[22] += mat[16] * d; 2610 mat[23] += mat[17] * d; 2611 d = mat[26] * di; 2612 mat[24] += mat[12] * d; 2613 mat[25] += mat[13] * d; 2614 mat[27] += mat[15] * d; 2615 mat[28] += mat[16] * d; 2616 mat[29] += mat[17] * d; 2617 d = mat[32] * di; 2618 mat[30] += mat[12] * d; 2619 mat[31] += mat[13] * d; 2620 mat[33] += mat[15] * d; 2621 mat[34] += mat[16] * d; 2622 mat[35] += mat[17] * d; 2623 di = mat[21]; 2624 s *= di; 2625 mat[21] = d = 1.0f / di; 2626 mat[18] *= d; 2627 mat[19] *= d; 2628 mat[20] *= d; 2629 mat[22] *= d; 2630 mat[23] *= d; 2631 d = -d; 2632 mat[3] *= d; 2633 mat[9] *= d; 2634 mat[15] *= d; 2635 mat[27] *= d; 2636 mat[33] *= d; 2637 d = mat[3] * di; 2638 mat[0] += mat[18] * d; 2639 mat[1] += mat[19] * d; 2640 mat[2] += mat[20] * d; 2641 mat[4] += mat[22] * d; 2642 mat[5] += mat[23] * d; 2643 d = mat[9] * di; 2644 mat[6] += mat[18] * d; 2645 mat[7] += mat[19] * d; 2646 mat[8] += mat[20] * d; 2647 mat[10] += mat[22] * d; 2648 mat[11] += mat[23] * d; 2649 d = mat[15] * di; 2650 mat[12] += mat[18] * d; 2651 mat[13] += mat[19] * d; 2652 mat[14] += mat[20] * d; 2653 mat[16] += mat[22] * d; 2654 mat[17] += mat[23] * d; 2655 d = mat[27] * di; 2656 mat[24] += mat[18] * d; 2657 mat[25] += mat[19] * d; 2658 mat[26] += mat[20] * d; 2659 mat[28] += mat[22] * d; 2660 mat[29] += mat[23] * d; 2661 d = mat[33] * di; 2662 mat[30] += mat[18] * d; 2663 mat[31] += mat[19] * d; 2664 mat[32] += mat[20] * d; 2665 mat[34] += mat[22] * d; 2666 mat[35] += mat[23] * d; 2667 di = mat[28]; 2668 s *= di; 2669 mat[28] = d = 1.0f / di; 2670 mat[24] *= d; 2671 mat[25] *= d; 2672 mat[26] *= d; 2673 mat[27] *= d; 2674 mat[29] *= d; 2675 d = -d; 2676 mat[4] *= d; 2677 mat[10] *= d; 2678 mat[16] *= d; 2679 mat[22] *= d; 2680 mat[34] *= d; 2681 d = mat[4] * di; 2682 mat[0] += mat[24] * d; 2683 mat[1] += mat[25] * d; 2684 mat[2] += mat[26] * d; 2685 mat[3] += mat[27] * d; 2686 mat[5] += mat[29] * d; 2687 d = mat[10] * di; 2688 mat[6] += mat[24] * d; 2689 mat[7] += mat[25] * d; 2690 mat[8] += mat[26] * d; 2691 mat[9] += mat[27] * d; 2692 mat[11] += mat[29] * d; 2693 d = mat[16] * di; 2694 mat[12] += mat[24] * d; 2695 mat[13] += mat[25] * d; 2696 mat[14] += mat[26] * d; 2697 mat[15] += mat[27] * d; 2698 mat[17] += mat[29] * d; 2699 d = mat[22] * di; 2700 mat[18] += mat[24] * d; 2701 mat[19] += mat[25] * d; 2702 mat[20] += mat[26] * d; 2703 mat[21] += mat[27] * d; 2704 mat[23] += mat[29] * d; 2705 d = mat[34] * di; 2706 mat[30] += mat[24] * d; 2707 mat[31] += mat[25] * d; 2708 mat[32] += mat[26] * d; 2709 mat[33] += mat[27] * d; 2710 mat[35] += mat[29] * d; 2711 di = mat[35]; 2712 s *= di; 2713 mat[35] = d = 1.0f / di; 2714 mat[30] *= d; 2715 mat[31] *= d; 2716 mat[32] *= d; 2717 mat[33] *= d; 2718 mat[34] *= d; 2719 d = -d; 2720 mat[5] *= d; 2721 mat[11] *= d; 2722 mat[17] *= d; 2723 mat[23] *= d; 2724 mat[29] *= d; 2725 d = mat[5] * di; 2726 mat[0] += mat[30] * d; 2727 mat[1] += mat[31] * d; 2728 mat[2] += mat[32] * d; 2729 mat[3] += mat[33] * d; 2730 mat[4] += mat[34] * d; 2731 d = mat[11] * di; 2732 mat[6] += mat[30] * d; 2733 mat[7] += mat[31] * d; 2734 mat[8] += mat[32] * d; 2735 mat[9] += mat[33] * d; 2736 mat[10] += mat[34] * d; 2737 d = mat[17] * di; 2738 mat[12] += mat[30] * d; 2739 mat[13] += mat[31] * d; 2740 mat[14] += mat[32] * d; 2741 mat[15] += mat[33] * d; 2742 mat[16] += mat[34] * d; 2743 d = mat[23] * di; 2744 mat[18] += mat[30] * d; 2745 mat[19] += mat[31] * d; 2746 mat[20] += mat[32] * d; 2747 mat[21] += mat[33] * d; 2748 mat[22] += mat[34] * d; 2749 d = mat[29] * di; 2750 mat[24] += mat[30] * d; 2751 mat[25] += mat[31] * d; 2752 mat[26] += mat[32] * d; 2753 mat[27] += mat[33] * d; 2754 mat[28] += mat[34] * d; 2755 2756 return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) ); 2757 #else 2758 // 6*27+2*30 = 222 multiplications 2759 // 2*1 = 2 divisions 2760 idMat3 r0, r1, r2, r3; 2761 float c0, c1, c2, det, invDet; 2762 float *mat = reinterpret_cast<float *>(this); 2763 2764 // r0 = m0.Inverse(); 2765 c0 = mat[1*6+1] * mat[2*6+2] - mat[1*6+2] * mat[2*6+1]; 2766 c1 = mat[1*6+2] * mat[2*6+0] - mat[1*6+0] * mat[2*6+2]; 2767 c2 = mat[1*6+0] * mat[2*6+1] - mat[1*6+1] * mat[2*6+0]; 2768 2769 det = mat[0*6+0] * c0 + mat[0*6+1] * c1 + mat[0*6+2] * c2; 2770 2771 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 2772 return false; 2773 } 2774 2775 invDet = 1.0f / det; 2776 2777 r0[0][0] = c0 * invDet; 2778 r0[0][1] = ( mat[0*6+2] * mat[2*6+1] - mat[0*6+1] * mat[2*6+2] ) * invDet; 2779 r0[0][2] = ( mat[0*6+1] * mat[1*6+2] - mat[0*6+2] * mat[1*6+1] ) * invDet; 2780 r0[1][0] = c1 * invDet; 2781 r0[1][1] = ( mat[0*6+0] * mat[2*6+2] - mat[0*6+2] * mat[2*6+0] ) * invDet; 2782 r0[1][2] = ( mat[0*6+2] * mat[1*6+0] - mat[0*6+0] * mat[1*6+2] ) * invDet; 2783 r0[2][0] = c2 * invDet; 2784 r0[2][1] = ( mat[0*6+1] * mat[2*6+0] - mat[0*6+0] * mat[2*6+1] ) * invDet; 2785 r0[2][2] = ( mat[0*6+0] * mat[1*6+1] - mat[0*6+1] * mat[1*6+0] ) * invDet; 2786 2787 // r1 = r0 * m1; 2788 r1[0][0] = r0[0][0] * mat[0*6+3] + r0[0][1] * mat[1*6+3] + r0[0][2] * mat[2*6+3]; 2789 r1[0][1] = r0[0][0] * mat[0*6+4] + r0[0][1] * mat[1*6+4] + r0[0][2] * mat[2*6+4]; 2790 r1[0][2] = r0[0][0] * mat[0*6+5] + r0[0][1] * mat[1*6+5] + r0[0][2] * mat[2*6+5]; 2791 r1[1][0] = r0[1][0] * mat[0*6+3] + r0[1][1] * mat[1*6+3] + r0[1][2] * mat[2*6+3]; 2792 r1[1][1] = r0[1][0] * mat[0*6+4] + r0[1][1] * mat[1*6+4] + r0[1][2] * mat[2*6+4]; 2793 r1[1][2] = r0[1][0] * mat[0*6+5] + r0[1][1] * mat[1*6+5] + r0[1][2] * mat[2*6+5]; 2794 r1[2][0] = r0[2][0] * mat[0*6+3] + r0[2][1] * mat[1*6+3] + r0[2][2] * mat[2*6+3]; 2795 r1[2][1] = r0[2][0] * mat[0*6+4] + r0[2][1] * mat[1*6+4] + r0[2][2] * mat[2*6+4]; 2796 r1[2][2] = r0[2][0] * mat[0*6+5] + r0[2][1] * mat[1*6+5] + r0[2][2] * mat[2*6+5]; 2797 2798 // r2 = m2 * r1; 2799 r2[0][0] = mat[3*6+0] * r1[0][0] + mat[3*6+1] * r1[1][0] + mat[3*6+2] * r1[2][0]; 2800 r2[0][1] = mat[3*6+0] * r1[0][1] + mat[3*6+1] * r1[1][1] + mat[3*6+2] * r1[2][1]; 2801 r2[0][2] = mat[3*6+0] * r1[0][2] + mat[3*6+1] * r1[1][2] + mat[3*6+2] * r1[2][2]; 2802 r2[1][0] = mat[4*6+0] * r1[0][0] + mat[4*6+1] * r1[1][0] + mat[4*6+2] * r1[2][0]; 2803 r2[1][1] = mat[4*6+0] * r1[0][1] + mat[4*6+1] * r1[1][1] + mat[4*6+2] * r1[2][1]; 2804 r2[1][2] = mat[4*6+0] * r1[0][2] + mat[4*6+1] * r1[1][2] + mat[4*6+2] * r1[2][2]; 2805 r2[2][0] = mat[5*6+0] * r1[0][0] + mat[5*6+1] * r1[1][0] + mat[5*6+2] * r1[2][0]; 2806 r2[2][1] = mat[5*6+0] * r1[0][1] + mat[5*6+1] * r1[1][1] + mat[5*6+2] * r1[2][1]; 2807 r2[2][2] = mat[5*6+0] * r1[0][2] + mat[5*6+1] * r1[1][2] + mat[5*6+2] * r1[2][2]; 2808 2809 // r3 = r2 - m3; 2810 r3[0][0] = r2[0][0] - mat[3*6+3]; 2811 r3[0][1] = r2[0][1] - mat[3*6+4]; 2812 r3[0][2] = r2[0][2] - mat[3*6+5]; 2813 r3[1][0] = r2[1][0] - mat[4*6+3]; 2814 r3[1][1] = r2[1][1] - mat[4*6+4]; 2815 r3[1][2] = r2[1][2] - mat[4*6+5]; 2816 r3[2][0] = r2[2][0] - mat[5*6+3]; 2817 r3[2][1] = r2[2][1] - mat[5*6+4]; 2818 r3[2][2] = r2[2][2] - mat[5*6+5]; 2819 2820 // r3.InverseSelf(); 2821 r2[0][0] = r3[1][1] * r3[2][2] - r3[1][2] * r3[2][1]; 2822 r2[1][0] = r3[1][2] * r3[2][0] - r3[1][0] * r3[2][2]; 2823 r2[2][0] = r3[1][0] * r3[2][1] - r3[1][1] * r3[2][0]; 2824 2825 det = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0]; 2826 2827 if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { 2828 return false; 2829 } 2830 2831 invDet = 1.0f / det; 2832 2833 r2[0][1] = r3[0][2] * r3[2][1] - r3[0][1] * r3[2][2]; 2834 r2[0][2] = r3[0][1] * r3[1][2] - r3[0][2] * r3[1][1]; 2835 r2[1][1] = r3[0][0] * r3[2][2] - r3[0][2] * r3[2][0]; 2836 r2[1][2] = r3[0][2] * r3[1][0] - r3[0][0] * r3[1][2]; 2837 r2[2][1] = r3[0][1] * r3[2][0] - r3[0][0] * r3[2][1]; 2838 r2[2][2] = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0]; 2839 2840 r3[0][0] = r2[0][0] * invDet; 2841 r3[0][1] = r2[0][1] * invDet; 2842 r3[0][2] = r2[0][2] * invDet; 2843 r3[1][0] = r2[1][0] * invDet; 2844 r3[1][1] = r2[1][1] * invDet; 2845 r3[1][2] = r2[1][2] * invDet; 2846 r3[2][0] = r2[2][0] * invDet; 2847 r3[2][1] = r2[2][1] * invDet; 2848 r3[2][2] = r2[2][2] * invDet; 2849 2850 // r2 = m2 * r0; 2851 r2[0][0] = mat[3*6+0] * r0[0][0] + mat[3*6+1] * r0[1][0] + mat[3*6+2] * r0[2][0]; 2852 r2[0][1] = mat[3*6+0] * r0[0][1] + mat[3*6+1] * r0[1][1] + mat[3*6+2] * r0[2][1]; 2853 r2[0][2] = mat[3*6+0] * r0[0][2] + mat[3*6+1] * r0[1][2] + mat[3*6+2] * r0[2][2]; 2854 r2[1][0] = mat[4*6+0] * r0[0][0] + mat[4*6+1] * r0[1][0] + mat[4*6+2] * r0[2][0]; 2855 r2[1][1] = mat[4*6+0] * r0[0][1] + mat[4*6+1] * r0[1][1] + mat[4*6+2] * r0[2][1]; 2856 r2[1][2] = mat[4*6+0] * r0[0][2] + mat[4*6+1] * r0[1][2] + mat[4*6+2] * r0[2][2]; 2857 r2[2][0] = mat[5*6+0] * r0[0][0] + mat[5*6+1] * r0[1][0] + mat[5*6+2] * r0[2][0]; 2858 r2[2][1] = mat[5*6+0] * r0[0][1] + mat[5*6+1] * r0[1][1] + mat[5*6+2] * r0[2][1]; 2859 r2[2][2] = mat[5*6+0] * r0[0][2] + mat[5*6+1] * r0[1][2] + mat[5*6+2] * r0[2][2]; 2860 2861 // m2 = r3 * r2; 2862 mat[3*6+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0]; 2863 mat[3*6+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1] + r3[0][2] * r2[2][1]; 2864 mat[3*6+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2] + r3[0][2] * r2[2][2]; 2865 mat[4*6+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0] + r3[1][2] * r2[2][0]; 2866 mat[4*6+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1] + r3[1][2] * r2[2][1]; 2867 mat[4*6+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2] + r3[1][2] * r2[2][2]; 2868 mat[5*6+0] = r3[2][0] * r2[0][0] + r3[2][1] * r2[1][0] + r3[2][2] * r2[2][0]; 2869 mat[5*6+1] = r3[2][0] * r2[0][1] + r3[2][1] * r2[1][1] + r3[2][2] * r2[2][1]; 2870 mat[5*6+2] = r3[2][0] * r2[0][2] + r3[2][1] * r2[1][2] + r3[2][2] * r2[2][2]; 2871 2872 // m0 = r0 - r1 * m2; 2873 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]; 2874 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]; 2875 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]; 2876 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]; 2877 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]; 2878 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]; 2879 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]; 2880 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]; 2881 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]; 2882 2883 // m1 = r1 * r3; 2884 mat[0*6+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0] + r1[0][2] * r3[2][0]; 2885 mat[0*6+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1] + r1[0][2] * r3[2][1]; 2886 mat[0*6+5] = r1[0][0] * r3[0][2] + r1[0][1] * r3[1][2] + r1[0][2] * r3[2][2]; 2887 mat[1*6+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0] + r1[1][2] * r3[2][0]; 2888 mat[1*6+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1] + r1[1][2] * r3[2][1]; 2889 mat[1*6+5] = r1[1][0] * r3[0][2] + r1[1][1] * r3[1][2] + r1[1][2] * r3[2][2]; 2890 mat[2*6+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0] + r1[2][2] * r3[2][0]; 2891 mat[2*6+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1] + r1[2][2] * r3[2][1]; 2892 mat[2*6+5] = r1[2][0] * r3[0][2] + r1[2][1] * r3[1][2] + r1[2][2] * r3[2][2]; 2893 2894 // m3 = -r3; 2895 mat[3*6+3] = -r3[0][0]; 2896 mat[3*6+4] = -r3[0][1]; 2897 mat[3*6+5] = -r3[0][2]; 2898 mat[4*6+3] = -r3[1][0]; 2899 mat[4*6+4] = -r3[1][1]; 2900 mat[4*6+5] = -r3[1][2]; 2901 mat[5*6+3] = -r3[2][0]; 2902 mat[5*6+4] = -r3[2][1]; 2903 mat[5*6+5] = -r3[2][2]; 2904 2905 return true; 2906 #endif 2907 } 2908 2909 /* 2910 ============= 2911 idMat6::ToString 2912 ============= 2913 */ 2914 const char *idMat6::ToString( int precision ) const { 2915 return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision ); 2916 }