From d20aa935ba9015299f5b606f81460c4d7aa278c5 Mon Sep 17 00:00:00 2001 From: Benjamin Jillich <43751992+amzn-jillich@users.noreply.github.com> Date: Fri, 17 Dec 2021 09:43:35 +0100 Subject: [PATCH] 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 --- .../AzCore/AzCore/Math/Quaternion.cpp | 22 +++- .../Framework/AzCore/AzCore/Math/Quaternion.h | 6 ++ .../AzCore/AzCore/Math/Quaternion.inl | 18 ++++ .../AzCore/Tests/Math/QuaternionTests.cpp | 102 ++++++++++++++++++ 4 files changed, 147 insertions(+), 1 deletion(-) diff --git a/Code/Framework/AzCore/AzCore/Math/Quaternion.cpp b/Code/Framework/AzCore/AzCore/Math/Quaternion.cpp index e06cd184ad..fac70bd05a 100644 --- a/Code/Framework/AzCore/AzCore/Math/Quaternion.cpp +++ b/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); + } + } } diff --git a/Code/Framework/AzCore/AzCore/Math/Quaternion.h b/Code/Framework/AzCore/AzCore/Math/Quaternion.h index f2c266ed3e..83e8012a62 100644 --- a/Code/Framework/AzCore/AzCore/Math/Quaternion.h +++ b/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; diff --git a/Code/Framework/AzCore/AzCore/Math/Quaternion.inl b/Code/Framework/AzCore/AzCore/Math/Quaternion.inl index 82cd9078fa..37319ae146 100644 --- a/Code/Framework/AzCore/AzCore/Math/Quaternion.inl +++ b/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); diff --git a/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp b/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp index cbff646990..9ef38d7115 100644 --- a/Code/Framework/AzCore/Tests/Math/QuaternionTests.cpp +++ b/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 + { + 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)); }