From de131c18ad15c0cbc749707878edc873d3117560 Mon Sep 17 00:00:00 2001
From: Bill Currie <bill@taniwha.org>
Date: Wed, 9 May 2012 22:28:22 +0900
Subject: [PATCH] Add VectorShear and really test Mat4Decompose.

And the tests really exercised VectorShear (first attempt had things
messed up when more than one shear value was non-zero). Also,
Mat4Decompose wasn't orthogonalizing the z axis row. Oops. Anyway,
Mat4Decompose is now known to work well, and the usage of its output is
understood :)
---
 include/QF/mathlib.h      | 16 +++++++++++++
 libs/util/mathlib.c       |  6 +++--
 libs/util/test/test-mat.c | 50 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 70 insertions(+), 2 deletions(-)

diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h
index 3e9af925c..6362e5f6e 100644
--- a/include/QF/mathlib.h
+++ b/include/QF/mathlib.h
@@ -104,6 +104,22 @@ extern const vec_t *const quat_origin;
 		(c)[1] = (a)[1] * (b); \
 		(c)[2] = (a)[2] * (b); \
 	} while (0)
+/** Shear vector \a b by vector \a a.
+
+	Vector a represents the shear factors XY, XZ, YZ, ie in matrix form:
+	[  1  0  0 ] [ b0 ]
+	[ a0  1  0 ] [ b1 ]
+	[ a1 a2  1 ] [ b2 ]
+
+	The reason for this particular scheme is that is how Mat4Decompose
+	calculates the shear from a matrix.
+*/
+#define VectorShear(a,b,c) \
+	do { \
+		(c)[2] = (b)[0] * (a)[1] + (b)[1] * (a)[2] + (b)[2]; \
+		(c)[1] = (b)[0] * (a)[0] + (b)[1]; \
+		(c)[0] = (b)[0]; \
+	} while (0)
 #define VectorCompMult(a,b,c) \
 	do { \
 		(c)[0] = (a)[0] * (b)[0]; \
diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c
index a76125eca..f6918a145 100644
--- a/libs/util/mathlib.c
+++ b/libs/util/mathlib.c
@@ -861,11 +861,10 @@ Mat4Decompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
 	vec_t       l, t;
 	int         i, j;
 
-	if (trans)
-		VectorCopy (m + 12, trans);
 	for (i = 0; i < 3; i++)
 		for (j = 0; j < 3; j++)
 			row[j][i] = m[i * 4 + j];
+
 	l = DotProduct (row[0], row[0]);
 	if (l < 1e-5)
 		return 0;
@@ -884,6 +883,7 @@ Mat4Decompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
 
 	VectorMultSub (row[2], shr[1], row[0], row[2]);
 	shr[2] = DotProduct (row[1], row[2]);
+	VectorMultSub (row[2], shr[2], row[1], row[2]);
 	l = DotProduct (row[2], row[2]);
 	if (l < 1e-5)
 		return 0;
@@ -895,6 +895,8 @@ Mat4Decompose (const mat4_t m, quat_t rot, vec3_t scale, vec3_t shear,
 		VectorCopy (scl, scale);
 	if (shear)
 		VectorCopy (shr, shear);
+	if (trans)
+		VectorCopy (m + 12, trans);
 	if (!rot)
 		return 1;
 
diff --git a/libs/util/test/test-mat.c b/libs/util/test/test-mat.c
index ef1329e45..2d711e38c 100644
--- a/libs/util/test/test-mat.c
+++ b/libs/util/test/test-mat.c
@@ -117,6 +117,7 @@ test_transform (const vec3_t angles, const vec3_t scale,
 	for (i = 0; i < 3; i++)
 		if (!compare (x[i], y[i]))
 			goto fail;
+
 	return 1;
 fail:
 	printf ("\n\n(%g %g %g) (%g %g %g) (%g %g %g)\n", VectorExpand (angles),
@@ -126,6 +127,46 @@ fail:
 	return 0;
 }
 
+static int
+test_transform2 (const vec3_t angles, const vec3_t scale,
+				 const vec3_t translation)
+{
+	int         i;
+	const vec3_t v = {4,5,6};
+	vec3_t      x, y;
+	quat_t      rotation;
+	mat4_t      mat;
+	vec3_t      rot, sc, sh, tr;
+
+	VectorCopy (v, x);
+	AngleQuat (angles, rotation);
+	VectorCompMult (scale, x, x);
+	QuatMultVec (rotation, x, x);
+	VectorAdd (translation, x, x);
+
+	Mat4Init (rotation, scale, translation, mat);
+	Mat4Decompose (mat, rot, sc, sh,  tr);
+
+	VectorCopy (v, y);
+	QuatMultVec (rot, y, y);
+	VectorShear (sh, y, y);
+	VectorCompMult (sc, y, y);//scale
+	VectorAdd (tr, y, y);
+
+	for (i = 0; i < 3; i++)
+		if (!compare (x[i], y[i]))
+			goto fail;
+
+	return 1;
+fail:
+	printf ("\n\n(%g %g %g) (%g %g %g) (%g %g %g) (%g %g %g)\n",
+			VectorExpand (angles), VectorExpand (scale),
+			VectorExpand (translation), VectorExpand (v));
+	printf ("  (%g %g %g)\n", VectorExpand (x));
+	printf ("  (%g %g %g)\n", VectorExpand (y));
+	return 0;
+}
+
 int
 main (int argc, const char **argv)
 {
@@ -145,5 +186,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_transform2 (test_angles[i], test_scales[j],
+									  test_translations[k]))
+					res = 1;
+			}
+		}
+	}
 	return res;
 }