diff --git a/quaternion.cpp b/quaternion.cpp index 3d125ae3..9bed59d8 100644 --- a/quaternion.cpp +++ b/quaternion.cpp @@ -17,27 +17,64 @@ #include "quaternion.hpp" +#include "maths.hpp" + + //----------------------------------------------------------------------------- using util::quaternion; +using util::matrix4; //----------------------------------------------------------------------------- -template<> const quaternion quaternion::IDENTITY = { 1, 0, 0, 0 }; +template<> const quaternion< float> quaternion< float>::IDENTITY = { 1, 0, 0, 0 }; template<> const quaternion quaternion::IDENTITY = { 1, 0, 0, 0 }; +/////////////////////////////////////////////////////////////////////////////// +template +quaternion::quaternion (T _a, T _b, T _c, T _d): + a (_a), + b (_b), + c (_c), + d (_d) +{ ; } + + //----------------------------------------------------------------------------- template +quaternion::quaternion (T _a): + a (_a), + b (T{}), + c (T{}), + d (T{}) +{ ; } + + +//----------------------------------------------------------------------------- +template +quaternion::quaternion (): + quaternion (T{}, T{}, T{}, T{}) +{ ; } + + +//----------------------------------------------------------------------------- +template +quaternion::quaternion (vector3 v): + quaternion (0, v.x, v.y, v.z) +{ ; } + + +/////////////////////////////////////////////////////////////////////////////// +template quaternion -quaternion::rotation (T radians, vector<3,T> axis) { - radians /= T{2}; - axis.normalise (); +quaternion::rotation (const T radians, const vector<3,T> axis) { + CHECK (axis.is_normalised ()); return { - std::cos (radians), - axis.x * std::sin (radians), - axis.y * std::sin (radians), - axis.z * std::sin (radians) + std::cos (radians / 2), + std::sin (radians / 2) * axis.x, + std::sin (radians / 2) * axis.y, + std::sin (radians / 2) * axis.z }; } @@ -45,11 +82,11 @@ quaternion::rotation (T radians, vector<3,T> axis) { //----------------------------------------------------------------------------- template quaternion -quaternion::rotation (vector<3,T> from, vector<3,T> to) { - auto v = util::cross (from, to); +quaternion::rotation (vector<3,T> src, vector<3,T> dst) { + auto v = util::cross (src, dst); return { - std::acos (dot (from, to)), + std::acos (dot (src, dst)), v.x, v.y, v.z @@ -57,19 +94,197 @@ quaternion::rotation (vector<3,T> from, vector<3,T> to) { } +/////////////////////////////////////////////////////////////////////////////// +template +T +quaternion::norm2 (void) const +{ + return a * a + b * b + c * c + d * d; +} + + +//----------------------------------------------------------------------------- +template +T +quaternion::norm (void) const +{ + return std::sqrt (norm2 ()); +} + + //----------------------------------------------------------------------------- template quaternion -quaternion::operator* (const quaternion &rhs) const { +quaternion::normalised (void) const +{ + return *this / norm (); +} + + +/////////////////////////////////////////////////////////////////////////////// +template +quaternion +quaternion::operator- (void) const +{ + return { -a, -b, -c, -d }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::conjugate (void) const +{ + return { a, -b, -c, -d }; +} + + +/////////////////////////////////////////////////////////////////////////////// +template +quaternion +quaternion::operator+ (const quaternion q) const +{ return { - w * rhs.w - (x * rhs.x + y * rhs.y + z * rhs.z), - w * rhs.x + rhs.w * x + y * rhs.z - z * rhs.y, - w * rhs.y + rhs.w * y + z * rhs.x - x * rhs.z, - w * rhs.z + rhs.w * z + x * rhs.y - y * rhs.x + a + q.a, + b + q.b, + c + q.c, + d + q.d }; } //----------------------------------------------------------------------------- +template +quaternion +quaternion::operator- (const quaternion q) const +{ + return { + a - q.a, + b - q.b, + c - q.c, + d - q.d + }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::operator* (const quaternion q) const { + return { + a * q.a - b * q.b - c * q.c - d * q.d, + a * q.b + b * q.a + c * q.d - d * q.c, + a * q.c - b * q.d + c * q.a + d * q.b, + a * q.d + b * q.c - c * q.b + d * q.a, + }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::operator/ (const quaternion q) const +{ + auto n = q.norm2 (); + + return { + ( a * q.a + b * q.b + c * q.c + d * q.d) / n, + (- a * q.b + b * q.a + c * q.d - d * q.c) / n, + (- a * q.c - b * q.d + c * q.a + d * q.b) / n, + (- a * q.d + b * q.c - c * q.b + d * q.a) / n + }; +} + +/////////////////////////////////////////////////////////////////////////////// +template +quaternion +quaternion::operator+ (const T t) const +{ + return { a + t, b, c, d }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::operator- (const T t) const +{ + return { a - t, b, c, d }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::operator* (const T t) const +{ + return { a * t, b, c, d }; +} + + +//----------------------------------------------------------------------------- +template +quaternion +quaternion::operator/ (const T t) const +{ + return { a / t, b, c, d }; +} + + +/////////////////////////////////////////////////////////////////////////////// +template +bool +quaternion::operator== (const quaternion rhs) const +{ + return almost_equal (a, rhs.a) && + almost_equal (b, rhs.b) && + almost_equal (c, rhs.c) && + almost_equal (d, rhs.d); +} + + +//----------------------------------------------------------------------------- +template +bool +quaternion::operator!= (const quaternion rhs) const +{ + return !(*this == rhs); +} + + +/////////////////////////////////////////////////////////////////////////////// +template +matrix4 +quaternion::rotation_matrix (void) const +{ + CHECK_EQ (1, norm ()); + + const T wx = w * x, wy = w * y, wz = w * z; + const T xx = x * x, xy = x * y, xz = x * z; + const T yy = y * y, yz = y * z, zz = z * z; + + return { { + { 1 - 2 * (yy - zz), 2 * (xy - wz), 2 * (xz + wy), 0 }, + { 2 * (xy + wz), 1 - 2 * (xx - zz), 2 * (yz - wx), 0 }, + { 2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx - yy), 0 }, + { 0, 0, 0, 1 } + } }; +} + + +/////////////////////////////////////////////////////////////////////////////// +template +std::ostream& +util::operator<< (std::ostream &os, quaternion q) +{ + os << q.w << ' ' << q.x << "i " << q.y << "j " << q.z << 'k'; + return os; +} + + +/////////////////////////////////////////////////////////////////////////////// template struct util::quaternion; template struct util::quaternion; + +template std::ostream& util::operator<< (std::ostream&, quaternion); +template std::ostream& util::operator<< (std::ostream&, quaternion); diff --git a/quaternion.hpp b/quaternion.hpp index 20104268..33e45303 100644 --- a/quaternion.hpp +++ b/quaternion.hpp @@ -18,19 +18,55 @@ #define __UTIL_QUATERNION_HPP #include "vector.hpp" +#include "matrix.hpp" + +#include + namespace util { template struct quaternion { - T w, x, y, z; + union { + struct { T w, x, y, z; }; + struct { T a, b, c, d; }; + T data[4]; + }; static const quaternion IDENTITY; static quaternion rotation (T radians, vector<3,T> axis); - static quaternion rotation (vector<3,T> from, vector<3,T> to); + static quaternion rotation (vector<3,T> src, vector<3,T> dst); - quaternion operator* (const quaternion&) const; + quaternion (T a, T b, T c, T d); + quaternion (T a); + quaternion (); + quaternion (vector<3,T>); + + T norm (void) const; + T norm2 (void) const; + quaternion normalised (void) const; + + quaternion operator- (void) const; + quaternion conjugate (void) const; + + quaternion operator+ (const quaternion) const; + quaternion operator- (const quaternion) const; + quaternion operator* (const quaternion) const; + quaternion operator/ (const quaternion) const; + + quaternion operator+ (const T) const; + quaternion operator- (const T) const; + quaternion operator* (const T) const; + quaternion operator/ (const T) const; + + bool operator== (const quaternion) const; + bool operator!= (const quaternion) const; + + matrix4 rotation_matrix (void) const; }; + + template + std::ostream& operator<< (std::ostream&, quaternion); }