diff --git a/HandmadeMath.h b/HandmadeMath.h index b451012..13de666 100644 --- a/HandmadeMath.h +++ b/HandmadeMath.h @@ -1245,6 +1245,8 @@ HMM_INLINE hmm_mat4 HMM_DivideMat4f(hmm_mat4 Matrix, float Scalar) HMM_EXTERN hmm_mat4 HMM_DivideMat4f(hmm_mat4 Matrix, float Scalar); #endif +HMM_EXTERN hmm_quaternion HMM_Mat4ToQuaternion(hmm_mat4 Matrix); + /* * Common graphics transformations @@ -2345,6 +2347,42 @@ hmm_mat4 HMM_DivideMat4f(hmm_mat4 Matrix, float Scalar) } #endif +hmm_quaternion HMM_Mat4ToQuaternion(hmm_mat4 m) +{ + hmm_quaternion q; + + float trace = m.Elements[0][0] + m.Elements[1][1] + m.Elements[2][2]; + if (trace > 0) { + float s = 0.5f / HMM_SquareRootF(trace + 1.0f); + q.X = (m.Elements[2][1] - m.Elements[1][2] ) * s; + q.Y = (m.Elements[0][2] - m.Elements[2][0] ) * s; + q.Z = (m.Elements[1][0] - m.Elements[0][1] ) * s; + q.W = 0.25f / s; + } else { + if (m.Elements[0][0] > m.Elements[1][1] && m.Elements[0][0] > m.Elements[2][2]) { + float s = 2.0f * HMM_SquareRootF(1.0f + m.Elements[0][0] - m.Elements[1][1] - m.Elements[2][2]); + q.X = 0.25f * s; + q.Y = (m.Elements[0][1] + m.Elements[1][0] ) / s; + q.Z = (m.Elements[0][2] + m.Elements[2][0] ) / s; + q.W = (m.Elements[2][1] - m.Elements[1][2] ) / s; + } else if (m.Elements[1][1] > m.Elements[2][2]) { + float s = 2.0f * HMM_SquareRootF( 1.0f + m.Elements[1][1] - m.Elements[0][0] - m.Elements[2][2]); + q.X = (m.Elements[0][1] + m.Elements[1][0] ) / s; + q.Y = 0.25f * s; + q.Z = (m.Elements[1][2] + m.Elements[2][1] ) / s; + q.W = (m.Elements[0][2] - m.Elements[2][0] ) / s; + } else { + float s = 2.0f * HMM_SquareRootF( 1.0f + m.Elements[2][2] - m.Elements[0][0] - m.Elements[1][1] ); + q.X = (m.Elements[0][2] + m.Elements[2][0] ) / s; + q.Y = (m.Elements[1][2] + m.Elements[2][1] ) / s; + q.Z = 0.25f * s; + q.W = (m.Elements[1][0] - m.Elements[0][1] ) / s; + } + } + + return q; +} + hmm_mat4 HMM_Rotate(float Angle, hmm_vec3 Axis) { hmm_mat4 Result = HMM_Mat4d(1.0f); diff --git a/test/categories/MatrixOps.h b/test/categories/MatrixOps.h new file mode 100644 index 0000000..d86400b --- /dev/null +++ b/test/categories/MatrixOps.h @@ -0,0 +1,56 @@ +#include "../HandmadeTest.h" + +TEST(MatrixOps, Transpose) +{ + hmm_mat4 m4 = HMM_Mat4(); // will have 1 - 16 + + // Fill the matrix + int Counter = 1; + for (int Column = 0; Column < 4; ++Column) + { + for (int Row = 0; Row < 4; ++Row) + { + m4.Elements[Column][Row] = Counter; + ++Counter; + } + } + + // Test the matrix + hmm_mat4 result = HMM_Transpose(m4); + EXPECT_FLOAT_EQ(result.Elements[0][0], 1.0f); + EXPECT_FLOAT_EQ(result.Elements[0][1], 5.0f); + EXPECT_FLOAT_EQ(result.Elements[0][2], 9.0f); + EXPECT_FLOAT_EQ(result.Elements[0][3], 13.0f); + EXPECT_FLOAT_EQ(result.Elements[1][0], 2.0f); + EXPECT_FLOAT_EQ(result.Elements[1][1], 6.0f); + EXPECT_FLOAT_EQ(result.Elements[1][2], 10.0f); + EXPECT_FLOAT_EQ(result.Elements[1][3], 14.0f); + EXPECT_FLOAT_EQ(result.Elements[2][0], 3.0f); + EXPECT_FLOAT_EQ(result.Elements[2][1], 7.0f); + EXPECT_FLOAT_EQ(result.Elements[2][2], 11.0f); + EXPECT_FLOAT_EQ(result.Elements[2][3], 15.0f); + EXPECT_FLOAT_EQ(result.Elements[3][0], 4.0f); + EXPECT_FLOAT_EQ(result.Elements[3][1], 8.0f); + EXPECT_FLOAT_EQ(result.Elements[3][2], 12.0f); + EXPECT_FLOAT_EQ(result.Elements[3][3], 16.0f); +} + +TEST(MatrixOps, ToQuaternion) +{ + hmm_mat4 rotateY = { + 0.0f, 0.0f, -1.0f, 0.0f, // first column (X) + 0.0f, 1.0f, 0.0f, 0.0f, // second column (Y) + 1.0f, 0.0f, 0.0f, 0.0f, // third column (Z) + 0.0f, 0.0f, 0.0f, 1.0f + }; + + hmm_quaternion expected = HMM_QuaternionFromAxisAngle(HMM_Vec3(0.0f, 1.0f, 0.0f), HMM_ToRadians(90.0f)); + hmm_quaternion actualResult = HMM_Mat4ToQuaternion(rotateY); + + printf("%f %f %f %f\n", expected.X, expected.Y, expected.Z, expected.W); + + EXPECT_FLOAT_EQ(actualResult.X, expected.X); + EXPECT_FLOAT_EQ(actualResult.Y, expected.Y); + EXPECT_FLOAT_EQ(actualResult.Z, expected.Z); + EXPECT_FLOAT_EQ(actualResult.W, expected.W); +} diff --git a/test/categories/VectorOps.h b/test/categories/VectorOps.h index 32990aa..be2d44e 100644 --- a/test/categories/VectorOps.h +++ b/test/categories/VectorOps.h @@ -178,43 +178,3 @@ TEST(VectorOps, DotVec4) EXPECT_FLOAT_EQ(HMM_Dot(v1, v2), 70.0f); #endif } - - -/* - * MatrixOps tests - */ - -TEST(MatrixOps, Transpose) -{ - hmm_mat4 m4 = HMM_Mat4(); // will have 1 - 16 - - // Fill the matrix - int Counter = 1; - for (int Column = 0; Column < 4; ++Column) - { - for (int Row = 0; Row < 4; ++Row) - { - m4.Elements[Column][Row] = Counter; - ++Counter; - } - } - - // Test the matrix - hmm_mat4 result = HMM_Transpose(m4); - EXPECT_FLOAT_EQ(result.Elements[0][0], 1.0f); - EXPECT_FLOAT_EQ(result.Elements[0][1], 5.0f); - EXPECT_FLOAT_EQ(result.Elements[0][2], 9.0f); - EXPECT_FLOAT_EQ(result.Elements[0][3], 13.0f); - EXPECT_FLOAT_EQ(result.Elements[1][0], 2.0f); - EXPECT_FLOAT_EQ(result.Elements[1][1], 6.0f); - EXPECT_FLOAT_EQ(result.Elements[1][2], 10.0f); - EXPECT_FLOAT_EQ(result.Elements[1][3], 14.0f); - EXPECT_FLOAT_EQ(result.Elements[2][0], 3.0f); - EXPECT_FLOAT_EQ(result.Elements[2][1], 7.0f); - EXPECT_FLOAT_EQ(result.Elements[2][2], 11.0f); - EXPECT_FLOAT_EQ(result.Elements[2][3], 15.0f); - EXPECT_FLOAT_EQ(result.Elements[3][0], 4.0f); - EXPECT_FLOAT_EQ(result.Elements[3][1], 8.0f); - EXPECT_FLOAT_EQ(result.Elements[3][2], 12.0f); - EXPECT_FLOAT_EQ(result.Elements[3][3], 16.0f); -} diff --git a/test/hmm_test.h b/test/hmm_test.h index 5774c09..f2b9d83 100644 --- a/test/hmm_test.h +++ b/test/hmm_test.h @@ -3,15 +3,16 @@ #include "HandmadeTest.h" #include "../HandmadeMath.h" -#include "categories/ScalarMath.h" -#include "categories/Initialization.h" -#include "categories/VectorOps.h" -#include "categories/QuaternionOps.h" #include "categories/Addition.h" -#include "categories/Subtraction.h" -#include "categories/Multiplication.h" #include "categories/Division.h" #include "categories/Equality.h" +#include "categories/Initialization.h" +#include "categories/MatrixOps.h" +#include "categories/Multiplication.h" #include "categories/Projection.h" -#include "categories/Transformation.h" +#include "categories/QuaternionOps.h" +#include "categories/ScalarMath.h" #include "categories/SSE.h" +#include "categories/Subtraction.h" +#include "categories/Transformation.h" +#include "categories/VectorOps.h"