392 lines
11 KiB
C++
392 lines
11 KiB
C++
/*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
|
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Copyright 2011-2016 Danny Robson <danny@nerdcruft.net>
|
|
*/
|
|
|
|
|
|
#include "quaternion.hpp"
|
|
|
|
#include "debug/assert.hpp"
|
|
#include "vector.hpp"
|
|
#include "coord/ops.hpp"
|
|
|
|
#include <ostream>
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
using cruft::quaternion;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T>
|
|
quaternion<T>::angle_axis (const T radians, const vector<3,T> axis)
|
|
{
|
|
CHECK (radians < cruft::pi<T> * 2);
|
|
CHECK (radians > -cruft::pi<T> * 2);
|
|
CHECK (is_normalised (axis));
|
|
|
|
auto w = std::cos (radians / 2);
|
|
auto xyz = std::sin (radians / 2) * axis;
|
|
|
|
return {
|
|
w, xyz.x, xyz.y, xyz.z
|
|
};
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
quaternion<T>
|
|
quaternion<T>::from_euler (vector<3,T> angles)
|
|
{
|
|
CHECK (all (angles > -cruft::pi<T> * 2));
|
|
CHECK (all (angles < cruft::pi<T> * 2));
|
|
auto half = angles / 2;
|
|
|
|
auto c = std::cos (half);
|
|
auto s = std::sin (half);
|
|
|
|
return {
|
|
c.x * c.y * c.z - s.x * s.y * s.z,
|
|
s.x * c.y * c.z + c.x * s.y * s.z,
|
|
c.x * s.y * c.z - s.x * c.y * s.z,
|
|
c.x * c.y * s.z + s.x * s.y * c.z,
|
|
};
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// vector-to-vector rotation algorithm from:
|
|
// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
|
|
template <typename T>
|
|
quaternion<T>
|
|
quaternion<T>::from_to (const vector<3,T> u, const vector<3,T> v)
|
|
{
|
|
CHECK (is_normalised (u));
|
|
CHECK (is_normalised (v));
|
|
|
|
#if 0
|
|
// Naive:
|
|
auto cos_theta = dot (u, v);
|
|
auto angle = std::acos (cos_theta);
|
|
auto axis = normalised (cross (u, v));
|
|
return angle_axis (angle, axis);
|
|
#elif 1
|
|
auto norm_u_norm_v = std::sqrt(dot(u, u) * dot(v, v));
|
|
auto real_part = norm_u_norm_v + dot(u, v);
|
|
cruft::vector<3,T> w;
|
|
|
|
if (real_part < 1.e-6f * norm_u_norm_v)
|
|
{
|
|
/* If u and v are exactly opposite, rotate 180 degrees
|
|
* around an arbitrary orthogonal axis. Axis normalisation
|
|
* can happen later, when we normalise the quaternion. */
|
|
real_part = 0.0f;
|
|
w = std::abs(u.x) > std::abs(u.z) ?
|
|
cruft::vector3<T> (-u.y, u.x, 0.f) :
|
|
cruft::vector3<T> (0.f, -u.z, u.y);
|
|
}
|
|
else
|
|
{
|
|
/* Otherwise, build quaternion the standard way. */
|
|
w = cross(u, v);
|
|
}
|
|
|
|
return normalised (cruft::quaternion<T> {real_part, w.x, w.y, w.z});
|
|
#endif
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T>
|
|
quaternion<T>::operator- () const noexcept
|
|
{
|
|
return { .w = -w, .x = -x, .y = -y, .z = -z };
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::slerp (quaternion<T> a, quaternion<T> b, T t)
|
|
{
|
|
// calc cosine theta
|
|
T cosine = dot (a, b);
|
|
|
|
// adjust signs (if necessary)
|
|
if (cosine < 0) {
|
|
cosine = -cosine;
|
|
b = -b;
|
|
}
|
|
|
|
// Calculate coefficients
|
|
T p, q;
|
|
if (T(0.999999999f) > cosine) {
|
|
T omega = std::acos (cosine); // extract theta from dot product's cos theta
|
|
T sine = std::sin (omega);
|
|
|
|
p = std::sin ((1 - t) * omega) / sine;
|
|
q = std::sin ( t * omega) / sine;
|
|
} else {
|
|
// Very close, do linear interp (because it's faster)
|
|
p = 1 - t;
|
|
q = t;
|
|
}
|
|
|
|
return a * p + b * q;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::nlerp (cruft::quaternion<T> a, cruft::quaternion<T> b, T t)
|
|
{
|
|
return cruft::normalised (
|
|
cruft::mix (a, b, t)
|
|
);
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::conjugate (quaternion<T> q)
|
|
{
|
|
return { q.w, -q.x, -q.y, -q.z };
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
T
|
|
cruft::dot (quaternion<T> a, quaternion<T> b)
|
|
{
|
|
return a.w * b.w +
|
|
a.x * b.x +
|
|
a.y * b.y +
|
|
a.z * b.z;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T> cruft::operator* (quaternion<T> a, T b)
|
|
{
|
|
return {
|
|
.w = a.w * b,
|
|
.x = a.x * b,
|
|
.y = a.y * b,
|
|
.z = a.z * b
|
|
};
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T> cruft::operator+ (quaternion<T> a, quaternion<T> b)
|
|
{
|
|
return {
|
|
.w = a.w + b.w,
|
|
.x = a.x + b.x,
|
|
.y = a.y + b.y,
|
|
.z = a.z + b.z
|
|
};
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::operator- (quaternion<T> a, quaternion<T> b)
|
|
{
|
|
return {
|
|
.w = a.w - b.w,
|
|
.x = a.x - b.x,
|
|
.y = a.y - b.y,
|
|
.z = a.z - b.z
|
|
};
|
|
}
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::operator* (const quaternion<T> a, const quaternion<T> b)
|
|
{
|
|
return {
|
|
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
|
|
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
|
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
|
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
|
|
};
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
quaternion<T>&
|
|
cruft::operator*= (quaternion<T> &a, const quaternion<T> b)
|
|
{
|
|
return a = a * b;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template <typename T>
|
|
quaternion<T>
|
|
cruft::operator/ (const quaternion<T> a, const quaternion<T> b)
|
|
{
|
|
CHECK (is_normalised (a));
|
|
CHECK (is_normalised (b));
|
|
|
|
return quaternion<T> {
|
|
a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z,
|
|
- a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
|
- a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
|
- a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
|
|
};
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
cruft::matrix4<T>
|
|
quaternion<T>::as_matrix (void) const
|
|
{
|
|
CHECK (is_normalised (*this));
|
|
|
|
const T wx = this->w * this->x, wy = this->w * this->y, wz = this->w * this->z;
|
|
const T xx = this->x * this->x, xy = this->x * this->y, xz = this->x * this->z;
|
|
const T yy = this->y * this->y, yz = this->y * this->z, zz = this->z * this->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 }
|
|
} };
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
|
|
template <typename T>
|
|
cruft::vector3<T>
|
|
cruft::rotate (vector3<T> v, quaternion<T> q)
|
|
{
|
|
CHECK (is_normalised (v));
|
|
CHECK (is_normalised (q));
|
|
|
|
#if 0
|
|
// Naive:
|
|
quaternion<T> p { 0, v.x, v.y, v.z };
|
|
auto p_ = q * p * conjugate (q);
|
|
return { p_.x, p_.y, p_.z };
|
|
#elif 1
|
|
// This code actually matches the stackexchange link, but is longer than
|
|
// the code below it (which also actually works)...
|
|
const cruft::vector3<T> u { q.x, q.y, q.z };
|
|
const auto s = q.w;
|
|
|
|
return 2 * dot (u, v) * u
|
|
+ (s * s - dot (u, u)) * v
|
|
+ 2 * s * cross (u, v);
|
|
#elif 0
|
|
// I have no idea where this code is from or how it was derived...
|
|
cruft::vector3<T> u { q.x, q.y, q.z };
|
|
return v + 2 * cross (u, cross (u, v) + q.w * v);
|
|
#endif
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// based on the implementation at:
|
|
// http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-17-quaternions/
|
|
template <typename T>
|
|
quaternion<T>
|
|
quaternion<T>::look (vector<3,T> fwd, vector<3,T> up)
|
|
{
|
|
CHECK (is_normalised (fwd));
|
|
CHECK (is_normalised (up));
|
|
|
|
constexpr cruft::vector3<T> FWD { 0, 0, -1 };
|
|
constexpr cruft::vector3<T> UP { 0, 1, 0 };
|
|
|
|
// find the rotation from the world fwd to the object fwd
|
|
auto q1 = from_to (FWD, fwd);
|
|
|
|
// orthogonalise the up vector
|
|
auto right = cross (fwd, up);
|
|
auto orthup = normalised (cross (right, fwd));
|
|
|
|
// recompute the up vector in object space
|
|
auto newup = rotate (UP, q1);
|
|
|
|
// find rotation from object up to world up
|
|
auto q2 = from_to (newup, orthup);
|
|
|
|
return q2 * q1;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
bool
|
|
cruft::almost_equal (quaternion<T> a, quaternion<T> b)
|
|
{
|
|
return almost_equal (a.w, b.w) &&
|
|
almost_equal (a.x, b.x) &&
|
|
almost_equal (a.y, b.y) &&
|
|
almost_equal (a.z, b.z);
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename T>
|
|
std::ostream&
|
|
cruft::operator<< (std::ostream &os, const quaternion<T> q)
|
|
{
|
|
return os << "[" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << "]";
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <typename ValueT>
|
|
struct cruft::debug::validator<quaternion<ValueT>> {
|
|
static constexpr
|
|
bool
|
|
is_valid (const quaternion<ValueT> &q)
|
|
{
|
|
return is_normalised (q);
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
template struct cruft::debug::validator<::cruft::quaternion<float>>;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
#define INSTANTIATE(T) \
|
|
template cruft::vector3<T> cruft::rotate (cruft::vector3<T>, cruft::quaternion<T>); \
|
|
template quaternion<T> cruft::slerp (quaternion<T>, quaternion<T>, T); \
|
|
template quaternion<T> cruft::nlerp (quaternion<T>, quaternion<T>, T); \
|
|
template quaternion<T> cruft::conjugate (quaternion<T>); \
|
|
template quaternion<T> cruft::operator* (quaternion<T>, quaternion<T>); \
|
|
template quaternion<T>& cruft::operator*= (quaternion<T>&, quaternion<T>); \
|
|
template quaternion<T> cruft::operator/ (quaternion<T>, quaternion<T>); \
|
|
template bool cruft::almost_equal (cruft::quaternion<T>, cruft::quaternion<T>); \
|
|
template std::ostream& cruft::operator<< (std::ostream&, quaternion<T>); \
|
|
template bool cruft::debug::is_valid(const quaternion<T>&); \
|
|
template struct cruft::quaternion<T>;
|
|
|
|
INSTANTIATE(float)
|
|
INSTANTIATE(double)
|