Add tests for matrix decomposition and fix the bugs.

include/QF/mathlib.h

@@ -483,6 +483,11 @@ extern const vec_t *const quat_origin;
 		Mat4Zero (a); \
 		a[15] = a[10] = a[5] = a[0] = 1; \
 	} while (0)
+#define Mat4Expand (a) \
+	QuatExpand (a + 0), \
+	QuatExpand (a + 4), \
+	QuatExpand (a + 8), \
+	QuatExpand (a + 12)
 
 #define qfrandom(MAX) ((float) MAX * (rand() * (1.0 / (RAND_MAX + 1.0))))
 
@@ -585,8 +590,8 @@ void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical);
 void Mat4Transpose (const mat4_t a, mat4_t b);
 void Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c);
 // Column major matrix
-int MatDecompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
+int Mat4Decompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
-				  vec3_t trans);
+				   vec3_t trans);
 
 #define BOX_ON_PLANE_SIDE(emins, emaxs, p)				\
 	(((p)->type < 3)?									\

libs/util/mathlib.c

@@ -833,8 +833,8 @@ Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c)
 }
 
 int
-MatDecompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
+Mat4Decompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
-			  vec3_t trans)
+			   vec3_t trans)
 {
 	vec3_t      row[3], shr, scl;
 	vec_t       l, t;
@@ -843,7 +843,7 @@ MatDecompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
 	if (trans)
 		VectorCopy (m + 12, trans);
 	for (i = 0; i < 3; i++)
-		for (j = 0; i < 3; j++)
+		for (j = 0; j < 3; j++)
 			row[j][i] = m[i * 4 + j];
 	l = DotProduct (row[0], row[0]);
 	if (l < 1e-5)
@@ -879,29 +879,29 @@ MatDecompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
 
 	t = 1 + row[0][0] + row[1][1] + row[2][2];
 	if (t >= 1e-5) {
-		vec_t       s = sqrt (t);
+		vec_t       s = sqrt (t) * 2;
 		rot[0] = s / 4;
-		rot[1] = (row[1][2] - row[2][1]) / s;
+		rot[1] = (row[2][1] - row[1][2]) / s;
-		rot[2] = (row[2][0] - row[0][2]) / s;
+		rot[2] = (row[0][2] - row[2][0]) / s;
-		rot[3] = (row[0][1] - row[1][4]) / s;
+		rot[3] = (row[1][0] - row[0][1]) / s;
 	} else {
 		if (row[0][0] > row[1][1] && row[0][0] > row[2][2]) {
-			vec_t       s = sqrt (1 + row[0][0] - row[1][1] - row[2][2]);
+			vec_t       s = sqrt (1 + row[0][0] - row[1][1] - row[2][2]) * 2;
-			rot[0] = (row[1][2] - row[2][1]) / s;
+			rot[0] = (row[2][1] - row[1][2]) / s;
 			rot[1] = s / 4;
-			rot[2] = (row[2][0] - row[0][2]) / s;
+			rot[2] = (row[1][0] + row[0][1]) / s;
-			rot[3] = (row[0][1] - row[1][0]) / s;
+			rot[3] = (row[0][2] + row[2][0]) / s;
 		} else if (row[1][1] > row[2][2]) {
-			vec_t       s = sqrt (1 + row[1][1] - row[0][0] - row[2][2]);
+			vec_t       s = sqrt (1 + row[1][1] - row[0][0] - row[2][2]) * 2;
-			rot[0] = (row[2][0] - row[0][2]) / s;
+			rot[0] = (row[0][2] - row[2][0]) / s;
-			rot[1] = (row[1][2] - row[2][1]) / s;
+			rot[1] = (row[1][0] + row[0][1]) / s;
 			rot[2] = s / 4;
-			rot[3] = (row[0][1] - row[1][0]) / s;
+			rot[3] = (row[2][1] + row[1][2]) / s;
 		} else {
-			vec_t       s = sqrt (1 + row[2][2] - row[0][0] - row[1][1]);
+			vec_t       s = sqrt (1 + row[2][2] - row[0][0] - row[1][1]) * 2;
-			rot[0] = (row[0][1] - row[1][0]) / s;
+			rot[0] = (row[1][0] - row[0][1]) / s;
-			rot[1] = (row[1][2] - row[2][1]) / s;
+			rot[1] = (row[0][2] + row[2][0]) / s;
-			rot[2] = (row[2][0] - row[0][2]) / s;
+			rot[2] = (row[2][1] + row[1][2]) / s;
 			rot[3] = s / 4;
 		}
 	}

libs/util/test/Makefile.am

@@ -2,7 +2,7 @@ AUTOMAKE_OPTIONS= foreign
 
 INCLUDES= -I$(top_srcdir)/include
 
-check_PROGRAMS=test-dq test-half test-qfs test-quat test-vrect
+check_PROGRAMS=test-dq test-half test-mat test-qfs test-quat test-vrect
 
 test_dq_SOURCES=test-dq.c
 test_dq_LDADD=$(top_builddir)/libs/util/libQFutil.la
@@ -12,6 +12,10 @@ test_half_SOURCES=test-half.c
 test_half_LDADD=$(top_builddir)/libs/util/libQFutil.la
 test_half_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
 
+test_mat_SOURCES=test-mat.c
+test_mat_LDADD=$(top_builddir)/libs/util/libQFutil.la
+test_mat_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
+
 test_qfs_SOURCES=test-qfs.c
 test_qfs_LDADD=$(top_builddir)/libs/util/libQFutil.la
 test_qfs_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la

libs/util/test/test-mat.c

@@ -0,0 +1,84 @@
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "QF/mathlib.h"
+
+//PITCH YAW ROLL
+static vec3_t test_angles[] = {
+	{ 0,  0,  0},
+	{ 0,  0,  0},
+	{45,  0,  0},
+	{45,  0,  0},
+	{ 0, 45,  0},
+	{ 0, 45,  0},
+	{ 0,  0, 45},
+	{ 0,  0, 45},
+	{45, 45,  0},
+	{45, 45,  0},
+	{ 0, 45, 45},
+	{ 0, 45, 45},
+	{45,  0, 45},
+	{45,  0, 45},
+	{45, 45, 45},
+	{45, 45, 45},
+	{0, 180, 180},
+	{180, 0, 180},
+	{180, 180, 0},
+};
+#define num_angle_tests \
+	(sizeof (test_angles) / sizeof (test_angles[0]))
+
+// return true if a and b are close enough (yay, floats)
+static int
+compare (vec_t a, vec_t b)
+{
+	vec_t       diff = a - b;
+	return diff * diff < 0.001;
+}
+
+static int
+test_angle (const vec3_t angles)
+{
+	int         i;
+	quat_t      rotation, r;
+	vec3_t      scale, shear, trans;
+	mat4_t      mat;
+
+	AngleQuat (angles, rotation);
+	QuatToMatrix (rotation, mat, 1, 1);
+	Mat4Decompose (mat, r, scale, shear, trans);
+	for (i = 0; i < 4; i++)
+		if (!compare (rotation[i], r[i]))
+			goto negate;
+	return 1;
+negate:
+	// Mat4Decompose always sets the rotation quaternion's scalar to +ve
+	// but AngleQuat might produce a -ve scalar.
+	QuatNegate (r, r);
+	for (i = 0; i < 4; i++)
+		if (!compare (rotation[i], r[i]))
+			goto fail;
+
+	return 1;
+fail:
+	printf ("\n\n(%g %g %g)\n", VectorExpand (angles));
+	printf ("  [%g %g %g %g]\n", QuatExpand (rotation));
+	printf ("  [%g %g %g %g] [%g %g %g] [%g %g %g] [%g %g %g]\n",
+			QuatExpand (r), VectorExpand (scale), VectorExpand (shear),
+			VectorExpand (trans));
+	return 0;
+}
+
+int
+main (int argc, const char **argv)
+{
+	int         res = 0;
+	size_t      i;
+
+	for (i = 0; i < num_angle_tests; i ++) {
+		if (!test_angle (test_angles[i]))
+			res = 1;
+	}
+	return res;
+}

libs/util/test/test-quat.c

@@ -14,6 +14,9 @@ static vec3_t test_angles[] = {
 	{0, 45, 45},
 	{45, 0, 45},
 	{45, 45, 45},
+	{0, 180, 180},
+	{180, 0, 180},
+	{180, 180, 0},
 };
 #define num_angle_tests (sizeof (test_angles) / sizeof (test_angles[0]))