diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h index 96e78e9b0..d34c86f1d 100644 --- a/include/QF/mathlib.h +++ b/include/QF/mathlib.h @@ -413,6 +413,7 @@ void RotatePointAroundVector (vec3_t dst, const vec3_t axis, const vec3_t point, float degrees); void QuatMult (const quat_t q1, const quat_t q2, quat_t out); +void QuatMultVec (const quat_t q, const vec3_t v, vec3_t out); void QuatInverse (const quat_t in, quat_t out); void QuatExp (const quat_t a, quat_t b); void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical); diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index f33ecf82e..1bdc804ac 100644 --- a/libs/util/mathlib.c +++ b/libs/util/mathlib.c @@ -181,6 +181,20 @@ QuatMult (const quat_t q1, const quat_t q2, quat_t out) out[0] = s; } +VISIBLE void +QuatMultVec (const quat_t q, const vec3_t v, vec3_t out) +{ + vec_t s; + vec3_t tv; + + s = -DotProduct (q + 1, v); + CrossProduct (q + 1, v, tv); + VectorMultAdd (tv, q[0], v, tv); + CrossProduct (q + 1, tv, out); + VectorMultSub (out, s, q + 1, out); + VectorMultAdd (out, q[0], tv, out); +} + VISIBLE void QuatInverse (const quat_t in, quat_t out) { diff --git a/libs/util/test/test-quat.c b/libs/util/test/test-quat.c index 0b2defb13..63f06671d 100644 --- a/libs/util/test/test-quat.c +++ b/libs/util/test/test-quat.c @@ -82,6 +82,52 @@ fail: return 0; } +static int +test_rotation2 (const vec3_t angles) +{ + int i; + vec3_t forward, right, up; + + quat_t quat; + vec3_t f, r, u; + vec3_t vf = {1, 0, 0}; + vec3_t vr = {0, -1, 0}; + vec3_t vu = {0, 0, 1}; + + AngleVectors (angles, forward, right, up); + + AngleQuat (angles, quat); + // rotate forward vector + QuatMultVec (quat, vf, f); + // rotate right vector + QuatMultVec (quat, vr, r); + // rotate up vector + QuatMultVec (quat, vu, u); + + for (i = 0; i < 3; i++) + if (!compare (forward[i], f[i])) + goto fail; + + for (i = 0; i < 3; i++) + if (!compare (right[i], r[i])) + goto fail; + + for (i = 0; i < 3; i++) + if (!compare (up[i], u[i])) + 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\n", f[0], f[1], f[2]); + printf ("%g %g %g\n", r[0], r[1], r[2]); + printf ("%g %g %g\n", u[0], u[1], u[2]); + return 0; +} + int main (int argc, const char **argv) { @@ -92,5 +138,10 @@ main (int argc, const char **argv) if (!test_rotation (test_angles[i])) res = 1; } + + for (i = 0; i < num_angle_tests; i ++) { + if (!test_rotation2 (test_angles[i])) + res = 1; + } return res; }