Implement and test Mat4Inverse().

include/QF/mathlib.h

@@ -502,7 +502,7 @@ extern const vec_t *const quat_origin;
 		Mat4Zero (a); \
 		a[15] = a[10] = a[5] = a[0] = 1; \
 	} while (0)
-#define Mat4Expand (a) \
+#define Mat4Expand(a) \
 	QuatExpand (a + 0), \
 	QuatExpand (a + 4), \
 	QuatExpand (a + 8), \
@@ -609,6 +609,7 @@ void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical);
 void Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans,
 			   mat4_t mat);
 void Mat4Transpose (const mat4_t a, mat4_t b);
+int Mat4Inverse (const mat4_t a, mat4_t b);
 void Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c);
 void Mat4MultVec (const mat4_t a, const vec3_t b, vec3_t c);
 /** Decompose a column major matrix into its component transformations.

libs/util/mathlib.c

@@ -825,6 +825,73 @@ Mat4Transpose (const mat4_t a, mat4_t b)
 	b[i * 4 + i] = a[i * 4 + i];		// in case b != a
 }
 
+typedef vec_t mat3_t[3 * 3];
+
+static vec_t
+Mat3Det (const mat3_t m)
+{
+	vec3_t      t;
+	CrossProduct (m + 3, m + 6, t);
+	return DotProduct (m, t);
+}
+
+static void
+Mat4Sub3 (const mat4_t m4, mat3_t m3, int i, int j)
+{
+	int         si, sj, di, dj;
+
+	for (di = 0; di < 3; di++) {
+		for (dj = 0; dj < 3; dj++) {
+			si = di + ((di >= i) ? 1 : 0);
+			sj = dj + ((dj >= j) ? 1 : 0);
+			m3[di * 3 + dj] = m4[si * 4 + sj];
+		}
+	}
+}
+
+static vec_t
+Mat4Det (const mat4_t m)
+{
+	mat3_t      t;
+	int         i;
+	vec_t       res = 0, det, s = 1;
+
+	for (i = 0; i < 4; i++, s = -s) {
+		Mat4Sub3 (m, t, 0, i);
+		det = Mat3Det (t);
+		res += m[i] * det * s;
+	}
+	return res;
+}
+
+int
+Mat4Inverse (const mat4_t a, mat4_t b)
+{
+	mat4_t      tb;
+	mat3_t      m3;
+	vec_t      *m = b;
+	int         i, j;
+	vec_t       det;
+	vec_t       sign[2] = { 1, -1};
+
+	det = Mat4Det (a);
+	if (det * det < 1e-6) {
+		Mat4Identity (b);
+		return 0;
+	}
+	if (b == a)
+		m = tb;
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 4; j++) {
+			Mat4Sub3 (a, m3, i, j);
+			m[j * 4 + i] = sign[(i + j) & 1] * Mat3Det (m3) / det;
+		}
+	}
+	if (m != b)
+		Mat4Copy (m, b);
+	return 1;
+}
+
 void
 Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c)
 {

libs/util/test/test-mat.c

@@ -2,6 +2,13 @@
 # include "config.h"
 #endif
 
+#ifdef HAVE_STRING_H
+# include <string.h>
+#endif
+#ifdef HAVE_STRINGS_H
+# include <strings.h>
+#endif
+
 #include "QF/mathlib.h"
 
 //PITCH YAW ROLL
@@ -167,6 +174,39 @@ fail:
 	return 0;
 }
 
+static int
+test_inverse (const vec3_t angles, const vec3_t scale,
+				 const vec3_t translation)
+{
+	int         i;
+	quat_t      rotation;
+	mat4_t      mat, inv, I, res;
+
+	AngleQuat (angles, rotation);
+	Mat4Init (rotation, scale, translation, mat);
+
+	Mat4Identity (I);
+	Mat4Inverse (mat, inv);
+	Mat4Mult (mat, inv, res);
+
+	for (i = 0; i < 4 * 4; i++)
+		if (!compare (I[i], res[i]))
+			goto fail;
+
+	return 1;
+fail:
+	printf ("\n\n(%g %g %g) (%g %g %g) (%g %g %g)\n",
+			VectorExpand (angles), VectorExpand (scale),
+			VectorExpand (translation));
+	printf ("  [%g %g %g %g]\n  [%g %g %g %g]\n"
+			"  [%g %g %g %g]\n  [%g %g %g %g]\n\n", Mat4Expand (mat));
+	printf ("  [%g %g %g %g]\n  [%g %g %g %g]\n"
+			"  [%g %g %g %g]\n  [%g %g %g %g]\n\n", Mat4Expand (inv));
+	printf ("  [%g %g %g %g]\n  [%g %g %g %g]\n"
+			"  [%g %g %g %g]\n  [%g %g %g %g]\n\n", Mat4Expand (res));
+	return 0;
+}
+
 int
 main (int argc, const char **argv)
 {
@@ -195,5 +235,14 @@ main (int argc, const char **argv)
 			}
 		}
 	}
+	for (i = 0; i < num_angle_tests; i ++) {
+		for (j = 0; j < num_translation_tests; j ++) {
+			for (k = 0; k < num_translation_tests; k ++) {
+				if (!test_inverse (test_angles[i], test_scales[j],
+								   test_translations[k]))
+					res = 1;
+			}
+		}
+	}
 	return res;
 }