Quaternion to scaled-axis angle representation (and back) helpers (#6421)

Direct conversion helpers for quaternion to the scaled axis-angle representation and back without the need to convert them first to the axis-angle format and manually scale (or normalize on the way back). This also avoids having to deal with the special case of an identity representation which is 0,0,0 in the scaled axis-angle format while our convention for axis-angle is 0,1,0 for the axis and 0 for the angle.

Added unit tests that check the conversion round-trips from quaternion -> (scaled) axis-angle -> quaternion as well as comparing the scaled axis-angle representations from the direct helper functions as well as the axis-angle while manually scaling/normalizing.

Signed-off-by: Benjamin Jillich <jillich@amazon.com>

Code/Framework/AzCore/AzCore/Math/Quaternion.cpp

@@ -254,13 +254,13 @@ namespace AZ
                 Method("CreateFromMatrix3x3", &Quaternion::CreateFromMatrix3x3)->
                 Method("CreateFromMatrix4x4", &Quaternion::CreateFromMatrix4x4)->
                 Method("CreateFromAxisAngle", &Quaternion::CreateFromAxisAngle)->
+                Method("CreateFromScaledAxisAngle", &Quaternion::CreateFromScaledAxisAngle)->
                 Method("CreateShortestArc", &Quaternion::CreateShortestArc)->
                 Method("CreateFromEulerAnglesDegrees", &Quaternion::CreateFromEulerAnglesDegrees)
                 ;
         }
     }
 
-
     Quaternion Quaternion::CreateFromMatrix3x3(const Matrix3x3& m)
     {
         return CreateFromBasis(m.GetBasisX(), m.GetBasisY(), m.GetBasisZ());
@@ -430,4 +430,24 @@ namespace AZ
             outAngle = 0.0f;
         }
     }
+
+
+    Vector3 Quaternion::ConvertToScaledAxisAngle() const
+    {
+        // Take the log of the quaternion to convert it to the exponential map
+        // and multiply it by 2.0 to bring it into the scaled axis-angle representation.
+        const AZ::Vector3 imaginary = GetImaginary();
+        const float length = imaginary.GetLength();
+        if (length < AZ::Constants::FloatEpsilon)
+        {
+            return imaginary * 2.0f;
+        }
+        else
+        {
+            const float halfAngle = acosf(AZ::GetClamp(GetW(), -1.0f, 1.0f));
+
+            // Multiply by 2.0 to convert the half angle into the full one.
+            return halfAngle * 2.0f * (imaginary / length);
+        }
+    }
 }

Code/Framework/AzCore/AzCore/Math/Quaternion.h

@@ -77,6 +77,9 @@ namespace AZ
 
         static Quaternion CreateFromAxisAngle(const Vector3& axis, float angle);
 
+        //! Create a quaternion from a scaled axis-angle representation.
+        static Quaternion CreateFromScaledAxisAngle(const Vector3& scaledAxisAngle);
+
         static Quaternion CreateShortestArc(const Vector3& v1, const Vector3& v2);
 
         //! Creates a quaternion using rotation in degrees about the axes. First rotated about the X axis, followed by the Y axis, then the Z axis.
@@ -231,6 +234,9 @@ namespace AZ
         //! @param[out] outAngle A float rotation angle around the axis in radians.
         void ConvertToAxisAngle(Vector3& outAxis, float& outAngle) const;
 
+        //! Convert the quaternion into scaled axis-angle representation.
+        Vector3 ConvertToScaledAxisAngle() const;
+
         //! Returns the imaginary (X/Y/Z) portion of the quaternion.
         Vector3 GetImaginary() const;
 

Code/Framework/AzCore/AzCore/Math/Quaternion.inl

@@ -109,6 +109,24 @@ namespace AZ
     }
 
 
+    AZ_MATH_INLINE Quaternion Quaternion::CreateFromScaledAxisAngle(const Vector3& scaledAxisAngle)
+    {
+        const AZ::Vector3 exponentialMap = scaledAxisAngle / 2.0f;
+        const float halfAngle = exponentialMap.GetLength();
+
+        if (halfAngle < AZ::Constants::FloatEpsilon)
+        {
+            return AZ::Quaternion::CreateFromVector3AndValue(exponentialMap, 1.0f).GetNormalized();
+        }
+        else
+        {
+            float sin, cos;
+            SinCos(halfAngle, sin, cos);
+            return AZ::Quaternion::CreateFromVector3AndValue((sin / halfAngle) * exponentialMap, cos);
+        }
+    }
+
+
     AZ_MATH_INLINE void Quaternion::StoreToFloat4(float* values) const
     {
         Simd::Vec4::StoreUnaligned(values, m_value);

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

@@ -408,4 +408,106 @@ namespace UnitTest
         Matrix4x4 m = Matrix4x4::CreateFromQuaternion(rotQuat);
         AZ_TEST_ASSERT(m.IsClose(rotMatrix));
     }
