diff options
Diffstat (limited to 'src/util')
| -rw-r--r-- | src/util/mini_math.h | 108 |
1 files changed, 65 insertions, 43 deletions
diff --git a/src/util/mini_math.h b/src/util/mini_math.h index 56e6832..2307087 100644 --- a/src/util/mini_math.h +++ b/src/util/mini_math.h @@ -258,51 +258,73 @@ struct mat4 { } mat4 inverse() const { - mat4 inv; - 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] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10]; - 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] - m[12] * m[6] * m[11] + - m[12] * m[7] * m[10]; - 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] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9]; - 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] - m[12] * m[5] * m[10] + - m[12] * m[6] * m[9]; - 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] - m[13] * m[2] * m[11] + - m[13] * m[3] * m[10]; - 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] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10]; - 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] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9]; - 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] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9]; - 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] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6]; - 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] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6]; - 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] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5]; - 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] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5]; - 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] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6]; - 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] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6]; - 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] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5]; - 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] - m[8] * m[2] * m[5]; + float n11 = m[0], n12 = m[4], n13 = m[8], n14 = m[12]; + float n21 = m[1], n22 = m[5], n23 = m[9], n24 = m[13]; + float n31 = m[2], n32 = m[6], n33 = m[10], n34 = m[14]; + float n41 = m[3], n42 = m[7], n43 = m[11], n44 = m[15]; - float det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - if (det == 0) - return mat4(); // Return identity on failure + float t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - + n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44; + float t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44; + float t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - + n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44; + float t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34; - det = 1.0f / det; - for (int i = 0; i < 16; i++) - inv[i] = inv[i] * det; - return inv; + float det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14; + + if (det == 0.0f) + return mat4(); + + float inv_det = 1.0f / det; + + mat4 res; + res.m[0] = t11 * inv_det; + res.m[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * + inv_det; + res.m[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - + n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * + inv_det; + res.m[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * + inv_det; + + res.m[4] = t12 * inv_det; + res.m[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - + n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * + inv_det; + res.m[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * + inv_det; + res.m[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - + n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * + inv_det; + + res.m[8] = t13 * inv_det; + res.m[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * + inv_det; + res.m[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - + n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * + inv_det; + res.m[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * + inv_det; + + res.m[12] = t14 * inv_det; + res.m[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - + n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * + inv_det; + res.m[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * + inv_det; + res.m[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - + n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * + inv_det; + + return res; } }; #endif /* defined(USE_MAT4) */ |