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)); }