diff --git a/include/Pomdog/Math/detail/FloatingPointQuaternion.hpp b/include/Pomdog/Math/detail/FloatingPointQuaternion.hpp index 640e65dff..bcbb0b8b5 100644 --- a/include/Pomdog/Math/detail/FloatingPointQuaternion.hpp +++ b/include/Pomdog/Math/detail/FloatingPointQuaternion.hpp @@ -121,6 +121,12 @@ class POMDOG_EXPORT FloatingPointQuaternion final { [[nodiscard]] static FloatingPointQuaternion Euler(const FloatingPointVector3& rotation); + /// Returns an euler angles of the given quaternion in radians. + /// + /// @return Euler angles in pitch-yaw-roll order, in radians. + [[nodiscard]] static FloatingPointVector3 + ToEulerAngles(const FloatingPointQuaternion& quaternion) noexcept; + /// Returns pointer to the first element. [[nodiscard]] const T* Data() const noexcept; diff --git a/src/Math/detail/FloatingPointQuaternion.cpp b/src/Math/detail/FloatingPointQuaternion.cpp index f24fe3c4b..24544f5cc 100644 --- a/src/Math/detail/FloatingPointQuaternion.cpp +++ b/src/Math/detail/FloatingPointQuaternion.cpp @@ -1,6 +1,7 @@ // Copyright (c) 2013-2019 mogemimi. Distributed under the MIT license. #include "Pomdog/Math/detail/FloatingPointQuaternion.hpp" +#include "Pomdog/Math/MathHelper.hpp" #include "Pomdog/Math/Radian.hpp" #include "Pomdog/Math/detail/FloatingPointMatrix3x3.hpp" #include "Pomdog/Math/detail/FloatingPointMatrix4x4.hpp" @@ -411,6 +412,46 @@ FloatingPointQuaternion::Euler(const FloatingPointVector3& rotation) return CreateFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z); } +template +FloatingPointVector3 +FloatingPointQuaternion::ToEulerAngles(const FloatingPointQuaternion& q) noexcept +{ + const auto ww = q.W * q.W; + const auto xx = q.X * q.X; + const auto yy = q.Y * q.Y; + const auto zz = q.Z * q.Z; + + constexpr auto epsilon = std::numeric_limits::epsilon(); + constexpr auto zero = static_cast(0); + constexpr auto one = static_cast(1); + constexpr auto two = static_cast(2); + constexpr auto half = (static_cast(0.5L) - epsilon); + + const auto lengthSquared = xx + yy + zz + ww; + const auto singularityTest = q.X * q.Z + q.Y * q.W; + const auto singularityValue = half * lengthSquared; + + // NOTE: {x, y, z} == {pitch, yaw, roll} + FloatingPointVector3 pitchYawRoll; + + if (singularityTest > singularityValue) { + pitchYawRoll.X = zero; + pitchYawRoll.Y = Math::PiOver2; + pitchYawRoll.Z = two * std::atan2(q.X, q.W); + } + else if (singularityTest < -singularityValue) { + pitchYawRoll.X = zero; + pitchYawRoll.Y = -Math::PiOver2; + pitchYawRoll.Z = -two * std::atan2(q.X, q.W); + } + else { + pitchYawRoll.X = std::atan2(two * (-q.Y * q.Z + q.X * q.W), one - two * (xx + yy)); + pitchYawRoll.Y = std::asin(two * (q.X * q.Z + q.Y * q.W)); + pitchYawRoll.Z = std::atan2(two * (-q.X * q.Y + q.Z * q.W), one - two * (yy + zz)); + } + return pitchYawRoll; +} + template const T* FloatingPointQuaternion::Data() const noexcept {