pinched m4_invert, m4_determinant and m3_determinant functions from LWJGL

diff --git a/math_3d.h b/math_3d.h
index f174102..78da4aa 100644 (file)
--- a/math_3d.h
+++ b/math_3d.h
@@ -203,8 +203,15 @@ static inline mat4_t m4_rotation_z   (float angle_in_rad);
 static inline mat4_t m4_transpose    (mat4_t matrix);
 static inline mat4_t m4_mul          (mat4_t a, mat4_t b);
               mat4_t m4_invert_affine(mat4_t matrix);
+                         float m3_determinant( float t00, float t01, float t02,
+                                                                       float t10, float t11, float t12,
+                                                                       float t20, float t21, float t22);
+                         float m4_determinant(mat4_t matrix);
+                         mat4_t m4_invert(mat4_t matrix);
               vec3_t m4_mul_pos      (mat4_t matrix, vec3_t position);
               vec3_t m4_mul_dir      (mat4_t matrix, vec3_t direction);
+                         //TODO: implement m4_transform_vec(mat4_t matrix, vec4_t vec);
+                         //TODO: ref: https://github.com/LWJGL/lwjgl/blob/2df01dd762e20ca0871edb75daf670ccacc89b60/src/java/org/lwjgl/util/vector/Matrix4f.java
 
               void   m4_print        (mat4_t matrix);
               void   m4_printp       (mat4_t matrix, int width, int precision);
@@ -638,4 +645,110 @@ void m4_fprintp(FILE* stream, mat4_t matrix, int width, int precision) {
        }
 }
 
+/**
+ * Determinant functions to help with calculating the matrix invert. I've got
+ * no idea how this works in all honesty, I just grabbed the values from LWJGL's
+ * Matrix4f class
+ * https://github.com/LWJGL/lwjgl/blob/2df01dd762e20ca0871edb75daf670ccacc89b60/src/java/org/lwjgl/util/vector/Matrix4f.java
+*/
+float m4_determinant(mat4_t m)
+{
+       float f = m.m00 * ((m.m11 * m.m22 * m.m33 + m.m12 * m.m23 * m.m31 + m.m13 * m.m21 * m.m32)
+                                                         - m.m13 * m.m22 * m.m31
+                                                         - m.m11 * m.m23 * m.m32
+                                                         - m.m12 * m.m21 * m.m33);
+
+       f -= m.m01 * ((m.m10 * m.m22 * m.m33 + m.m12 * m.m23 * m.m30 + m.m13 * m.m20 * m.m32)
+                                                - m.m13 * m.m22 * m.m30
+                                                - m.m10 * m.m23 * m.m32
+                                                - m.m12 * m.m20 * m.m33);
+
+       f += m.m02 * ((m.m10 * m.m21 * m.m33 + m.m11 * m.m23 * m.m30 + m.m13 * m.m20 * m.m31)
+                                                - m.m13 * m.m21 * m.m30
+                                                - m.m10 * m.m23 * m.m31
+                                                - m.m11 * m.m20 * m.m33);
+
+       f -= m.m03 * ((m.m10 * m.m21 * m.m32 + m.m11 * m.m22 * m.m30 + m.m12 * m.m20 * m.m31)
+                                                - m.m12 * m.m21 * m.m30
+                                                - m.m10 * m.m22 * m.m31
+                                                - m.m11 * m.m20 * m.m32);
+
+       return f;
+}
+
+float m3_determinant(float t00, float t01, float t02,
+                                        float t10, float t11, float t12,
+                                        float t20, float t21, float t22)
+{
+       return    t00 * (t11 * t22 - t12 * t21)
+                       + t01 * (t12 * t20 - t10 * t22)
+                       + t02 * (t10 * t21 - t11 * t20);
+}
+
+/**
+ * Again, copy-paste wizardry. I just want something working.
+*/
+mat4_t m4_invert(mat4_t m)
+{
+       float determinant = m4_determinant(m);
+       float determinant_inv = 1.f/determinant;
+       mat4_t inv = m4_identity();
+
+       // first row
+       float t00 =  m3_determinant(m.m11, m.m12, m.m13, m.m21, m.m22,
+                       m.m23, m.m31, m.m32, m.m33);
+       float t01 = -m3_determinant(m.m10, m.m12, m.m13, m.m20, m.m22,
+                       m.m23, m.m30, m.m32, m.m33);
+       float t02 =  m3_determinant(m.m10, m.m11, m.m13, m.m20, m.m21,
+                       m.m23, m.m30, m.m31, m.m33);
+       float t03 = -m3_determinant(m.m10, m.m11, m.m12, m.m20, m.m21,
+                       m.m22, m.m30, m.m31, m.m32);
+       // second row
+       float t10 = -m3_determinant(m.m01, m.m02, m.m03, m.m21, m.m22,
+                       m.m23, m.m31, m.m32, m.m33);
+       float t11 =  m3_determinant(m.m00, m.m02, m.m03, m.m20, m.m22,
+                       m.m23, m.m30, m.m32, m.m33);
+       float t12 = -m3_determinant(m.m00, m.m01, m.m03, m.m20, m.m21,
+                       m.m23, m.m30, m.m31, m.m33);
+       float t13 =  m3_determinant(m.m00, m.m01, m.m02, m.m20, m.m21,
+                       m.m22, m.m30, m.m31, m.m32);
+       // third row
+       float t20 =  m3_determinant(m.m01, m.m02, m.m03, m.m11, m.m12,
+                       m.m13, m.m31, m.m32, m.m33);
+       float t21 = -m3_determinant(m.m00, m.m02, m.m03, m.m10, m.m12,
+                       m.m13, m.m30, m.m32, m.m33);
+       float t22 =  m3_determinant(m.m00, m.m01, m.m03, m.m10, m.m11,
+                       m.m13, m.m30, m.m31, m.m33);
+       float t23 = -m3_determinant(m.m00, m.m01, m.m02, m.m10, m.m11,
+                       m.m12, m.m30, m.m31, m.m32);
+       // fourth row
+       float t30 = -m3_determinant(m.m01, m.m02, m.m03, m.m11, m.m12,
+                       m.m13, m.m21, m.m22, m.m23);
+       float t31 =  m3_determinant(m.m00, m.m02, m.m03, m.m10, m.m12,
+                       m.m13, m.m20, m.m22, m.m23);
+       float t32 = -m3_determinant(m.m00, m.m01, m.m03, m.m10, m.m11,
+                       m.m13, m.m20, m.m21, m.m23);
+       float t33 =  m3_determinant(m.m00, m.m01, m.m02, m.m10, m.m11,
+                       m.m12, m.m20, m.m21, m.m22);
+
+       // transpose and divide by the determinant
+       inv.m00 = t00*determinant_inv;
+       inv.m11 = t11*determinant_inv;
+       inv.m22 = t22*determinant_inv;
+       inv.m33 = t33*determinant_inv;
+       inv.m01 = t10*determinant_inv;
+       inv.m10 = t01*determinant_inv;
+       inv.m20 = t02*determinant_inv;
+       inv.m02 = t20*determinant_inv;
+       inv.m12 = t21*determinant_inv;
+       inv.m21 = t12*determinant_inv;
+       inv.m03 = t30*determinant_inv;
+       inv.m30 = t03*determinant_inv;
+       inv.m13 = t31*determinant_inv;
+       inv.m31 = t13*determinant_inv;
+       inv.m32 = t23*determinant_inv;
+       inv.m23 = t32*determinant_inv;
+       return inv;
+}
+
 #endif // MATH_3D_IMPLEMENTATION