596 lines
15 KiB
C++
596 lines
15 KiB
C++
// quaternion.h
|
|
//
|
|
// Copyright (C) 2000, Chris Laurel <claurel@shatters.net>
|
|
//
|
|
// Template-ized quaternion math library.
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License
|
|
// as published by the Free Software Foundation; either version 2
|
|
// of the License, or (at your option) any later version.
|
|
|
|
#ifndef _QUATERNION_H_
|
|
#define _QUATERNION_H_
|
|
|
|
#include "mathlib.h"
|
|
#include "vecmath.h"
|
|
|
|
|
|
template<class T> class Quaternion
|
|
{
|
|
public:
|
|
inline Quaternion();
|
|
inline Quaternion(const Quaternion<T>&);
|
|
inline Quaternion(T);
|
|
inline Quaternion(const Vector3<T>&);
|
|
inline Quaternion(T, const Vector3<T>&);
|
|
inline Quaternion(T, T, T, T);
|
|
|
|
Quaternion(Matrix3<T>&);
|
|
|
|
inline Quaternion& operator+=(Quaternion);
|
|
inline Quaternion& operator-=(Quaternion);
|
|
inline Quaternion& operator*=(T);
|
|
Quaternion& operator*=(Quaternion);
|
|
|
|
inline Quaternion operator~(); // conjugate
|
|
inline Quaternion operator-();
|
|
inline Quaternion operator+();
|
|
|
|
void setAxisAngle(Vector3<T> axis, T angle);
|
|
|
|
void getAxisAngle(Vector3<T>& axis, T& angle) const;
|
|
Matrix4<T> toMatrix4() const;
|
|
Matrix3<T> toMatrix3() const;
|
|
|
|
static Quaternion<T> slerp(Quaternion<T>, Quaternion<T>, T);
|
|
|
|
void rotate(Vector3<T> axis, T angle);
|
|
void xrotate(T angle);
|
|
void yrotate(T angle);
|
|
void zrotate(T angle);
|
|
|
|
bool isPure() const;
|
|
bool isReal() const;
|
|
T normalize();
|
|
|
|
friend Quaternion operator+(Quaternion, Quaternion);
|
|
friend Quaternion operator-(Quaternion, Quaternion);
|
|
friend Quaternion operator*(Quaternion, Quaternion);
|
|
friend Quaternion operator*(T, Quaternion);
|
|
friend Quaternion operator*(Vector3<T>, Quaternion);
|
|
|
|
friend bool operator==(Quaternion, Quaternion);
|
|
friend bool operator!=(Quaternion, Quaternion);
|
|
|
|
friend T real(Quaternion);
|
|
friend Vector3<T> imag(Quaternion);
|
|
|
|
// private:
|
|
T w, x, y, z;
|
|
};
|
|
|
|
|
|
typedef Quaternion<float> Quatf;
|
|
typedef Quaternion<double> Quatd;
|
|
|
|
|
|
template<class T> Quaternion<T>::Quaternion() : w(0), x(0), y(0), z(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Quaternion<T>::Quaternion(const Quaternion<T>& q) :
|
|
w(q.w), x(q.x), y(q.y), z(q.z)
|
|
{
|
|
}
|
|
|
|
template<class T> Quaternion<T>::Quaternion(T re) :
|
|
w(re), x(0), y(0), z(0)
|
|
{
|
|
}
|
|
|
|
// Create a 'pure' quaternion
|
|
template<class T> Quaternion<T>::Quaternion(const Vector3<T>& im) :
|
|
w(0), x(im.x), y(im.y), z(im.z)
|
|
{
|
|
}
|
|
|
|
template<class T> Quaternion<T>::Quaternion(T re, const Vector3<T>& im) :
|
|
w(re), x(im.x), y(im.y), z(im.z)
|
|
{
|
|
}
|
|
|
|
template<class T> Quaternion<T>::Quaternion(T _w, T _x, T _y, T _z) :
|
|
w(_w), x(_x), y(_y), z(_z)
|
|
{
|
|
}
|
|
|
|
// Create a quaternion from a rotation matrix
|
|
template<class T> Quaternion<T>::Quaternion(Matrix3<T>& m)
|
|
{
|
|
T trace = m[0][0] + m[1][1] + m[2][2];
|
|
T root;
|
|
|
|
if (trace > 0)
|
|
{
|
|
root = (T) sqrt(trace + 1);
|
|
w = (T) 0.5 * root;
|
|
root = (T) 0.5 / root;
|
|
x = (m[2][1] - m[1][2]) * root;
|
|
y = (m[0][2] - m[2][0]) * root;
|
|
z = (m[1][0] - m[0][1]) * root;
|
|
}
|
|
else
|
|
{
|
|
int i = 0;
|
|
if (m[1][1] > m[1][1])
|
|
i = 1;
|
|
if (m[2][2] > m[1][1])
|
|
i = 2;
|
|
int j = (i == 2) ? 0 : i + 1;
|
|
int k = (j == 2) ? 0 : j + 1;
|
|
|
|
root = (T) sqrt(m[i][i] - m[j][j] - m[k][k] + 1);
|
|
T* xyz[3] = { &x, &y, &z };
|
|
*xyz[i] = (T) 0.5 * root;
|
|
root = (T) 0.5 / root;
|
|
w = (m[k][j] - m[j][k]) * root;
|
|
*xyz[j] = (m[j][i] + m[i][j]) * root;
|
|
*xyz[k] = (m[k][i] + m[i][k]) * root;
|
|
}
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T>& Quaternion<T>::operator+=(Quaternion<T> a)
|
|
{
|
|
x += a.x; y += a.y; z += a.z; w += a.w;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Quaternion<T>& Quaternion<T>::operator-=(Quaternion<T> a)
|
|
{
|
|
x -= a.x; y -= a.y; z -= a.z; w -= a.w;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Quaternion<T>& Quaternion<T>::operator*=(Quaternion<T> q)
|
|
{
|
|
*this = Quaternion<T>(w * q.w - x * q.x - y * q.y - z * q.z,
|
|
w * q.x + x * q.w + y * q.z - z * q.y,
|
|
w * q.y + y * q.w + z * q.x - x * q.z,
|
|
w * q.z + z * q.w + x * q.y - y * q.x);
|
|
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Quaternion<T>& Quaternion<T>::operator*=(T s)
|
|
{
|
|
x *= s; y *= s; z *= s; w *= s;
|
|
return *this;
|
|
}
|
|
|
|
// conjugate operator
|
|
template<class T> Quaternion<T> Quaternion<T>::operator~()
|
|
{
|
|
return Quaternion<T>(w, -x, -y, -z);
|
|
}
|
|
|
|
template<class T> Quaternion<T> Quaternion<T>::operator-()
|
|
{
|
|
return Quaternion<T>(-w, -x, -y, -z);
|
|
}
|
|
|
|
template<class T> Quaternion<T> Quaternion<T>::operator+()
|
|
{
|
|
return *this;
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T> operator+(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return Quaternion<T>(a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator-(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return Quaternion<T>(a.w - b.w, a.x - b.x, a.y - b.y, a.z - b.z);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator*(Quaternion<T> a, Quaternion<T> 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.y * b.w + a.z * b.x - a.x * b.z,
|
|
a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator*(T s, Quaternion<T> q)
|
|
{
|
|
return Quaternion<T>(s * q.w, s * q.x, s * q.y, s * q.z);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator*(Quaternion<T> q, T s)
|
|
{
|
|
return Quaternion<T>(s * q.w, s * q.x, s * q.y, s * q.z);
|
|
}
|
|
|
|
// equivalent to multiplying by the quaternion (0, v)
|
|
template<class T> Quaternion<T> operator*(Vector3<T> v, Quaternion<T> q)
|
|
{
|
|
return Quaternion<T>(-v.x * q.x - v.y * q.y - v.z * q.z,
|
|
v.x * q.w + v.y * q.z - v.z * q.y,
|
|
v.y * q.w + v.z * q.x - v.x * q.z,
|
|
v.z * q.w + v.x * q.y - v.y * q.x);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator/(Quaternion<T> q, T s)
|
|
{
|
|
return q * (1 / s);
|
|
}
|
|
|
|
template<class T> Quaternion<T> operator/(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return a * (~b / abs(b));
|
|
}
|
|
|
|
|
|
template<class T> bool operator==(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
|
|
}
|
|
|
|
template<class T> bool operator!=(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w;
|
|
}
|
|
|
|
|
|
// elementary functions
|
|
template<class T> Quaternion<T> conjugate(Quaternion<T> q)
|
|
{
|
|
return Quaternion<T>(q.w, -q.x, -q.y, -q.z);
|
|
}
|
|
|
|
template<class T> T norm(Quaternion<T> q)
|
|
{
|
|
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
|
|
}
|
|
|
|
template<class T> T abs(Quaternion<T> q)
|
|
{
|
|
return (T) sqrt(norm(q));
|
|
}
|
|
|
|
template<class T> Quaternion<T> exp(Quaternion<T> q)
|
|
{
|
|
if (q.isReal())
|
|
{
|
|
return Quaternion<T>((T) exp(q.w));
|
|
}
|
|
else
|
|
{
|
|
T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
|
|
T s = (T) sin(l);
|
|
T c = (T) cos(l);
|
|
T e = (T) exp(q.w);
|
|
T t = e * s / l;
|
|
return Quaternion<T>(e * c, t * q.x, t * q.y, t * q.z);
|
|
}
|
|
}
|
|
|
|
template<class T> Quaternion<T> log(Quaternion<T> q)
|
|
{
|
|
if (q.isReal())
|
|
{
|
|
if (q.w > 0)
|
|
{
|
|
return Quaternion<T>((T) log(q.w));
|
|
}
|
|
else if (q.w < 0)
|
|
{
|
|
// The log of a negative purely real quaternion has
|
|
// infinitely many values, all of the form (ln(-w), PI * I),
|
|
// where I is any unit vector. We arbitrarily choose an I
|
|
// of (1, 0, 0) here and whereever else a similar choice is
|
|
// necessary. Geometrically, the set of roots is a sphere
|
|
// of radius PI centered at ln(-w) on the real axis.
|
|
return Quaternion<T>((T) log(-q.w), (T) PI, 0, 0);
|
|
}
|
|
else
|
|
{
|
|
// error . . . ln(0) not defined
|
|
return Quaternion<T>(0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
|
|
T r = (T) sqrt(l * l + q.w * q.w);
|
|
T theta = (T) atan2(l, q.w);
|
|
T t = theta / l;
|
|
return Quaternion<T>((T) log(r), t * q.x, t * q.y, t * q.z);
|
|
}
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T> pow(Quaternion<T> q, T s)
|
|
{
|
|
return exp(s * log(q));
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T> pow(Quaternion<T> q, Quaternion<T> p)
|
|
{
|
|
return exp(p * log(q));
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T> sin(Quaternion<T> q)
|
|
{
|
|
if (q.isReal())
|
|
{
|
|
return Quaternion<T>((T) sin(q.w));
|
|
}
|
|
else
|
|
{
|
|
T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
|
|
T m = q.w;
|
|
T s = (T) sin(m);
|
|
T c = (T) cos(m);
|
|
T il = 1 / l;
|
|
T e0 = (T) exp(-l);
|
|
T e1 = (T) exp(l);
|
|
|
|
T c0 = (T) -0.5 * e0 * il * c;
|
|
T c1 = (T) 0.5 * e1 * il * c;
|
|
|
|
return Quaternion<T>((T) 0.5 * e0 * s, c0 * q.x, c0 * q.y, c0 * q.z) +
|
|
Quaternion<T>((T) 0.5 * e1 * s, c1 * q.x, c1 * q.y, c1 * q.z);
|
|
}
|
|
}
|
|
|
|
template<class T> Quaternion<T> cos(Quaternion<T> q)
|
|
{
|
|
if (q.isReal())
|
|
{
|
|
return Quaternion<T>((T) cos(q.w));
|
|
}
|
|
else
|
|
{
|
|
T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
|
|
T m = q.w;
|
|
T s = (T) sin(m);
|
|
T c = (T) cos(m);
|
|
T il = 1 / l;
|
|
T e0 = (T) exp(-l);
|
|
T e1 = (T) exp(l);
|
|
|
|
T c0 = (T) 0.5 * e0 * il * s;
|
|
T c1 = (T) -0.5 * e1 * il * s;
|
|
|
|
return Quaternion<T>((T) 0.5 * e0 * c, c0 * q.x, c0 * q.y, c0 * q.z) +
|
|
Quaternion<T>((T) 0.5 * e1 * c, c1 * q.x, c1 * q.y, c1 * q.z);
|
|
}
|
|
}
|
|
|
|
template<class T> Quaternion<T> sqrt(Quaternion<T> q)
|
|
{
|
|
// In general, the square root of a quaternion has two values, one
|
|
// of which is the negative of the other. However, any negative purely
|
|
// real quaternion has an infinite number of square roots.
|
|
// This function returns the positive root for positive reals and
|
|
// the root on the positive i axis for negative reals.
|
|
if (q.isReal())
|
|
{
|
|
if (q.w >= 0)
|
|
return Quaternion<T>((T) sqrt(q.w), 0, 0, 0);
|
|
else
|
|
return Quaternion<T>(0, (T) sqrt(-q.w), 0, 0);
|
|
}
|
|
else
|
|
{
|
|
T b = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
|
|
T r = (T) sqrt(q.w * q.w + b * b);
|
|
if (q.w >= 0)
|
|
{
|
|
T m = (T) sqrt((T) 0.5 * (r + q.w));
|
|
T l = b / (2 * m);
|
|
T t = l / b;
|
|
return Quaternion<T>(m, q.x * t, q.y * t, q.z * t);
|
|
}
|
|
else
|
|
{
|
|
T l = (T) sqrt((T) 0.5 * (r - q.w));
|
|
T m = b / (2 * l);
|
|
T t = l / b;
|
|
return Quaternion<T>(m, q.x * t, q.y * t, q.z * t);
|
|
}
|
|
}
|
|
}
|
|
|
|
template<class T> T real(Quaternion<T> q)
|
|
{
|
|
return q.w;
|
|
}
|
|
|
|
template<class T> Vector3<T> imag(Quaternion<T> q)
|
|
{
|
|
return Vector3<T>(q.x, q.y, q.z);
|
|
}
|
|
|
|
|
|
// Quaternion methods
|
|
|
|
template<class T> bool Quaternion<T>::isReal() const
|
|
{
|
|
return (x == 0 && y == 0 && z == 0);
|
|
}
|
|
|
|
template<class T> bool Quaternion<T>::isPure() const
|
|
{
|
|
return w == 0;
|
|
}
|
|
|
|
template<class T> T Quaternion<T>::normalize()
|
|
{
|
|
T s = abs(*this);
|
|
T invs = (T) 1 / (T) s;
|
|
x *= invs;
|
|
y *= invs;
|
|
z *= invs;
|
|
w *= invs;
|
|
|
|
return s;
|
|
}
|
|
|
|
// Set to the unit quaternion representing an axis angle rotation. Assume
|
|
// that axis is a unit vector
|
|
template<class T> void Quaternion<T>::setAxisAngle(Vector3<T> axis, T angle)
|
|
{
|
|
T s, c;
|
|
|
|
Math<T>::sincos(angle * (T) 0.5, s, c);
|
|
x = s * axis.x;
|
|
y = s * axis.y;
|
|
z = s * axis.z;
|
|
w = c;
|
|
}
|
|
|
|
|
|
// Assuming that this a unit quaternion, return the in axis/angle form the
|
|
// orientation which it represents.
|
|
template<class T> void Quaternion<T>::getAxisAngle(Vector3<T>& axis,
|
|
T& angle) const
|
|
{
|
|
// The quaternion has the form:
|
|
// w = cos(angle/2), (x y z) = sin(angle/2)*axis
|
|
|
|
T magSquared = x * x + y * y + z * z;
|
|
if (magSquared > (T) 1e-10)
|
|
{
|
|
T s = (T) 1 / (T) sqrt(magSquared);
|
|
axis.x = x * s;
|
|
axis.y = y * s;
|
|
axis.z = z * s;
|
|
if (w <= -1 || w >= 1)
|
|
angle = 0;
|
|
else
|
|
angle = (T) acos(w) * 2;
|
|
}
|
|
else
|
|
{
|
|
// The angle is zero, so we pick an arbitrary unit axis
|
|
axis.x = 1;
|
|
axis.y = 0;
|
|
axis.z = 0;
|
|
angle = 0;
|
|
}
|
|
}
|
|
|
|
|
|
// Convert this (assumed to be normalized) quaternion to a rotation matrix
|
|
template<class T> Matrix4<T> Quaternion<T>::toMatrix4() const
|
|
{
|
|
T wx = w * x * 2;
|
|
T wy = w * y * 2;
|
|
T wz = w * z * 2;
|
|
T xx = x * x * 2;
|
|
T xy = x * y * 2;
|
|
T xz = x * z * 2;
|
|
T yy = y * y * 2;
|
|
T yz = y * z * 2;
|
|
T zz = z * z * 2;
|
|
|
|
return Matrix4<T>(Vector4<T>(1 - yy - zz, xy + wz, xz - wy, 0),
|
|
Vector4<T>(xy - wz, 1 - xx - zz, yz + wx, 0),
|
|
Vector4<T>(xz + wy, yz - wx, 1 - xx - yy, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
// Convert this (assumed to be normalized) quaternion to a rotation matrix
|
|
template<class T> Matrix3<T> Quaternion<T>::toMatrix3() const
|
|
{
|
|
T wx = w * x * 2;
|
|
T wy = w * y * 2;
|
|
T wz = w * z * 2;
|
|
T xx = x * x * 2;
|
|
T xy = x * y * 2;
|
|
T xz = x * z * 2;
|
|
T yy = y * y * 2;
|
|
T yz = y * z * 2;
|
|
T zz = z * z * 2;
|
|
|
|
return Matrix3<T>(Vector3<T>(1 - yy - zz, xy + wz, xz - wy),
|
|
Vector3<T>(xy - wz, 1 - xx - zz, yz + wx),
|
|
Vector3<T>(xz + wy, yz - wx, 1 - xx - yy));
|
|
}
|
|
|
|
|
|
template<class T> T dot(Quaternion<T> a, Quaternion<T> b)
|
|
{
|
|
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
}
|
|
|
|
|
|
template<class T> Quaternion<T> Quaternion<T>::slerp(Quaternion<T> q0,
|
|
Quaternion<T> q1,
|
|
T t)
|
|
{
|
|
T c = dot(q0, q1);
|
|
T angle = (T) acos(c);
|
|
|
|
if (abs(angle) < (T) 1.0e-5)
|
|
return q0;
|
|
|
|
T s = (T) sin(angle);
|
|
T is = (T) 1.0 / s;
|
|
|
|
return q0 * ((T) sin((1 - t) * angle) * is) +
|
|
q1 * ((T) sin(t * angle) * is);
|
|
}
|
|
|
|
|
|
// Assuming that this is a unit quaternion representing an orientation,
|
|
// apply a rotation of angle radians about the specfied axis
|
|
template<class T> void Quaternion<T>::rotate(Vector3<T> axis, T angle)
|
|
{
|
|
Quaternion q;
|
|
q.setAxisAngle(axis, angle);
|
|
*this = q * *this;
|
|
}
|
|
|
|
|
|
// Assuming that this is a unit quaternion representing an orientation,
|
|
// apply a rotation of angle radians about the x-axis
|
|
template<class T> void Quaternion<T>::xrotate(T angle)
|
|
{
|
|
T s, c;
|
|
|
|
Math<T>::sincos(angle * (T) 0.5, s, c);
|
|
*this = Quaternion<T>(c, s, 0, 0) * *this;
|
|
}
|
|
|
|
// Assuming that this is a unit quaternion representing an orientation,
|
|
// apply a rotation of angle radians about the y-axis
|
|
template<class T> void Quaternion<T>::yrotate(T angle)
|
|
{
|
|
T s, c;
|
|
|
|
Math<T>::sincos(angle * (T) 0.5, s, c);
|
|
*this = Quaternion<T>(c, 0, s, 0) * *this;
|
|
}
|
|
|
|
// Assuming that this is a unit quaternion representing an orientation,
|
|
// apply a rotation of angle radians about the z-axis
|
|
template<class T> void Quaternion<T>::zrotate(T angle)
|
|
{
|
|
T s, c;
|
|
|
|
Math<T>::sincos(angle * (T) 0.5, s, c);
|
|
*this = Quaternion<T>(c, 0, 0, s) * *this;
|
|
}
|
|
|
|
|
|
#endif // _QUATERNION_H_
|