diff --git a/src/lol/math/vector.h b/src/lol/math/vector.h index 86bb5176..8a7c4f6c 100644 --- a/src/lol/math/vector.h +++ b/src/lol/math/vector.h @@ -489,6 +489,8 @@ template struct Vec3 : BVec3 explicit inline Vec3(XVec3 const &v) : BVec3(v[0], v[1], v[2]) {} + static Vec3 toeuler(Quat const &q); + DECLARE_MEMBER_OPS(Vec3) #if !defined __ANDROID__ @@ -1565,6 +1567,8 @@ template struct Mat3 static Mat3 rotate(T angle, T x, T y, T z); static Mat3 rotate(T angle, Vec3 v); static Mat3 rotate(Quat q); + static Mat3 fromeuler(T x, T y, T z); + static Mat3 fromeuler(Vec3 const &v); static inline Mat3 rotate(Mat3 mat, T angle, Vec3 v) { @@ -1706,6 +1710,16 @@ template struct Mat4 return rotate(angle, v) * mat; } + static inline Mat4 fromeuler(T x, T y, T z) + { + return Mat4(Mat3::fromeuler(x, y, z), (T)1); + } + + static inline Mat4 fromeuler(Vec3 const &v) + { + return Mat4(Mat3::fromeuler(v), (T)1); + } + /* Helpers for view matrices */ static Mat4 lookat(Vec3 eye, Vec3 center, Vec3 up); diff --git a/src/math/vector.cpp b/src/math/vector.cpp index 3a77ffe4..7098d40f 100644 --- a/src/math/vector.cpp +++ b/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;