From 6adf5538069ea58674b0fbbcdf70bf59b9b06329 Mon Sep 17 00:00:00 2001
From: Sam Hocevar <sam@hocevar.net>
Date: Sat, 14 Sep 2013 12:08:11 +0000
Subject: [PATCH] math: implement vec3::toeuler_* for Tait-Bryan angles.

---
 src/camera.cpp        |  2 +-
 src/lol/math/vector.h | 14 +++++++++++++-
 src/math/vector.cpp   | 41 +++++++++++++++++++++++++++++++++++------
 3 files changed, 49 insertions(+), 8 deletions(-)

diff --git a/src/camera.cpp b/src/camera.cpp
index 111a1d4c..425b93e5 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -210,7 +210,7 @@ vec3 Camera::GetUp()
 
 vec3 Camera::GetRotationEuler()
 {
-    return vec3::toeuler(GetRotation());
+    return vec3::toeuler_zyx(GetRotation());
 }
 
 quat Camera::GetRotation()
diff --git a/src/lol/math/vector.h b/src/lol/math/vector.h
index a06d6bc9..7026fece 100644
--- a/src/lol/math/vector.h
+++ b/src/lol/math/vector.h
@@ -503,7 +503,19 @@ template <typename T> struct Vec3 : BVec3<T>
     explicit inline Vec3(XVec3<U, N> const &v)
       : BVec3<T>(v[0], v[1], v[2]) {}
 
-    static Vec3<T> toeuler(Quat<T> const &q);
+    static Vec3<T> toeuler_xyx(Quat<T> const &q);
+    static Vec3<T> toeuler_xzx(Quat<T> const &q);
+    static Vec3<T> toeuler_yxy(Quat<T> const &q);
+    static Vec3<T> toeuler_yzy(Quat<T> const &q);
+    static Vec3<T> toeuler_zxz(Quat<T> const &q);
+    static Vec3<T> toeuler_zyz(Quat<T> const &q);
+
+    static Vec3<T> toeuler_xyz(Quat<T> const &q);
+    static Vec3<T> toeuler_xzy(Quat<T> const &q);
+    static Vec3<T> toeuler_yxz(Quat<T> const &q);
+    static Vec3<T> toeuler_yzx(Quat<T> const &q);
+    static Vec3<T> toeuler_zxy(Quat<T> const &q);
+    static Vec3<T> toeuler_zyx(Quat<T> const &q);
 
     LOL_MEMBER_OPS(Vec3, x)
 
diff --git a/src/math/vector.cpp b/src/math/vector.cpp
index 037484bb..5f079454 100644
--- a/src/math/vector.cpp
+++ b/src/math/vector.cpp
@@ -530,18 +530,38 @@ template<> quat slerp(quat const &qa, quat const &qb, float f)
     return qa * (s0 * d) + qb * (s1 * d);
 }
 
-template<> vec3 vec3::toeuler(quat const &q)
+static inline vec3 quat_toeuler_generic(quat const &q, int i, int j, int k)
 {
     float n = norm(q);
 
     if (!n)
         return vec3(0.f);
 
-    vec3 ret(atan2(2.f * (q.w * q.x + q.y * q.z),
-                   1.f - 2.f * (q.x * q.x + q.y * q.y)),
-             asin(2.f * (q.w * q.y - q.z * q.x)),
-             atan2(2.f * (q.w * q.z + q.y * q.x),
-                   1.f - 2.f * (q.z * q.z + q.y * q.y)));
+    vec3 v(q.x, q.y, q.z), ret;
+
+    if (i != k)
+    {
+        if ((2 + i - j) % 3)
+        {
+            ret[0] = atan2(2.f * (q.w * v[i] - v[j] * v[k]),
+                           1.f - 2.f * (v[i] * v[i] + v[j] * v[j]));
+            ret[1] = asin(2.f * (q.w * v[j] + v[i] * v[k]));
+            ret[2] = atan2(2.f * (q.w * v[k] - v[j] * v[i]),
+                           1.f - 2.f * (v[k] * v[k] + v[j] * v[j]));
+        }
+        else
+        {
+            ret[0] = atan2(2.f * (q.w * v[i] + v[j] * v[k]),
+                           1.f - 2.f * (v[i] * v[i] + v[j] * v[j]));
+            ret[1] = asin(2.f * (q.w * v[j] - v[i] * v[k]));
+            ret[2] = atan2(2.f * (q.w * v[k] + v[j] * v[i]),
+                           1.f - 2.f * (v[k] * v[k] + v[j] * v[j]));
+        }
+    }
+    else
+    {
+        /* FIXME: TODO */
+    }
 
     return (180.0f / F_PI / n) * ret;
 }
@@ -653,6 +673,7 @@ static inline quat quat_fromeuler_generic(vec3 const &v, int i, int j, int k)
 }
 
 #define DEFINE_FROMEULER_GENERIC(name, i, j, k) \
+    /* Create quaternions from Euler angles */ \
     template<> quat quat::fromeuler_##name(vec3 const &v) \
     { \
         return quat_fromeuler_generic(v, i, j, k); \
@@ -663,6 +684,7 @@ static inline quat quat_fromeuler_generic(vec3 const &v, int i, int j, int k)
         return quat::fromeuler_##name(vec3(phi, theta, psi)); \
     } \
     \
+    /* Create 3×3 matrices from Euler angles */ \
     template<> mat3 mat3::fromeuler_##name(vec3 const &v) \
     { \
         return mat3_fromeuler_generic(v, i, j, k); \
@@ -673,6 +695,7 @@ static inline quat quat_fromeuler_generic(vec3 const &v, int i, int j, int k)
         return mat3::fromeuler_##name(vec3(phi, theta, psi)); \
     } \
     \
+    /* Create 4×4 matrices from Euler angles */ \
     template<> mat4 mat4::fromeuler_##name(vec3 const &v) \
     { \
         return mat4(mat3_fromeuler_generic(v, i, j, k), 1.f); \
@@ -681,6 +704,12 @@ static inline quat quat_fromeuler_generic(vec3 const &v, int i, int j, int k)
     template<> mat4 mat4::fromeuler_##name(float phi, float theta, float psi) \
     { \
         return mat4::fromeuler_##name(vec3(phi, theta, psi)); \
+    } \
+    \
+    /* Retrieve Euler angles from a quaternion */ \
+    template<> vec3 vec3::toeuler_##name(quat const &q) \
+    { \
+        return quat_toeuler_generic(q, i, j, k); \
     }
 
 DEFINE_FROMEULER_GENERIC(xyx, 0, 1, 0)