Quaternion shortest equivalent (#6472)

* 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 <jillich@amazon.com>

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.

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)

Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp

@@ -11,6 +11,7 @@
 #include <AzCore/Math/Matrix3x3.h>
 #include <AzCore/Math/Transform.h>
 #include <AzCore/UnitTest/TestTypes.h>
+#include <AZTestShared/Math/MathTestHelpers.h>
 
 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));
+    }
 }