From 78e6feea82836fe85769c00676fba119875e8f06 Mon Sep 17 00:00:00 2001 From: Ben Visness Date: Wed, 31 Jul 2019 16:38:03 -0500 Subject: [PATCH] Add HMM_Mat4ToQuaternion (#103) * Add mat4 to quaternion method * Capitalize variables --- .travis.yml | 6 +-- HandmadeMath.h | 84 +++++++++++++++++++++++++++++---- test/Makefile | 10 +++- test/categories/QuaternionOps.h | 63 ++++++++++++++++++++++++- 4 files changed, 145 insertions(+), 18 deletions(-) diff --git a/.travis.yml b/.travis.yml index a894d3e..38708be 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,9 +4,5 @@ compiler: - gcc install: - cd test - - make script: - - build/hmm_test_c - - build/hmm_test_c_no_sse - - build/hmm_test_cpp - - build/hmm_test_cpp_no_sse + - make all diff --git a/HandmadeMath.h b/HandmadeMath.h index a0c3504..87006f2 100644 --- a/HandmadeMath.h +++ b/HandmadeMath.h @@ -1419,6 +1419,7 @@ HMM_INLINE hmm_quaternion HMM_NLerp(hmm_quaternion Left, float Time, hmm_quatern HMM_EXTERN hmm_quaternion HMM_Slerp(hmm_quaternion Left, float Time, hmm_quaternion Right); HMM_EXTERN hmm_mat4 HMM_QuaternionToMat4(hmm_quaternion Left); +HMM_EXTERN hmm_quaternion HMM_Mat4ToQuaternion(hmm_mat4 Left); HMM_EXTERN hmm_quaternion HMM_QuaternionFromAxisAngle(hmm_vec3 Axis, float AngleOfRotation); #ifdef __cplusplus @@ -2453,7 +2454,6 @@ hmm_quaternion HMM_Slerp(hmm_quaternion Left, float Time, hmm_quaternion Right) hmm_mat4 HMM_QuaternionToMat4(hmm_quaternion Left) { hmm_mat4 Result; - Result = HMM_Mat4d(1); hmm_quaternion NormalizedQuaternion = HMM_NormalizeQuaternion(Left); @@ -2474,33 +2474,97 @@ hmm_mat4 HMM_QuaternionToMat4(hmm_quaternion Left) Result.Elements[0][0] = 1.0f - 2.0f * (YY + ZZ); Result.Elements[0][1] = 2.0f * (XY + WZ); Result.Elements[0][2] = 2.0f * (XZ - WY); + Result.Elements[0][3] = 0.0f; Result.Elements[1][0] = 2.0f * (XY - WZ); Result.Elements[1][1] = 1.0f - 2.0f * (XX + ZZ); Result.Elements[1][2] = 2.0f * (YZ + WX); + Result.Elements[1][3] = 0.0f; Result.Elements[2][0] = 2.0f * (XZ + WY); Result.Elements[2][1] = 2.0f * (YZ - WX); Result.Elements[2][2] = 1.0f - 2.0f * (XX + YY); + Result.Elements[2][3] = 0.0f; + + Result.Elements[3][0] = 0.0f; + Result.Elements[3][1] = 0.0f; + Result.Elements[3][2] = 0.0f; + Result.Elements[3][3] = 1.0f; return (Result); } +// This method taken from Mike Day at Insomniac Games. +// https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf +// +// Note that as mentioned at the top of the paper, the paper assumes the matrix +// would be *post*-multiplied to a vector to rotate it, meaning the matrix is +// the transpose of what we're dealing with. But, because our matrices are +// stored in column-major order, the indices *appear* to match the paper. +// +// For example, m12 in the paper is row 1, column 2. We need to transpose it to +// row 2, column 1. But, because the column comes first when referencing +// elements, it looks like M.Elements[1][2]. +// +// Don't be confused! Or if you must be confused, at least trust this +// comment. :) +hmm_quaternion HMM_Mat4ToQuaternion(hmm_mat4 M) +{ + float T; + hmm_quaternion Q; + + if (M.Elements[2][2] < 0.0f) { + if (M.Elements[0][0] > M.Elements[1][1]) { + T = 1 + M.Elements[0][0] - M.Elements[1][1] - M.Elements[2][2]; + Q = HMM_Quaternion( + T, + M.Elements[0][1] + M.Elements[1][0], + M.Elements[2][0] + M.Elements[0][2], + M.Elements[1][2] - M.Elements[2][1] + ); + } else { + T = 1 - M.Elements[0][0] + M.Elements[1][1] - M.Elements[2][2]; + Q = HMM_Quaternion( + M.Elements[0][1] + M.Elements[1][0], + T, + M.Elements[1][2] + M.Elements[2][1], + M.Elements[2][0] - M.Elements[0][2] + ); + } + } else { + if (M.Elements[0][0] < -M.Elements[1][1]) { + T = 1 - M.Elements[0][0] - M.Elements[1][1] + M.Elements[2][2]; + Q = HMM_Quaternion( + M.Elements[2][0] + M.Elements[0][2], + M.Elements[1][2] + M.Elements[2][1], + T, + M.Elements[0][1] - M.Elements[1][0] + ); + } else { + T = 1 + M.Elements[0][0] + M.Elements[1][1] + M.Elements[2][2]; + Q = HMM_Quaternion( + M.Elements[1][2] - M.Elements[2][1], + M.Elements[2][0] - M.Elements[0][2], + M.Elements[0][1] - M.Elements[1][0], + T + ); + } + } + + Q = HMM_MultiplyQuaternionF(Q, 0.5f / HMM_SquareRootF(T)); + + return Q; +} + hmm_quaternion HMM_QuaternionFromAxisAngle(hmm_vec3 Axis, float AngleOfRotation) { hmm_quaternion Result; - - hmm_vec3 RotatedVector; - - float AxisNorm = 0; - float SineOfRotation = 0; - AxisNorm = HMM_SquareRootF(HMM_DotVec3(Axis, Axis)); - SineOfRotation = HMM_SinF(AngleOfRotation / 2.0f); - RotatedVector = HMM_MultiplyVec3f(Axis, SineOfRotation); + hmm_vec3 AxisNormalized = HMM_NormalizeVec3(Axis); + float SineOfRotation = HMM_SinF(AngleOfRotation / 2.0f); + Result.XYZ = HMM_MultiplyVec3f(AxisNormalized, SineOfRotation); Result.W = HMM_CosF(AngleOfRotation / 2.0f); - Result.XYZ = HMM_DivideVec3f(RotatedVector, AxisNorm); return (Result); } diff --git a/test/Makefile b/test/Makefile index 45b3261..efd324b 100644 --- a/test/Makefile +++ b/test/Makefile @@ -1,8 +1,14 @@ -BUILD_DIR=build +BUILD_DIR=./build CXXFLAGS+=-g -Wall -Wextra -pthread -Wno-missing-braces -Wno-missing-field-initializers -all: c c_no_sse cpp cpp_no_sse +all: build_all + $(BUILD_DIR)/hmm_test_c + $(BUILD_DIR)/hmm_test_c_no_sse + $(BUILD_DIR)/hmm_test_cpp + $(BUILD_DIR)/hmm_test_cpp_no_sse + +build_all: c c_no_sse cpp cpp_no_sse clean: rm -rf $(BUILD_DIR) diff --git a/test/categories/QuaternionOps.h b/test/categories/QuaternionOps.h index 6dd525d..06b5a3e 100644 --- a/test/categories/QuaternionOps.h +++ b/test/categories/QuaternionOps.h @@ -76,7 +76,7 @@ TEST(QuaternionOps, Slerp) EXPECT_FLOAT_EQ(result.W, 0.86602540f); } -TEST(QuaternionOps, ToMat4) +TEST(QuaternionOps, QuatToMat4) { const float abs_error = 0.0001f; @@ -105,6 +105,67 @@ TEST(QuaternionOps, ToMat4) EXPECT_NEAR(result.Elements[3][3], 1.0f, abs_error); } +TEST(QuaternionOps, Mat4ToQuat) +{ + const float abs_error = 0.0001f; + + // Rotate 90 degrees on the X axis + { + hmm_mat4 m = HMM_Rotate(90, HMM_Vec3(1, 0, 0)); + hmm_quaternion result = HMM_Mat4ToQuaternion(m); + + float cosf = 0.707107f; // cos(90/2 degrees) + float sinf = 0.707107f; // sin(90/2 degrees) + + EXPECT_NEAR(result.X, sinf, abs_error); + EXPECT_NEAR(result.Y, 0.0f, abs_error); + EXPECT_NEAR(result.Z, 0.0f, abs_error); + EXPECT_NEAR(result.W, cosf, abs_error); + } + + // Rotate 90 degrees on the Y axis (axis not normalized, just for fun) + { + hmm_mat4 m = HMM_Rotate(90, HMM_Vec3(0, 2, 0)); + hmm_quaternion result = HMM_Mat4ToQuaternion(m); + + float cosf = 0.707107f; // cos(90/2 degrees) + float sinf = 0.707107f; // sin(90/2 degrees) + + EXPECT_NEAR(result.X, 0.0f, abs_error); + EXPECT_NEAR(result.Y, sinf, abs_error); + EXPECT_NEAR(result.Z, 0.0f, abs_error); + EXPECT_NEAR(result.W, cosf, abs_error); + } + + // Rotate 90 degrees on the Z axis + { + hmm_mat4 m = HMM_Rotate(90, HMM_Vec3(0, 0, 1)); + hmm_quaternion result = HMM_Mat4ToQuaternion(m); + + float cosf = 0.707107f; // cos(90/2 degrees) + float sinf = 0.707107f; // sin(90/2 degrees) + + EXPECT_NEAR(result.X, 0.0f, abs_error); + EXPECT_NEAR(result.Y, 0.0f, abs_error); + EXPECT_NEAR(result.Z, sinf, abs_error); + EXPECT_NEAR(result.W, cosf, abs_error); + } + + // Rotate 135 degrees on the Y axis (this hits case 4) + { + hmm_mat4 m = HMM_Rotate(135, HMM_Vec3(0, 1, 0)); + hmm_quaternion result = HMM_Mat4ToQuaternion(m); + + float cosf = 0.3826834324f; // cos(135/2 degrees) + float sinf = 0.9238795325f; // sin(135/2 degrees) + + EXPECT_NEAR(result.X, 0.0f, abs_error); + EXPECT_NEAR(result.Y, sinf, abs_error); + EXPECT_NEAR(result.Z, 0.0f, abs_error); + EXPECT_NEAR(result.W, cosf, abs_error); + } +} + TEST(QuaternionOps, FromAxisAngle) { hmm_vec3 axis = HMM_Vec3(1.0f, 0.0f, 0.0f);