From 602cb39101c23d392af571b3ca3f1aaa27b1a9ac Mon Sep 17 00:00:00 2001
From: iwubcode <iwubcode@users.noreply.github.com>
Date: Sun, 17 May 2020 12:41:59 -0500
Subject: [PATCH] Common: Add function to get Euler angles from a Quaternion

---
 Source/Core/Common/Matrix.cpp | 28 ++++++++++++++++++++++++++++
 Source/Core/Common/Matrix.h   |  2 ++
 2 files changed, 30 insertions(+)

diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp
index 9f5ff40cb3..06891edf1e 100644
--- a/Source/Core/Common/Matrix.cpp
+++ b/Source/Core/Common/Matrix.cpp
@@ -4,6 +4,8 @@
 
 #include "Common/Matrix.h"
 
+#include "Common/MathUtil.h"
+
 #include <algorithm>
 #include <cmath>
 
@@ -121,6 +123,32 @@ Vec3 operator*(const Quaternion& lhs, const Vec3& rhs)
   return Vec3(result.data.x, result.data.y, result.data.z);
 }
 
+Vec3 FromQuaternionToEuler(const Quaternion& q)
+{
+  Vec3 result;
+
+  const float qx = q.data.x;
+  const float qy = q.data.y;
+  const float qz = q.data.z;
+  const float qw = q.data.w;
+
+  const float sinr_cosp = 2 * (qw * qx + qy * qz);
+  const float cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
+  result.x = std::atan2(sinr_cosp, cosr_cosp);
+
+  const float sinp = 2 * (qw * qy - qz * qx);
+  if (std::abs(sinp) >= 1)
+    result.y = std::copysign(MathUtil::PI / 2, sinp);  // use 90 degrees if out of range
+  else
+    result.y = std::asin(sinp);
+
+  const float siny_cosp = 2 * (qw * qz + qx * qy);
+  const float cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
+  result.z = std::atan2(siny_cosp, cosy_cosp);
+
+  return result;
+}
+
 Matrix33 Matrix33::Identity()
 {
   Matrix33 mtx = {};
diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h
index a1c9f088c6..c3341a9bba 100644
--- a/Source/Core/Common/Matrix.h
+++ b/Source/Core/Common/Matrix.h
@@ -359,6 +359,8 @@ public:
 Quaternion operator*(Quaternion lhs, const Quaternion& rhs);
 Vec3 operator*(const Quaternion& lhs, const Vec3& rhs);
 
+Vec3 FromQuaternionToEuler(const Quaternion& q);
+
 class Matrix33
 {
 public: