quaternion: update with more operators
This commit is contained in:
parent
8840bae7b0
commit
fdc801628b
245
quaternion.cpp
245
quaternion.cpp
@ -17,8 +17,12 @@
|
|||||||
|
|
||||||
#include "quaternion.hpp"
|
#include "quaternion.hpp"
|
||||||
|
|
||||||
|
#include "maths.hpp"
|
||||||
|
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
//-----------------------------------------------------------------------------
|
||||||
using util::quaternion;
|
using util::quaternion;
|
||||||
|
using util::matrix4;
|
||||||
|
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
//-----------------------------------------------------------------------------
|
||||||
@ -26,18 +30,51 @@ 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>);
|
||||||
|
@ -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>);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user