Add a function to convert quake angles to a quaternion.

The test works by comparing the result of AngleVectors with forward,
right, and up vectors rotated by the quaternion resulting from AngleQuat.

include/QF/mathlib.h

@@ -240,6 +240,7 @@ int GreatestCommonDivisor (int i1, int i2);
 
 void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right,
 				   vec3_t up);
+void AngleQuat (const vec3_t angles, quat_t q);
 void VectorVectors (const vec3_t forward, vec3_t right, vec3_t up);
 int BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs,
 					struct mplane_s *plane);

libs/util/mathlib.c

@@ -385,7 +385,7 @@ BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs, mplane_t *p)
 
 	the math in AngleVectors has the entity frame as left handed with x
 	(forward) axis forward, y (right) axis to the right and z (up) up. However,
-	the world is a right (?) handed system with x to the right, y forward and
+	the world is a right handed system with x to the right, y forward and
 	z up.
 
 	pitch =
@@ -437,6 +437,29 @@ AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
 	up[2] = cr * cp;
 }
 
+VISIBLE void
+AngleQuat (const vec3_t angles, quat_t q)
+{
+	float       alpha, sr, sp, sy, cr, cp, cy;
+
+	// alpha is half the angle
+	alpha = angles[YAW] * (M_PI / 360);
+	sy = sin (alpha);
+	cy = cos (alpha);
+	alpha = angles[PITCH] * (M_PI / 360);
+	sp = sin (alpha);
+	cp = cos (alpha);
+	alpha = angles[ROLL] * (M_PI / 360);
+	sr = sin (alpha);
+	cr = cos (alpha);
+
+	QuatSet (cy * cp * cr + sy * sp * sr,
+			 cy * cp * sr - sy * sp * cr,
+			 cy * sp * cr + sy * cp * sr,
+			 sy * cp * cr - cy * sp * sr,
+			 q);
+}
+
 VISIBLE int
 _VectorCompare (const vec3_t v1, const vec3_t v2)
 {

libs/util/test/Makefile.am

@@ -2,10 +2,14 @@ AUTOMAKE_OPTIONS= foreign
 
 INCLUDES= -I$(top_srcdir)/include
 
-check_PROGRAMS=test-qfs
+check_PROGRAMS=test-qfs test-quat
 
 test_qfs_SOURCES=test-qfs.c
 test_qfs_LDADD=$(top_builddir)/libs/util/libQFutil.la
 test_qfs_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
 
+test_quat_SOURCES=test-quat.c
+test_quat_LDADD=$(top_builddir)/libs/util/libQFutil.la
+test_quat_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
+
 TESTS=$(check_PROGRAMS)

libs/util/test/test-quat.c

@@ -0,0 +1,96 @@
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "QF/mathlib.h"
+
+//PITCH YAW ROLL
+static vec3_t test_angles[] = {
+	{0, 0, 0},
+	{45, 0, 0},
+	{0, 45, 0},
+	{0, 0, 45},
+	{45, 45, 0},
+	{0, 45, 45},
+	{45, 0, 45},
+	{45, 45, 45},
+};
+#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_rotation (const vec3_t angles)
+{
+	int         i;
+	vec3_t      forward, right, up;
+
+	quat_t      quat, f, r, u, t;
+	quat_t      qf = {0, 1,  0, 0};
+	quat_t      qr = {0, 0, -1, 0};
+	quat_t      qu = {0, 0,  0, 1};
+
+	AngleVectors (angles, forward, right, up);
+
+	AngleQuat (angles, quat);
+	// rotate forward vector
+	QuatConj (quat, t);
+	QuatMult (qf, t, t);
+	QuatMult (quat, t, f);
+	// rotate right vector
+	QuatConj (quat, t);
+	QuatMult (qr, t, t);
+	QuatMult (quat, t, r);
+	// rotate up vector
+	QuatConj (quat, t);
+	QuatMult (qu, t, t);
+	QuatMult (quat, t, u);
+
+	if (!compare (f[0], 0))
+		goto fail;
+	for (i = 0; i < 3; i++)
+		if (!compare (forward[i], f[i + 1]))
+			goto fail;
+
+	if (!compare (r[0], 0))
+		goto fail;
+	for (i = 0; i < 3; i++)
+		if (!compare (right[i], r[i + 1]))
+			goto fail;
+
+	if (!compare (u[0], 0))
+		goto fail;
+	for (i = 0; i < 3; i++)
+		if (!compare (up[i], u[i + 1]))
+			goto fail;
+	return 1;
+fail:
+	printf ("\n\n%g %g %g\n\n", angles[0], angles[1], angles[2]);
+	printf ("%g %g %g\n", forward[0], forward[1], forward[2]);
+	printf ("%g %g %g\n", right[0], right[1], right[2]);
+	printf ("%g %g %g\n\n", up[0], up[1], up[2]);
+
+	printf ("%g %g %g %g\n", f[0], f[1], f[2], f[3]);
+	printf ("%g %g %g %g\n", r[0], r[1], r[2], r[3]);
+	printf ("%g %g %g %g\n", u[0], u[1], u[2], u[3]);
+	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_rotation (test_angles[i]))
+			res = 1;
+	}
+	return res;
+}