Common: Add function to get Euler angles from a Quaternion

1 files changed, 28 insertions, 0 deletions

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 = {};