From c727f6a130ba9bbffd69fbe9a7a0f59b5c104f0d Mon Sep 17 00:00:00 2001 From: Bill Currie Date: Sat, 6 Jul 2019 14:45:40 +0900 Subject: [PATCH] Rewrite QuatMultVec to be faster Could be faster still using SSE, but that's another project. --- libs/util/mathlib.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index a1e11f48b..db68455f3 100644 --- a/libs/util/mathlib.c +++ b/libs/util/mathlib.c @@ -258,15 +258,18 @@ QuatMult (const quat_t q1, const quat_t q2, quat_t out) VISIBLE void QuatMultVec (const quat_t q, const vec3_t v, vec3_t out) { - vec_t s; vec3_t tv; + vec_t dqv, dqq; + vec_t s; - s = -DotProduct (q, v); + s = q[3]; CrossProduct (q, v, tv); - VectorMultAdd (tv, q[3], v, tv); - CrossProduct (q, tv, out); - VectorMultSub (out, s, q, out); - VectorMultAdd (out, q[3], tv, out); + dqv = DotProduct (q, v); + dqq = DotProduct (q, q); + VectorScale (tv, s, tv); + VectorMultAdd (tv, dqv, q, tv); + VectorAdd (tv, tv, tv); + VectorMultAdd (tv, s * s - dqq, v, out); } VISIBLE void