quaternion: update with more operators

quaternion.cpp

@@ -17,27 +17,64 @@
 
 #include "quaternion.hpp"
 
+#include "maths.hpp"
+
+
 //-----------------------------------------------------------------------------
 using util::quaternion;
+using util::matrix4;
 
 
 //-----------------------------------------------------------------------------
-template<> const quaternion<float> quaternion<float>::IDENTITY = { 1, 0, 0, 0 };
+template<> const quaternion< float> quaternion< float>::IDENTITY = { 1, 0, 0, 0 };
 template<> const quaternion<double> quaternion<double>::IDENTITY = { 1, 0, 0, 0 };
 
 
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+quaternion<T>::quaternion (T _a, T _b, T _c, T _d):
+    a (_a),
+    b (_b),
+    c (_c),
+    d (_d)
+{ ; }
+
+
 //-----------------------------------------------------------------------------
 template <typename T>
+quaternion<T>::quaternion (T _a):
+    a (_a),
+    b (T{}),
+    c (T{}),
+    d (T{})
+{ ; }
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>::quaternion ():
+    quaternion (T{}, T{}, T{}, T{})
+{ ; }
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>::quaternion (vector3<T> v):
+    quaternion (0, v.x, v.y, v.z)
+{ ; }
+
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
 quaternion<T>
-quaternion<T>::rotation (T radians, vector<3,T> axis) {
+quaternion<T>::rotation (const T radians, const vector<3,T> axis) {
-    radians /= T{2};
+    CHECK (axis.is_normalised ());
-    axis.normalise ();
 
     return {
-        std::cos (radians),
+        std::cos (radians / 2),
-        axis.x * std::sin (radians),
+        std::sin (radians / 2) * axis.x,
-        axis.y * std::sin (radians),
+        std::sin (radians / 2) * axis.y,
-        axis.z * std::sin (radians)
+        std::sin (radians / 2) * axis.z
     };
 }
 
@@ -45,11 +82,11 @@ quaternion<T>::rotation (T radians, vector<3,T> axis) {
 //-----------------------------------------------------------------------------
 template <typename T>
 quaternion<T>
-quaternion<T>::rotation (vector<3,T> from, vector<3,T> to) {
+quaternion<T>::rotation (vector<3,T> src, vector<3,T> dst) {
-    auto v = util::cross (from, to);
+    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<T>::rotation (vector<3,T> from, vector<3,T> to) {
 }
 
 
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+T
+quaternion<T>::norm2 (void) const
+{
+    return a * a + b * b + c * c + d * d;
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+T
+quaternion<T>::norm (void) const
+{
+    return std::sqrt (norm2 ());
+}
+
+
 //-----------------------------------------------------------------------------
 template <typename T>
 quaternion<T>
-quaternion<T>::operator* (const quaternion<T> &rhs) const {
+quaternion<T>::normalised (void) const
+{
+    return *this / norm ();
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+quaternion<T>
+quaternion<T>::operator- (void) const
+{
+    return { -a, -b, -c, -d };
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::conjugate (void) const
+{
+    return { a, -b, -c, -d };
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+quaternion<T>
+quaternion<T>::operator+ (const quaternion<T> q) const
+{
     return {
-        w * rhs.w - (x * rhs.x + y * rhs.y + z * rhs.z),
+        a + q.a,
-        w * rhs.x + rhs.w * x + y * rhs.z - z * rhs.y,
+        b + q.b,
-        w * rhs.y + rhs.w * y + z * rhs.x - x * rhs.z,
+        c + q.c,
-        w * rhs.z + rhs.w * z + x * rhs.y - y * rhs.x
+        d + q.d
     };
 }
 
 
 //-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::operator- (const quaternion<T> q) const
+{
+    return {
+        a - q.a,
+        b - q.b,
+        c - q.c,
+        d - q.d
+    };
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::operator* (const quaternion<T> 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 <typename T>
+quaternion<T>
+quaternion<T>::operator/ (const quaternion<T> 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 <typename T>
+quaternion<T>
+quaternion<T>::operator+ (const T t) const
+{
+    return { a + t, b, c, d };
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::operator- (const T t) const
+{
+    return { a - t, b, c, d };
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::operator* (const T t) const
+{
+    return { a * t, b, c, d };
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+quaternion<T>
+quaternion<T>::operator/ (const T t) const
+{
+    return { a / t, b, c, d };
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+bool
+quaternion<T>::operator== (const quaternion<T> rhs) const
+{
+    return almost_equal (a, rhs.a) &&
+           almost_equal (b, rhs.b) &&
+           almost_equal (c, rhs.c) &&
+           almost_equal (d, rhs.d);
+}
+
+
+//-----------------------------------------------------------------------------
+template <typename T>
+bool
+quaternion<T>::operator!= (const quaternion<T> rhs) const
+{
+    return !(*this == rhs);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename T>
+matrix4<T>
+quaternion<T>::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 <typename T>
+std::ostream&
+util::operator<< (std::ostream &os, quaternion<T> q)
+{
+    os << q.w << ' ' << q.x << "i " << q.y << "j " << q.z << 'k';
+    return os;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
 template struct util::quaternion<float>;
 template struct util::quaternion<double>;
+
+template std::ostream& util::operator<< (std::ostream&, quaternion<float>);
+template std::ostream& util::operator<< (std::ostream&, quaternion<double>);

quaternion.hpp

@@ -18,19 +18,55 @@
 #define __UTIL_QUATERNION_HPP
 
 #include "vector.hpp"
+#include "matrix.hpp"
+
+#include <iostream>
+
 
 namespace util {
     template <typename T>
     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<T> rotation_matrix (void) const;
     };
+
+    template <typename T>
+    std::ostream& operator<< (std::ostream&, quaternion<T>);
 }