From: Matthew Owens <matthew@owens.tech>
Date: Fri, 20 Nov 2020 08:26:12 +0000 (+0000)
Subject: pinched m4_invert, m4_determinant and m3_determinant functions from LWJGL
X-Git-Url: https://git.owens.tech///git?a=commitdiff_plain;h=fa07dfa8708ecae82f5beed06fbed00dcfa03883;p=forks%2Fsingle-header-file-c-libs.git

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
--- 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