From 79dd65e1ccfdef85747cbc26c8f868b42df060d6 Mon Sep 17 00:00:00 2001 From: Benjamin Jillich <43751992+amzn-jillich@users.noreply.github.com> Date: Mon, 3 Jan 2022 10:20:27 +0100 Subject: [PATCH] Quaternion shortest equivalent (#6472) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added new helper functions to get the shortest equivalent of the rotation. In case the w component of the quaternion is negative the rotation is > 180° and taking the longer path. The quaternion will be inverted in that case to take the shortest path of rotation. * Added unit test. * Renamed the angle parameter of the CreateRotationX/Y/Z() functions into angleInRadians. Signed-off-by: Benjamin Jillich --- .../Framework/AzCore/AzCore/Math/Quaternion.h | 16 +++++++--- .../AzCore/AzCore/Math/Quaternion.inl | 29 +++++++++++++++---- .../AzCore/Tests/Math/QuaternionTests.cpp | 19 ++++++++++-- 3 files changed, 51 insertions(+), 13 deletions(-) diff --git a/Code/Framework/AzCore/AzCore/Math/Quaternion.h b/Code/Framework/AzCore/AzCore/Math/Quaternion.h index 83e8012a62..ad454aab4f 100644 --- a/Code/Framework/AzCore/AzCore/Math/Quaternion.h +++ b/Code/Framework/AzCore/AzCore/Math/Quaternion.h @@ -54,11 +54,11 @@ namespace AZ //! Sets components using a Vector3 for the imaginary part and a float for the real part. static Quaternion CreateFromVector3AndValue(const Vector3& v, float w); - //! Sets the quaternion to be a rotation around a specified axis. + //! Sets the quaternion to be a rotation around a specified axis in radians. //! @{ - static Quaternion CreateRotationX(float angle); - static Quaternion CreateRotationY(float angle); - static Quaternion CreateRotationZ(float angle); + static Quaternion CreateRotationX(float angleInRadians); + static Quaternion CreateRotationY(float angleInRadians); + static Quaternion CreateRotationZ(float angleInRadians); //! @} //! Creates a quaternion from a Matrix3x3 @@ -168,6 +168,14 @@ namespace AZ float NormalizeWithLengthEstimate(); //! @} + //! Get the shortest equivalent of the rotation. + //! In case the w component of the quaternion is negative the rotation is > 180° and taking the longer path. + //! The quaternion will be inverted in that case to take the shortest path of rotation. + //! @{ + Quaternion GetShortestEquivalent() const; + void ShortestEquivalent(); + //! @} + //! Linearly interpolate towards a destination quaternion. //! @param[in] dest The quaternion to interpolate towards. //! @param[in] t Normalized interpolation value where 0.0 represents the current and 1.0 the destination value. diff --git a/Code/Framework/AzCore/AzCore/Math/Quaternion.inl b/Code/Framework/AzCore/AzCore/Math/Quaternion.inl index 37319ae146..bd51a90fca 100644 --- a/Code/Framework/AzCore/AzCore/Math/Quaternion.inl +++ b/Code/Framework/AzCore/AzCore/Math/Quaternion.inl @@ -73,27 +73,27 @@ namespace AZ } - AZ_MATH_INLINE Quaternion Quaternion::CreateRotationX(float angle) + AZ_MATH_INLINE Quaternion Quaternion::CreateRotationX(float angleInRadians) { - const float halfAngle = 0.5f * angle; + const float halfAngle = 0.5f * angleInRadians; float sin, cos; SinCos(halfAngle, sin, cos); return Quaternion(sin, 0.0f, 0.0f, cos); } - AZ_MATH_INLINE Quaternion Quaternion::CreateRotationY(float angle) + AZ_MATH_INLINE Quaternion Quaternion::CreateRotationY(float angleInRadians) { - const float halfAngle = 0.5f * angle; + const float halfAngle = 0.5f * angleInRadians; float sin, cos; SinCos(halfAngle, sin, cos); return Quaternion(0.0f, sin, 0.0f, cos); } - AZ_MATH_INLINE Quaternion Quaternion::CreateRotationZ(float angle) + AZ_MATH_INLINE Quaternion Quaternion::CreateRotationZ(float angleInRadians) { - const float halfAngle = 0.5f * angle; + const float halfAngle = 0.5f * angleInRadians; float sin, cos; SinCos(halfAngle, sin, cos); return Quaternion(0.0f, 0.0f, sin, cos); @@ -345,6 +345,23 @@ namespace AZ } + AZ_MATH_INLINE Quaternion Quaternion::GetShortestEquivalent() const + { + if (GetW() < 0.0f) + { + return -(*this); + } + + return *this; + } + + + AZ_MATH_INLINE void Quaternion::ShortestEquivalent() + { + *this = GetShortestEquivalent(); + } + + AZ_MATH_INLINE Quaternion Quaternion::Lerp(const Quaternion& dest, float t) const { if (Dot(dest) >= 0.0f) diff --git a/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp b/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp index 9ef38d7115..3de60a1fd4 100644 --- a/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp +++ b/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp @@ -11,6 +11,7 @@ #include #include #include +#include using namespace AZ; @@ -453,7 +454,7 @@ namespace UnitTest AZ::Quaternion backFromScaledAxisAngle = AZ::Quaternion::CreateFromScaledAxisAngle(scaledAxisAngle); // Compare the original quaternion with the one after the conversion. - EXPECT_TRUE(testQuat.IsClose(backFromScaledAxisAngle, 1e-6f)); + EXPECT_THAT(testQuat, IsCloseTolerance(backFromScaledAxisAngle, 1e-6f)); } TEST_P(QuaternionScaledAxisAngleConversionFixture, AxisAngleQuatRoundtripTests) @@ -467,7 +468,7 @@ namespace UnitTest // Convert the axis-angle back into a quaternion and compare the original quaternion with the one after the conversion. const AZ::Quaternion backFromAxisAngle = AZ::Quaternion::CreateFromAxisAngle(axis, angle); - EXPECT_TRUE(testQuat.IsClose(backFromAxisAngle, 1e-6f)); + EXPECT_THAT(testQuat, IsCloseTolerance(backFromAxisAngle, 1e-6f)); } TEST_P(QuaternionScaledAxisAngleConversionFixture, CompareAxisAngleConversionTests) @@ -506,8 +507,20 @@ namespace UnitTest } const AZ::Quaternion backFromAxisAngle = AZ::Quaternion::CreateFromAxisAngle(axisFromScaledResult, angleFromScaledResult); - EXPECT_TRUE(testQuat.IsClose(backFromAxisAngle, 1e-6f)); + EXPECT_THAT(testQuat, IsCloseTolerance(backFromAxisAngle, 1e-6f)); } INSTANTIATE_TEST_CASE_P(MATH_Quaternion, QuaternionScaledAxisAngleConversionFixture, ::testing::ValuesIn(RotationRepresentationConversionTestQuats)); + + TEST(MATH_Quaternion, ShortestEquivalent) + { + const AZ::Quaternion testQuat = AZ::Quaternion::CreateRotationX(AZ::Constants::HalfPi * 3.0f); + + AZ::Quaternion absQuat = testQuat; + absQuat.ShortestEquivalent(); + EXPECT_THAT(testQuat.GetShortestEquivalent(), IsCloseTolerance(absQuat, 1e-6f)); + + const float angle = absQuat.GetEulerRadians().GetX(); + EXPECT_THAT(angle, testing::FloatEq(-AZ::Constants::HalfPi)); + } }