| 377 | } |
| 378 | |
| 379 | void Matrix::Invert(const Matrix& value, Matrix& result) |
| 380 | { |
| 381 | const float b0 = value.M31 * value.M42 - value.M32 * value.M41; |
| 382 | const float b1 = value.M31 * value.M43 - value.M33 * value.M41; |
| 383 | const float b2 = value.M34 * value.M41 - value.M31 * value.M44; |
| 384 | const float b3 = value.M32 * value.M43 - value.M33 * value.M42; |
| 385 | const float b4 = value.M34 * value.M42 - value.M32 * value.M44; |
| 386 | const float b5 = value.M33 * value.M44 - value.M34 * value.M43; |
| 387 | |
| 388 | const float d11 = value.M22 * b5 + value.M23 * b4 + value.M24 * b3; |
| 389 | const float d12 = value.M21 * b5 + value.M23 * b2 + value.M24 * b1; |
| 390 | const float d13 = value.M21 * -b4 + value.M22 * b2 + value.M24 * b0; |
| 391 | const float d14 = value.M21 * b3 + value.M22 * -b1 + value.M23 * b0; |
| 392 | |
| 393 | float det = value.M11 * d11 - value.M12 * d12 + value.M13 * d13 - value.M14 * d14; |
| 394 | if (Math::Abs(det) <= 1e-12f) |
| 395 | { |
| 396 | result = Zero; |
| 397 | return; |
| 398 | } |
| 399 | |
| 400 | det = 1.0f / det; |
| 401 | |
| 402 | const float a0 = value.M11 * value.M22 - value.M12 * value.M21; |
| 403 | const float a1 = value.M11 * value.M23 - value.M13 * value.M21; |
| 404 | const float a2 = value.M14 * value.M21 - value.M11 * value.M24; |
| 405 | const float a3 = value.M12 * value.M23 - value.M13 * value.M22; |
| 406 | const float a4 = value.M14 * value.M22 - value.M12 * value.M24; |
| 407 | const float a5 = value.M13 * value.M24 - value.M14 * value.M23; |
| 408 | |
| 409 | const float d21 = value.M12 * b5 + value.M13 * b4 + value.M14 * b3; |
| 410 | const float d22 = value.M11 * b5 + value.M13 * b2 + value.M14 * b1; |
| 411 | const float d23 = value.M11 * -b4 + value.M12 * b2 + value.M14 * b0; |
| 412 | const float d24 = value.M11 * b3 + value.M12 * -b1 + value.M13 * b0; |
| 413 | |
| 414 | const float d31 = value.M42 * a5 + value.M43 * a4 + value.M44 * a3; |
| 415 | const float d32 = value.M41 * a5 + value.M43 * a2 + value.M44 * a1; |
| 416 | const float d33 = value.M41 * -a4 + value.M42 * a2 + value.M44 * a0; |
| 417 | const float d34 = value.M41 * a3 + value.M42 * -a1 + value.M43 * a0; |
| 418 | |
| 419 | const float d41 = value.M32 * a5 + value.M33 * a4 + value.M34 * a3; |
| 420 | const float d42 = value.M31 * a5 + value.M33 * a2 + value.M34 * a1; |
| 421 | const float d43 = value.M31 * -a4 + value.M32 * a2 + value.M34 * a0; |
| 422 | const float d44 = value.M31 * a3 + value.M32 * -a1 + value.M33 * a0; |
| 423 | |
| 424 | result.M11 = +d11 * det; |
| 425 | result.M12 = -d21 * det; |
| 426 | result.M13 = +d31 * det; |
| 427 | result.M14 = -d41 * det; |
| 428 | result.M21 = -d12 * det; |
| 429 | result.M22 = +d22 * det; |
| 430 | result.M23 = -d32 * det; |
| 431 | result.M24 = +d42 * det; |
| 432 | result.M31 = +d13 * det; |
| 433 | result.M32 = -d23 * det; |
| 434 | result.M33 = +d33 * det; |
| 435 | result.M34 = -d43 * det; |
| 436 | result.M41 = -d14 * det; |