+
+    class QuaternionScaledAxisAngleConversionFixture
+        : public ::testing::TestWithParam<AZ::Quaternion>
+    {
+    public:
+        AZ::Quaternion GetAbs(const AZ::Quaternion& in)
+        {
+            // Take the shortest path for quaternions containing rotations bigger than 180.0°.
+            if (in.GetW() < 0.0f)
+            {
+                return -in;
+            }
+
+            return in;
+        }
+    };
+
+    static const AZ::Quaternion RotationRepresentationConversionTestQuats[] =
+    {
+        AZ::Quaternion::CreateIdentity(),
+        -AZ::Quaternion::CreateIdentity(),
+        AZ::Quaternion::CreateRotationX(AZ::Constants::TwoPi),
+        AZ::Quaternion::CreateRotationY(AZ::Constants::Pi),
+        AZ::Quaternion::CreateRotationZ(AZ::Constants::HalfPi),
+        AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi),
+        AZ::Quaternion(0.64f, 0.36f, 0.48f, 0.48f),
+        AZ::Quaternion(0.70f, -0.34f, 0.10f, 0.62f),
+        AZ::Quaternion(-0.38f, 0.34f, 0.70f, -0.50f),
+        AZ::Quaternion(0.70f, -0.34f, -0.38f, 0.50f),
+        AZ::Quaternion(0.00f, 0.00f, -0.28f, 0.96f),
+        AZ::Quaternion(0.24f, -0.64f, 0.72f, 0.12f),
+        AZ::Quaternion(-0.66f, 0.62f, 0.42f, 0.06f)
+    };
+
+    TEST_P(QuaternionScaledAxisAngleConversionFixture, ScaledAxisAngleQuatRoundtripTests)
+    {
+        const AZ::Quaternion testQuat = GetAbs(GetParam());
+
+        // Convert test quaternion to scaled axis-angle representation.
+        const AZ::Vector3 scaledAxisAngle = testQuat.ConvertToScaledAxisAngle();
+
+        // Convert the scaled axis-angle back into a quaternion.
+        AZ::Quaternion backFromScaledAxisAngle = AZ::Quaternion::CreateFromScaledAxisAngle(scaledAxisAngle);
+
+        // Compare the original quaternion with the one after the conversion.
+        EXPECT_TRUE(testQuat.IsClose(backFromScaledAxisAngle, 1e-6f));
+    }
+
+    TEST_P(QuaternionScaledAxisAngleConversionFixture, AxisAngleQuatRoundtripTests)
+    {
+        const AZ::Quaternion testQuat = GetAbs(GetParam());
+
+        // Convert test quaternion to axis-angle representation.
+        AZ::Vector3 axis;
+        float angle;
+        testQuat.ConvertToAxisAngle(axis, angle);
+
+        // 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));
+    }
+
+    TEST_P(QuaternionScaledAxisAngleConversionFixture, CompareAxisAngleConversionTests)
+    {
+        const AZ::Quaternion testQuat = GetAbs(GetParam());
+
+        // Convert test quaternion to scaled axis-angle representation.
+        const AZ::Vector3 scaledAxisAngle = testQuat.ConvertToScaledAxisAngle();
+
+        // Convert test quaternion to axis-angle representation and scale it manually.
+        AZ::Vector3 axis;
+        float angle;
+        testQuat.ConvertToAxisAngle(axis, angle);
+
+        // Compare the scaled result to the version from the helper that directly converts it to scaled axis-angle.
+        AZ::Vector3 scaledResult = axis*angle;
+        EXPECT_TRUE(scaledResult.IsClose(scaledAxisAngle, 1e-5f));
+    }
+
+    TEST_P(QuaternionScaledAxisAngleConversionFixture, CompareScaledAxisAngleConversionTests)
+    {
+        const AZ::Quaternion testQuat = GetAbs(GetParam());
+
+        // Convert test quaternion to axis-angle representation and scale it manually.
+        AZ::Vector3 axis;
+        float angle;
+        testQuat.ConvertToAxisAngle(axis, angle);
+        AZ::Vector3 scaledResult = axis*angle;
+
+        // Special case handling for identity rotation.
+        AZ::Vector3 axisFromScaledResult = scaledResult.GetNormalized();
+        float angleFromScaledResult = scaledResult.GetLength();
+        if (AZ::IsClose(angleFromScaledResult, 0.0f))
+        {
+            axisFromScaledResult = AZ::Vector3::CreateAxisY();
+        }
+
+        const AZ::Quaternion backFromAxisAngle = AZ::Quaternion::CreateFromAxisAngle(axisFromScaledResult, angleFromScaledResult);
+        EXPECT_TRUE(testQuat.IsClose(backFromAxisAngle, 1e-6f));
+    }
+
+    INSTANTIATE_TEST_CASE_P(MATH_Quaternion, QuaternionScaledAxisAngleConversionFixture, ::testing::ValuesIn(RotationRepresentationConversionTestQuats));
 }