math: add methods to convert from a quaternion to Euler angles and from

Euler angles to a rotation matrix. Also fix quat::rotate() which was not
building the correct quaternion.

src/lol/math/vector.h

@@ -489,6 +489,8 @@ 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);
+
     DECLARE_MEMBER_OPS(Vec3)
 
 #if !defined __ANDROID__
@@ -1565,6 +1567,8 @@ template <typename T> struct Mat3
     static Mat3<T> rotate(T angle, T x, T y, T z);
     static Mat3<T> rotate(T angle, Vec3<T> v);
     static Mat3<T> rotate(Quat<T> q);
+    static Mat3<T> fromeuler(T x, T y, T z);
+    static Mat3<T> fromeuler(Vec3<T> const &v);
 
     static inline Mat3<T> rotate(Mat3<T> mat, T angle, Vec3<T> v)
     {
@@ -1706,6 +1710,16 @@ template <typename T> struct Mat4
         return rotate(angle, v) * mat;
     }
 
+    static inline Mat4<T> fromeuler(T x, T y, T z)
+    {
+        return Mat4<T>(Mat3<T>::fromeuler(x, y, z), (T)1);
+    }
+
+    static inline Mat4<T> fromeuler(Vec3<T> const &v)
+    {
+        return Mat4<T>(Mat3<T>::fromeuler(v), (T)1);
+    }
+
     /* Helpers for view matrices */
     static Mat4<T> lookat(Vec3<T> eye, Vec3<T> center, Vec3<T> up);
 

src/math/vector.cpp

@@ -462,7 +462,7 @@ template<> quat quat::rotate(float angle, vec3 const &v)
 
     vec3 tmp = normalize(v) * std::sin(angle);
 
-    return quat(tmp.x, tmp.y, tmp.z, std::cos(angle));
+    return quat(std::cos(angle), tmp.x, tmp.y, tmp.z);
 }
 
 template<> quat quat::rotate(float angle, float x, float y, float z)
@@ -470,6 +470,57 @@ template<> quat quat::rotate(float angle, float x, float y, float z)
     return quat::rotate(angle, vec3(x, y, z));
 }
 
+template<> vec3 vec3::toeuler(quat const &q)
+{
+    using std::atan2;
+    using std::asin;
+
+    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)));
+
+    return (180.0f / M_PI / n) * ret;
+}
+
+template<> mat3 mat3::fromeuler(vec3 const &v)
+{
+    using std::sin;
+    using std::cos;
+
+    mat3 ret;
+
+    vec3 radians = (M_PI / 180.0f) * v;
+    float sx = sin(radians.x), cx = cos(radians.x);
+    float sy = sin(radians.y), cy = cos(radians.y);
+    float sz = sin(radians.z), cz = cos(radians.z);
+
+    ret[0][0] =   cy * cz;
+    ret[1][0] =   cx * sz - sx * sy * sz;
+    ret[2][0] =   sx * sz - cx * sy * cz;
+
+    ret[0][1] = - cy * sz;
+    ret[1][1] =   cx * cz - sx * sy * sz;
+    ret[2][1] =   sx * cz + cx * sy * sz;
+
+    ret[0][2] =   sy;
+    ret[1][2] = - sx * cy;
+    ret[2][2] =   cx * cy;
+
+    return ret;
+}
+
+template<> mat3 mat3::fromeuler(float x, float y, float z)
+{
+    return mat3::fromeuler(vec3(x, y, z));
+}
+
 template<> quat quat::fromeuler(vec3 const &v)
 {
     using std::sin;