diff --git a/Engine/Source/Common/Include/Common/Math/Matrix.h b/Engine/Source/Common/Include/Common/Math/Matrix.h index acc49e2f3..efffeed18 100644 --- a/Engine/Source/Common/Include/Common/Math/Matrix.h +++ b/Engine/Source/Common/Include/Common/Math/Matrix.h @@ -24,6 +24,9 @@ namespace Common { max }; + template + struct Quaternion; + // matrix stored in row-major template requires ValidMatDims @@ -115,6 +118,10 @@ namespace Common { bool CanInverse() const; Mat Inverse() const; T Determinant() const; + + Vec ExtractTranslation() const; + Vec ExtractScale() const; + Quaternion ExtractRotation() const; }; template @@ -812,6 +819,82 @@ namespace Common { return result; } + template + Vec Mat::ExtractTranslation() const + { + static_assert( R == 4 && C == 4); + Vec ret = Vec(this->data[3], this->data[7], this->data[11]); + return ret; + } + + template + Quaternion Mat::ExtractRotation() const + { + static_assert( R == 4 && C == 4); + Quaternion ret = Quaternion(1, 0, 0, 0); + + T sx = Vec(this->data[0], this->data[4], this->data[8]).Model(); + T sy = Vec(this->data[1], this->data[5], this->data[9]).Model(); + T sz = Vec(this->data[2], this->data[6], this->data[10]).Model(); + T det = this->Determinant(); + if (det < 0) { + sx = -sx; + } + + T m11 = this->data[0] / sx, m21 = this->data[4] / sx, m31 = this->data[8] / sx; + T m12 = this->data[1] / sy, m22 = this->data[5] / sy, m32 = this->data[9] / sy; + T m13 = this->data[2] / sz, m23 = this->data[6] / sz, m33 = this->data[10] / sz; + + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm + // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled) + T trace = m11 + m22 + m33; + if (trace > 0) { + T s = 0.5 / std::sqrt(trace + 1.0); + ret.w = 0.25 / s; + ret.x = (m32 - m23) * s; + ret.y = (m13 - m31) * s; + ret.z = (m21 - m12) * s; + } else if (m11 > m22 && m11 > m33) { + T s = 2.0 * std::sqrt(1.0 + m11 - m22 - m33); + ret.w = ( m32 - m23 ) / s; + ret.x = 0.25 * s; + ret.y = ( m12 + m21 ) / s; + ret.z = ( m13 + m31 ) / s; + } else if (m22 > m33) { + T s = 2.0 * std::sqrt( 1.0 + m22 - m11 - m33 ); + ret.w = ( m13 - m31 ) / s; + ret.x = ( m12 + m21 ) / s; + ret.y = 0.25 * s; + ret.z = ( m23 + m32 ) / s; + } else { + T s = 2.0 * std::sqrt( 1.0 + m33 - m11 - m22 ); + ret.w = ( m21 - m12 ) / s; + ret.x = ( m13 + m31 ) / s; + ret.y = ( m23 + m32 ) / s; + ret.z = 0.25 * s; + } + + return ret; + } + + template + Vec Mat::ExtractScale() const + { + static_assert( R == 4 && C == 4); + + T sx = Vec(this->data[0], this->data[4], this->data[8]).Model(); + T sy = Vec(this->data[1], this->data[5], this->data[9]).Model(); + T sz = Vec(this->data[2], this->data[6], this->data[10]).Model(); + + T det = this->Determinant(); + if (det < 0) { + sx = -sx; + } + Vec ret = Vec(sx, sy, sz); + + return ret; + } + template template requires ValidSubMatDims diff --git a/Engine/Source/Common/Include/Common/Math/Transform.h b/Engine/Source/Common/Include/Common/Math/Transform.h index 8a37f48b7..84c64d08d 100644 --- a/Engine/Source/Common/Include/Common/Math/Transform.h +++ b/Engine/Source/Common/Include/Common/Math/Transform.h @@ -23,6 +23,7 @@ namespace Common { static Transform LookAt(const Vec& inPosition, const Vec& inTargetPosition, const Vec& inUpDirection = VecConsts::unitZ); Transform(); + Transform(const Mat& inMatrix); Transform(Quaternion inRotation, Vec inTranslation); Transform(Vec inScale, Quaternion inRotation, Vec inTranslation); Transform(const Transform& other); @@ -160,6 +161,14 @@ namespace Common { this->translation = VecConsts::zero; } + template + Transform::Transform(const Mat& inMatrix) + { + this->translation = inMatrix.ExtractTranslation(); + this->rotation = inMatrix.ExtractRotation(); + this->scale = inMatrix.ExtractScale(); + } + template Transform::Transform(Quaternion inRotation, Vec inTranslation) { diff --git a/Engine/Source/Common/Test/MathTest.cpp b/Engine/Source/Common/Test/MathTest.cpp index e66d9634b..e745be6ae 100644 --- a/Engine/Source/Common/Test/MathTest.cpp +++ b/Engine/Source/Common/Test/MathTest.cpp @@ -351,6 +351,22 @@ TEST(MathTest, SubMatrixTest) ASSERT_TRUE(m1.Row(2) == FVec3(7, 8, 9)); } +TEST(MathTest, MatExtractionTest) +{ + const FMat4x4 m = { + 0, -2, 0, 7, + 4, 0, 0, 5, + 0, 0, 3, 3, + 7, 5, 3, 1 + }; + + const FTransform trans = FTransform(m); + + ASSERT_TRUE(trans.translation == FVec3(7.0f, 5.0f, 3.0f)); + ASSERT_TRUE(trans.scale == FVec3(4.0f, 2.0f, 3.0f)); + ASSERT_TRUE(trans.rotation == FQuat(0.7071067f, .0f, .0f, .7071067f)); +} + TEST(MathTest, MatViewTest) { const FMat3x3 v0( diff --git a/Engine/Source/Runtime/Src/System/Transform.cpp b/Engine/Source/Runtime/Src/System/Transform.cpp index 5c3b84800..28ebe3399 100644 --- a/Engine/Source/Runtime/Src/System/Transform.cpp +++ b/Engine/Source/Runtime/Src/System/Transform.cpp @@ -51,7 +51,7 @@ namespace Runtime { const auto& localToWorldMatrix = worldTransform.localToWorld.GetTransformMatrix(); const auto& parentLocalToWorldMatrix = parentWorldTransform.localToWorld.GetTransformMatrix(); - // TODO localTransform.localToParent = parentLocalToWorldMatrix.Inverse() * localToWorldMatrix + localTransform.localToParent = Common::FTransform(parentLocalToWorldMatrix.Inverse() * localToWorldMatrix); } // Step2: update world transforms @@ -66,7 +66,7 @@ namespace Runtime { const auto& parentLocalToWorldMatrix = parentWorldTransform.localToWorld.GetTransformMatrix(); const auto& childLocalToParentMatrix = childLocalTransform.localToParent.GetTransformMatrix(); - // TODO childWorldTransform.localToWorld = parentLocalToWorldMatrix * childLocalToParentMatrix + childWorldTransform.localToWorld = Common::FTransform(parentLocalToWorldMatrix * childLocalToParentMatrix); }; for (const auto e : pendingUpdateChildrenWorldTransforms) {