quaternion: update with more operators

This commit is contained in:
Danny Robson 2015-07-13 16:27:35 +10:00
parent 8840bae7b0
commit fdc801628b
2 changed files with 270 additions and 19 deletions

View File

@ -17,27 +17,64 @@
#include "quaternion.hpp" #include "quaternion.hpp"
#include "maths.hpp"
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
using util::quaternion; 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<> 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> 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>
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 { 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> template <typename T>
quaternion<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 { return {
std::acos (dot (from, to)), std::acos (dot (src, dst)),
v.x, v.x,
v.y, v.y,
v.z 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> template <typename T>
quaternion<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 { 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<float>;
template struct util::quaternion<double>; template struct util::quaternion<double>;
template std::ostream& util::operator<< (std::ostream&, quaternion<float>);
template std::ostream& util::operator<< (std::ostream&, quaternion<double>);

View File

@ -18,19 +18,55 @@
#define __UTIL_QUATERNION_HPP #define __UTIL_QUATERNION_HPP
#include "vector.hpp" #include "vector.hpp"
#include "matrix.hpp"
#include <iostream>
namespace util { namespace util {
template <typename T> template <typename T>
struct quaternion { 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 const quaternion IDENTITY;
static quaternion rotation (T radians, vector<3,T> axis); 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>);
} }