| 76 | } |
| 77 | |
| 78 | Mat4 Inverse(const Mat4& a) |
| 79 | { |
| 80 | const std::array<float, 16>& m = a.m; |
| 81 | std::array<float, 16> inv{}; |
| 82 | |
| 83 | inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + |
| 84 | m[13] * m[6] * m[11] - m[13] * m[7] * m[10]; |
| 85 | inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - |
| 86 | m[12] * m[6] * m[11] + m[12] * m[7] * m[10]; |
| 87 | inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + |
| 88 | m[12] * m[5] * m[11] - m[12] * m[7] * m[9]; |
| 89 | inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - |
| 90 | m[12] * m[5] * m[10] + m[12] * m[6] * m[9]; |
| 91 | inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - |
| 92 | m[13] * m[2] * m[11] + m[13] * m[3] * m[10]; |
| 93 | inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + |
| 94 | m[12] * m[2] * m[11] - m[12] * m[3] * m[10]; |
| 95 | inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - |
| 96 | m[12] * m[1] * m[11] + m[12] * m[3] * m[9]; |
| 97 | inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + |
| 98 | m[12] * m[1] * m[10] - m[12] * m[2] * m[9]; |
| 99 | inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + |
| 100 | m[13] * m[2] * m[7] - m[13] * m[3] * m[6]; |
| 101 | inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - |
| 102 | m[12] * m[2] * m[7] + m[12] * m[3] * m[6]; |
| 103 | inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + |
| 104 | m[12] * m[1] * m[7] - m[12] * m[3] * m[5]; |
| 105 | inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - |
| 106 | m[12] * m[1] * m[6] + m[12] * m[2] * m[5]; |
| 107 | inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - |
| 108 | m[9] * m[2] * m[7] + m[9] * m[3] * m[6]; |
| 109 | inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + |
| 110 | m[8] * m[2] * m[7] - m[8] * m[3] * m[6]; |
| 111 | inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - |
| 112 | m[8] * m[1] * m[7] + m[8] * m[3] * m[5]; |
| 113 | inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - |
| 114 | m[8] * m[2] * m[5]; |
| 115 | |
| 116 | float det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; |
| 117 | if (std::fabs(det) < 1e-20f) |
| 118 | { |
| 119 | return Identity(); |
| 120 | } |
| 121 | float invDet = 1.0f / det; |
| 122 | Mat4 out; |
| 123 | for (int i = 0; i < 16; i++) |
| 124 | { |
| 125 | out.m[i] = inv[i] * invDet; |
| 126 | } |
| 127 | return out; |
| 128 | } |
| 129 | |
| 130 | Vec4 Transform(const Mat4& a, const Vec4& v) |
| 131 | { |