From 149c18d449ac46ad6c5bf99ca7ae811991bfdb58 Mon Sep 17 00:00:00 2001 From: Olivier Perret Date: Sun, 29 Oct 2023 18:32:09 +0100 Subject: [PATCH] Add HMM_RotateQV3(), for rotating a vec3 by a quaternion --- HandmadeMath.h | 13 +++++++++++++ test/categories/QuaternionOps.h | 22 ++++++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/HandmadeMath.h b/HandmadeMath.h index e53ecd3..edfda72 100644 --- a/HandmadeMath.h +++ b/HandmadeMath.h @@ -2528,6 +2528,19 @@ static inline HMM_Quat HMM_QFromAxisAngle_LH(HMM_Vec3 Axis, float AngleOfRotatio } +// implementation from +// https://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/ +COVERAGE(HMM_RotateQV3, 1) +static inline HMM_Vec3 HMM_RotateQV3(HMM_Quat Q, HMM_Vec3 V) +{ + ASSERT_COVERED(HMM_RotateQV3); + + HMM_Vec3 t = HMM_MulV3F(HMM_Cross(Q.XYZ, V), 2); + return HMM_AddV3(V, HMM_AddV3(HMM_MulV3F(t, Q.W), HMM_Cross(Q.XYZ, t))); +} + + + #ifdef __cplusplus } #endif diff --git a/test/categories/QuaternionOps.h b/test/categories/QuaternionOps.h index 20173be..c96096c 100644 --- a/test/categories/QuaternionOps.h +++ b/test/categories/QuaternionOps.h @@ -274,3 +274,25 @@ TEST(QuaternionOps, FromAxisAngle) EXPECT_NEAR(result.W, 0.707107f, 0.001f); } } + +TEST(QuaternionOps, RotateVector) +{ + { + HMM_Vec3 axis = HMM_V3(0.0f, 1.0f, 0.0f); + float angle = -HMM_PI32 / 2.0f; + HMM_Quat q = HMM_QFromAxisAngle_RH(axis, angle); + HMM_Vec3 result = HMM_RotateQV3(q, HMM_V3(1.0f, 0.0f, 0.0f)); + EXPECT_NEAR(result.X, 0.0f, 0.001f); + EXPECT_NEAR(result.Y, 0.0f, 0.001f); + EXPECT_NEAR(result.Z, 1.0f, 0.001f); + } + { + HMM_Vec3 axis = HMM_V3(1.0f, 0.0f, 0.0f); + float angle = HMM_PI32 / 4.0f; + HMM_Quat q = HMM_QFromAxisAngle_RH(axis, angle); + HMM_Vec3 result = HMM_RotateQV3(q, HMM_V3(0.0f, 0.0f, 1.0f)); + EXPECT_NEAR(result.X, 0.0f, 0.001f); + EXPECT_NEAR(result.Y, -0.707170f, 0.001f); + EXPECT_NEAR(result.Z, 0.707170f, 0.001f); + } +}