diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h index 4dde62a38..b16b6b70a 100644 --- a/include/QF/mathlib.h +++ b/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); diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index a9b3d8e41..8e4871ec1 100644 --- a/libs/util/mathlib.c +++ b/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) { diff --git a/libs/util/test/Makefile.am b/libs/util/test/Makefile.am index 750682068..e10bbac59 100644 --- a/libs/util/test/Makefile.am +++ b/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) diff --git a/libs/util/test/test-quat.c b/libs/util/test/test-quat.c new file mode 100644 index 000000000..0b2defb13 --- /dev/null +++ b/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; +}