Add HMM_Mat4ToQuaternion (#103)

* Add mat4 to quaternion method

* Capitalize variables

.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

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);
 }

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)

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);