From: John Tsiombikas Date: Sun, 3 Jan 2016 12:29:15 +0000 (+0200) Subject: implemented quaternions X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?p=gph-math;a=commitdiff_plain;h=16f27fa9b94ad0a8a07c4e9a315d1aad2adc4e1a implemented quaternions --- diff --git a/src/quat.cc b/src/quat.cc index 292ab24..595943f 100644 --- a/src/quat.cc +++ b/src/quat.cc @@ -4,6 +4,4 @@ namespace gph { Quaternion Quaternion::identity; - - } // namespace gph diff --git a/src/quat.h b/src/quat.h index d77f70f..a03ae70 100644 --- a/src/quat.h +++ b/src/quat.h @@ -2,6 +2,7 @@ #define QUATERNION_H_ #include "vector.h" +#include "matrix.h" namespace gph { @@ -15,10 +16,16 @@ public: Quaternion(float x_, float y_, float z_, float w_) : x(x_), y(y_), z(z_), w(w_) {} Quaternion(const Vector3 &v, float s) : x(v.x), y(v.y), z(v.z), w(s) {} - void normalize(); - void invert(); + inline void normalize(); + inline void conjugate(); + inline void invert(); - Matrix4x4 calc_matrix() const; + inline void set_rotation(const Vector3 &axis, float angle); + inline void rotate(const Vector3 &axis, float angle); + // rotate by a quaternion rq by doing: rq * *this * conjugate(rq) + inline void rotate(const Quaternion &rq); + + inline Matrix4x4 calc_matrix() const; }; inline Quaternion operator -(const Quaternion &q); @@ -30,12 +37,11 @@ inline Quaternion &operator +=(Quaternion &a, const Quaternion &b); inline Quaternion &operator -=(Quaternion &a, const Quaternion &b); inline Quaternion &operator *=(Quaternion &a, const Quaternion &b); -inline Quaternion conjugate(const Quaternion &q); - inline float length(const Quaternion &q); inline float length_sq(const Quaternion &q); inline Quaternion normalize(const Quaternion &q); +inline Quaternion conjugate(const Quaternion &q); inline Quaternion inverse(const Quaternion &q); Quaternion slerp(const Quaternion &a, const Quaternion &b, float t); diff --git a/src/quat.inl b/src/quat.inl index 5552165..46606d0 100644 --- a/src/quat.inl +++ b/src/quat.inl @@ -15,4 +15,189 @@ inline Quaternion operator -(const Quaternion &a, const Quaternion &b) inline Quaternion operator *(const Quaternion &a, const Quaternion &b) { - float x = a.w * b.w - (a.x * b.x + a.y * b.y + a.z * b.z); + Vector3 a_im = Vector3(a.x, a.y, a.z); + Vector3 b_im = Vector3(b.x, b.y, b.z); + + float w = a.w * b.w - dot(a_im, b_im); + Vector3 im = a.w * b_im + b.w * a_im + cross(a_im, b_im); + return Quaternion(im.x, im.y, im.z, w); +} + +inline Quaternion &operator +=(Quaternion &a, const Quaternion &b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; + return a; +} + +inline Quaternion &operator -=(Quaternion &a, const Quaternion &b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; + return a; +} + +inline Quaternion &operator *=(Quaternion &a, const Quaternion &b) +{ + Vector3 a_im = Vector3(a.x, a.y, a.z); + Vector3 b_im = Vector3(b.x, b.y, b.z); + + float w = a.w * b.w - dot(a_im, b_im); + Vector3 im = a.w * b_im + b.w * a_im + cross(a_im, b_im); + a = Quaternion(im.x, im.y, im.z, w); + return a; +} + +inline float length(const Quaternion &q) +{ + return (float)sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); +} + +inline float length_sq(const Quaternion &q) +{ + return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; +} + +inline void Quaternion::normalize() +{ + float len = length(*this); + if(len != 0.0f) { + x /= len; + y /= len; + z /= len; + w /= len; + } +} + +inline Quaternion normalize(const Quaternion &q) +{ + float len = length(q); + if(len != 0.0f) { + return Quaternion(q.x / len, q.y / len, q.z / len, q.w / len); + } + return q; +} + +inline void Quaternion::conjugate() +{ + x = -x; + y = -y; + z = -z; +} + +inline Quaternion conjugate(const Quaternion &q) +{ + return Quaternion(-q.x, -q.y, -q.z, q.w); +} + +inline void Quaternion::invert() +{ + Quaternion conj = gph::conjugate(*this); + float len_sq = length_sq(conj); + if(len_sq != 0.0) { + x = conj.x / len_sq; + y = conj.y / len_sq; + z = conj.z / len_sq; + w = conj.w / len_sq; + } +} + +inline Quaternion inverse(const Quaternion &q) +{ + Quaternion conj = conjugate(q); + float len_sq = length_sq(conj); + if(len_sq != 0.0) { + return Quaternion(conj.x / len_sq, conj.y / len_sq, conj.z / len_sq, conj.w / len_sq); + } + return q; +} + +inline void Quaternion::set_rotation(const Vector3 &axis, float angle) +{ + float half_angle = angle * 0.5f; + w = cos(half_angle); + float sin_ha = sin(half_angle); + x = axis.x * sin_ha; + y = axis.y * sin_ha; + z = axis.z * sin_ha; +} + +inline void Quaternion::rotate(const Vector3 &axis, float angle) +{ + Quaternion q; + float half_angle = angle * 0.5f; + q.w = cos(half_angle); + float sin_ha = sin(half_angle); + q.x = axis.x * sin_ha; + q.y = axis.y * sin_ha; + q.z = axis.z * sin_ha; + *this *= q; +} + +inline void Quaternion::rotate(const Quaternion &rq) +{ + *this = rq * *this * gph::conjugate(rq); +} + +inline Matrix4x4 Quaternion::calc_matrix() const +{ + float xsq2 = 2.0 * x * x; + float ysq2 = 2.0 * y * y; + float zsq2 = 2.0 * z * z; + float sx = 1.0 - ysq2 - zsq2; + float sy = 1.0 - xsq2 - zsq2; + float sz = 1.0 - xsq2 - ysq2; + + return Matrix4x4( + sx, 2.0 * x * y - 2.0 * w * z, 2.0 * z * x + 2.0 * w * y, 0, + 2.0 * x * y + 2.0 * w * z, sy, 2.0 * y * z - 2.0 * w * x, 0, + 2.0 * z * x - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, sz, 0, + 0, 0, 0, 1); +} + +inline Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, float t) +{ + Quaternion q1 = quat1; + float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; + + if(dot < 0.0) { + /* make sure we interpolate across the shortest arc */ + q1 = -quat1; + dot = -dot; + } + + /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to + * floating point imprecisions + */ + if(dot < -1.0) dot = -1.0; + if(dot > 1.0) dot = 1.0; + + float angle = acos(dot); + float a, b; + + float sin_angle = sin(angle); + if(sin_angle == 0.0f) { + // use linear interpolation to avoid div/zero + a = 1.0f - t; + b = t; + } else { + a = sin((1.0f - t) * angle) / sin_angle; + b = sin(t * angle) / sin_angle; + } + + float x = q1.x * a + q2.x * b; + float y = q1.y * a + q2.y * b; + float z = q1.z * a + q2.z * b; + float w = q1.w * a + q2.w * b; + + return Quaternion(x, y, z, w); +} + +inline Quaternion lerp(const Quaternion &a, const Quaternion &b, float t) +{ + return slerp(a, b, t); +}