From f0cb6169aeddc626352ff49d05fcdebece41a8f5 Mon Sep 17 00:00:00 2001
From: parax0 <parax0@github.com>
Date: Thu, 20 Aug 2015 22:18:13 -0400
Subject: [PATCH] Switched CQuaternion from XYZW to WXYZ, added new conversion
 functions (Quats->Eulers, Degs<->Rads)

---
 Common/CQuaternion.cpp | 40 +++++++++++++++++++++++++++-------------
 Common/CQuaternion.h   |  5 +++--
 Common/Math.cpp        | 10 ++++++++++
 Common/Math.h          |  8 ++++++++
 4 files changed, 48 insertions(+), 15 deletions(-)

diff --git a/Common/CQuaternion.cpp b/Common/CQuaternion.cpp
index 4fbdd369..ab1b14eb 100644
--- a/Common/CQuaternion.cpp
+++ b/Common/CQuaternion.cpp
@@ -1,21 +1,22 @@
-    #include "CQuaternion.h"
+#include "CQuaternion.h"
 #include <cmath>
 #include <math.h>
+#include <Common/Math.h>
 
 CQuaternion::CQuaternion()
 {
-    x = 0;
-    y = 0;
-    z = 0;
-    w = 0;
+    w = 0.f;
+    x = 0.f;
+    y = 0.f;
+    z = 0.f;
 }
 
-CQuaternion::CQuaternion(float _x, float _y, float _z, float _w)
+CQuaternion::CQuaternion(float _w, float _x, float _y, float _z)
 {
+    w = _w;
     x = _x;
     y = _y;
     z = _z;
-    w = _w;
 }
 
 CVector3f CQuaternion::XAxis()
@@ -40,12 +41,25 @@ CQuaternion CQuaternion::Inverse()
     if (fNorm > 0.f)
     {
         float fInvNorm = 1.f / fNorm;
-        return CQuaternion(-x * fInvNorm, -y * fInvNorm, -z * fInvNorm, w * fInvNorm);
+        return CQuaternion( w * fInvNorm, -x * fInvNorm, -y * fInvNorm, -z * fInvNorm);
     }
     else
         return CQuaternion::skZero;
 }
 
+CVector3f CQuaternion::ToEuler()
+{
+    // There is more than one way to do this conversion, based on rotation order.
+    // But since we only care about the rotation order used in Retro games, which is consistent,
+    // we can just have a single conversion function. Handy!
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+    float ex = atan2f(2 * (w*x + y*z), 1 - (2 * (Math::Pow(x,2) + Math::Pow(y,2))));
+    float ey = asinf(2 * (w*y - z*x));
+    float ez = atan2f(2 * (w*z + x*y), 1 - (2 * (Math::Pow(y,2) + Math::Pow(z,2))));
+    return CVector3f(Math::RadiansToDegrees(ex), Math::RadiansToDegrees(ey), Math::RadiansToDegrees(ez));
+}
+
 // ************ OPERATORS ************
 CVector3f CQuaternion::operator*(const CVector3f& vec) const
 {
@@ -62,10 +76,10 @@ CVector3f CQuaternion::operator*(const CVector3f& vec) const
 CQuaternion CQuaternion::operator*(const CQuaternion& other) const
 {
     CQuaternion out;
+    out.w = (-x * other.x) - (y * other.y) - (z * other.z) + (w * other.w);
     out.x = ( x * other.w) + (y * other.z) - (z * other.y) + (w * other.x);
     out.y = (-x * other.z) + (y * other.w) + (z * other.x) + (w * other.y);
     out.z = ( x * other.y) - (y * other.x) + (z * other.w) + (w * other.z);
-    out.w = (-x * other.x) - (y * other.y) - (z * other.z) + (w * other.w);
     return out;
 }
 
@@ -113,16 +127,16 @@ CQuaternion CQuaternion::FromAxisAngle(float angle, CVector3f axis)
 {
     CQuaternion quat;
     axis = axis.Normalized();
-    angle = angle * 3.14159265358979323846f / 180;
+    angle = Math::DegreesToRadians(angle);
 
-    float sa = sin(angle / 2);
+    float sa = sinf(angle / 2);
+    quat.w = cosf(angle / 2);
     quat.x = axis.x * sa;
     quat.y = axis.y * sa;
     quat.z = axis.z * sa;
-    quat.w = cos(angle / 2);
     return quat;
 
 }
 
-CQuaternion CQuaternion::skIdentity = CQuaternion(0.f, 0.f, 0.f, 1.f);
+CQuaternion CQuaternion::skIdentity = CQuaternion(1.f, 0.f, 0.f, 0.f);
 CQuaternion CQuaternion::skZero = CQuaternion(0.f, 0.f, 0.f, 0.f);
diff --git a/Common/CQuaternion.h b/Common/CQuaternion.h
index c6da1cc5..11240338 100644
--- a/Common/CQuaternion.h
+++ b/Common/CQuaternion.h
@@ -6,15 +6,16 @@
 class CQuaternion
 {
 public:
-    float x, y, z, w;
+    float w, x, y, z;
 
     CQuaternion();
-    CQuaternion(float _x, float _y, float _z, float _w);
+    CQuaternion(float _w, float _x, float _y, float _z);
 
     CVector3f XAxis();
     CVector3f YAxis();
     CVector3f ZAxis();
     CQuaternion Inverse();
+    CVector3f ToEuler();
 
     // Operators
     CVector3f operator*(const CVector3f& vec) const;
diff --git a/Common/Math.cpp b/Common/Math.cpp
index 3684d9f5..6c80a2b7 100644
--- a/Common/Math.cpp
+++ b/Common/Math.cpp
@@ -20,6 +20,16 @@ float Distance(const CVector3f& A, const CVector3f& B)
                   Pow(B.z - A.z, 2.f) );
 }
 
+float DegreesToRadians(float deg)
+{
+    return deg * skPi / 180.f;
+}
+
+float RadiansToDegrees(float rad)
+{
+    return rad * 180.f / skPi;
+}
+
 std::pair<bool,float> RayPlaneIntersecton(const CRay& ray, const CPlane& plane)
 {
     // Code based on ray/plane intersect code from Ogre
diff --git a/Common/Math.h b/Common/Math.h
index f5d5b955..e5fd8b84 100644
--- a/Common/Math.h
+++ b/Common/Math.h
@@ -17,6 +17,10 @@ float Pow(float Base, float Exponent);
 
 float Distance(const CVector3f& A, const CVector3f& B);
 
+float DegreesToRadians(float deg);
+
+float RadiansToDegrees(float rad);
+
 std::pair<bool,float> RayPlaneIntersecton(const CRay& ray, const CPlane& plane);
 
 std::pair<bool,float> RayBoxIntersection(const CRay& Ray, const CAABox& Box);
@@ -27,6 +31,10 @@ std::pair<bool,float> RayLineIntersection(const CRay& ray, const CVector3f& poin
 std::pair<bool,float> RayTriangleIntersection(const CRay& Ray, const CVector3f& PointA,
                                               const CVector3f& PointB, const CVector3f& PointC,
                                               bool AllowBackfaces = false);
+
+// Constants
+static const float skPi = 3.14159265358979323846f;
+static const float skHalfPi = skPi / 2.f;
 }
 
 #endif // MATH