From 0a267e7afe97fed53bdf54b07c83567ddaa3d837 Mon Sep 17 00:00:00 2001
From: Chuck Walbourn <walbourn@users.noreply.github.com>
Date: Tue, 18 Jan 2022 13:35:36 -0800
Subject: [PATCH] Added ToEuler methods to Matrix and Quaternion (#115)

---
 Inc/SimpleMath.h   | 16 +++++++++++++
 Inc/SimpleMath.inl | 58 ++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 74 insertions(+)

diff --git a/Inc/SimpleMath.h b/Inc/SimpleMath.h
index 9f4e17f..5c8384c 100644
--- a/Inc/SimpleMath.h
+++ b/Inc/SimpleMath.h
@@ -542,6 +542,9 @@ namespace DirectX
 
             float Determinant() const noexcept;
 
+            // Computes rotation about y-axis (y), then x-axis (x), then z-axis (z)
+            Vector3 ToEuler() const noexcept;
+
             // Static functions
             static Matrix CreateBillboard(
                 const Vector3& object, const Vector3& cameraPosition, const Vector3& cameraUp, _In_opt_ const Vector3* cameraForward = nullptr) noexcept;
@@ -574,8 +577,12 @@ namespace DirectX
 
             static Matrix CreateFromQuaternion(const Quaternion& quat) noexcept;
 
+            // Rotates about y-axis (yaw), then x-axis (pitch), then z-axis (roll)
             static Matrix CreateFromYawPitchRoll(float yaw, float pitch, float roll) noexcept;
 
+            // Rotates about y-axis (angles.y), then x-axis (angles.x), then z-axis (angles.z)
+            static Matrix CreateFromYawPitchRoll(const Vector3& angles) noexcept;
+
             static Matrix CreateShadow(const Vector3& lightDir, const Plane& plane) noexcept;
 
             static Matrix CreateReflection(const Plane& plane) noexcept;
@@ -706,9 +713,18 @@ namespace DirectX
 
             float Dot(const Quaternion& Q) const noexcept;
 
+            // Computes rotation about y-axis (y), then x-axis (x), then z-axis (z)
+            Vector3 ToEuler() const noexcept;
+
             // Static functions
             static Quaternion CreateFromAxisAngle(const Vector3& axis, float angle) noexcept;
+
+            // Rotates about y-axis (yaw), then x-axis (pitch), then z-axis (roll)
             static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) noexcept;
+
+            // Rotates about y-axis (angles.y), then x-axis (angles.x), then z-axis (angles.z)
+            static Quaternion CreateFromYawPitchRoll(const Vector3& angles) noexcept;
+
             static Quaternion CreateFromRotationMatrix(const Matrix& M) noexcept;
 
             static void Lerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion& result) noexcept;
diff --git a/Inc/SimpleMath.inl b/Inc/SimpleMath.inl
index 442c8ff..4a6daa4 100644
--- a/Inc/SimpleMath.inl
+++ b/Inc/SimpleMath.inl
@@ -2360,6 +2360,20 @@ inline float Matrix::Determinant() const noexcept
     return XMVectorGetX(XMMatrixDeterminant(M));
 }
 
+inline Vector3 Matrix::ToEuler() const noexcept
+{
+    float cy = sqrtf(_33 * _33 + _31 * _31);
+    float cx = atan2f(-_32, cy);
+    if (cy > 16.f * FLT_EPSILON)
+    {
+        return Vector3(cx, atan2f(_31, _33), atan2f(_12, _22));
+    }
+    else
+    {
+        return Vector3(cx, 0.f, atan2f(-_21, _11));
+    }
+}
+
 //------------------------------------------------------------------------------
 // Static functions
 //------------------------------------------------------------------------------
@@ -2648,6 +2662,14 @@ inline Matrix Matrix::CreateFromYawPitchRoll(float yaw, float pitch, float roll)
     return R;
 }
 
+inline Matrix Matrix::CreateFromYawPitchRoll(const Vector3& angles) noexcept
+{
+    using namespace DirectX;
+    Matrix R;
+    XMStoreFloat4x4(&R, XMMatrixRotationRollPitchYawFromVector(angles));
+    return R;
+}
+
 inline Matrix Matrix::CreateShadow(const Vector3& lightDir, const Plane& plane) noexcept
 {
     using namespace DirectX;
@@ -3093,6 +3115,34 @@ inline float Quaternion::Dot(const Quaternion& q) const noexcept
     return XMVectorGetX(XMQuaternionDot(q1, q2));
 }
 
+inline Vector3 Quaternion::ToEuler() const noexcept
+{
+    float xx = x * x;
+    float yy = y * y;
+    float zz = z * z;
+
+    float m31 = 2.f * x * z + 2.f * y * w;
+    float m32 = 2.f * y * z - 2.f * x * w;
+    float m33 = 1.f - 2.f * xx - 2.f * yy;
+
+    float cy = sqrtf(m33 * m33 + m31 * m31);
+    float cx = atan2f(-m32, cy);
+    if (cy > 16.f * FLT_EPSILON)
+    {
+        float m12 = 2.f * x * y + 2.f * z * w;
+        float m22 = 1.f - 2.f * xx - 2.f * zz;
+
+        return Vector3(cx, atan2f(m31, m33), atan2f(m12, m22));
+    }
+    else
+    {
+        float m11 = 1.f - 2.f * yy - 2.f * zz;
+        float m21 = 2.f * x * y - 2.f * z * w;
+
+        return Vector3(cx, 0.f, atan2f(-m21, m11));
+    }
+}
+
 //------------------------------------------------------------------------------
 // Static functions
 //------------------------------------------------------------------------------
@@ -3115,6 +3165,14 @@ inline Quaternion Quaternion::CreateFromYawPitchRoll(float yaw, float pitch, flo
     return R;
 }
 
+inline Quaternion Quaternion::CreateFromYawPitchRoll(const Vector3& angles) noexcept
+{
+    using namespace DirectX;
+    Quaternion R;
+    XMStoreFloat4(&R, XMQuaternionRotationRollPitchYawFromVector(angles));
+    return R;
+}
+
 inline Quaternion Quaternion::CreateFromRotationMatrix(const Matrix& M) noexcept
 {
     using namespace DirectX;