Serious-Engine/Sources/Engine/Math/Quaternion.h

467 lines
12 KiB
C
Raw Normal View History

2016-03-12 01:20:51 +01:00
/* Copyright (c) 2002-2012 Croteam Ltd.
This program is free software; you can redistribute it and/or modify
it under the terms of version 2 of the GNU General Public License as published by
the Free Software Foundation
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */
2016-03-11 14:57:17 +01:00
#ifndef SE_INCL_QUATERNION_H
#define SE_INCL_QUATERNION_H
#ifdef PRAGMA_ONCE
#pragma once
#endif
#include <Engine/Base/Assert.h>
#include <Engine/Math/Functions.h>
2016-03-11 14:57:17 +01:00
/*
* Template class for quaternion of arbitrary precision.
*/
template<class Type>
class Quaternion {
public:
Type q_w, q_x, q_y, q_z;
public:
// default constructor
inline Quaternion(void);
// constructor from four scalar values
inline Quaternion(Type w, Type x, Type y, Type z);
// conversion from euler angles
void FromEuler(const Vector<Type, 3> &a);
inline Type EPS(Type orig) const;
2016-03-11 14:57:17 +01:00
// conversion to matrix
void ToMatrix(Matrix<Type, 3, 3> &m) const;
// conversion from matrix
void FromMatrix(Matrix<Type, 3, 3> &m);
// conversion to/from axis-angle
void FromAxisAngle(const Vector<Type, 3> &n, Type a);
void ToAxisAngle(Vector<Type, 3> &n, Type &a);
// unary minus (fliping of the quaternion)
inline Quaternion<Type> operator-(void) const;
// conjugation
inline Quaternion<Type> operator~(void) const;
// inversion
inline Quaternion<Type> Inv(void) const;
// multiplication/division by a scalar
inline Quaternion<Type> operator*(Type t) const;
inline Quaternion<Type> &operator*=(Type t);
friend Quaternion<Type> operator*(Type t, Quaternion<Type> q)
{
return Quaternion<Type>(q.q_w*t, q.q_x*t, q.q_y*t, q.q_z*t);
}
2016-03-11 14:57:17 +01:00
inline Quaternion<Type> operator/(Type t) const;
inline Quaternion<Type> &operator/=(Type t);
// addition/substraction
inline Quaternion<Type> operator+(const Quaternion<Type> &q2) const;
inline Quaternion<Type> &operator+=(const Quaternion<Type> &q2);
inline Quaternion<Type> operator-(const Quaternion<Type> &q2) const;
inline Quaternion<Type> &operator-=(const Quaternion<Type> &q2);
// multiplication
inline Quaternion<Type> operator*(const Quaternion<Type> &q2) const;
inline Quaternion<Type> &operator*=(const Quaternion<Type> &q2);
// dot product
inline Type operator%(const Quaternion<Type> &q2) const;
// quaternion norm (euclidian length of a 4d vector)
inline Type Norm(void) const;
friend __forceinline CTStream &operator>>(CTStream &strm, Quaternion<Type> &q) {
strm>>q.q_w;
strm>>q.q_x;
strm>>q.q_y;
strm>>q.q_z;
return strm;
}
friend __forceinline CTStream &operator<<(CTStream &strm, const Quaternion<Type> &q) {
strm<<q.q_w;
strm<<q.q_x;
strm<<q.q_y;
strm<<q.q_z;
return strm;
}
2016-03-11 14:57:17 +01:00
// transcendental functions
friend Quaternion<Type> Exp(const Quaternion<Type> &q)
{
Type tAngle = (Type)sqrt(q.q_x*q.q_x + q.q_y*q.q_y + q.q_z*q.q_z);
Type tSin = sin(tAngle);
Type tCos = cos(tAngle);
if (fabs(tSin)<0.001) {
return Quaternion<Type>(tCos, q.q_x, q.q_y, q.q_z);
} else {
Type tRatio = tSin/tAngle;
return Quaternion<Type>(tCos, q.q_x*tRatio, q.q_y*tRatio, q.q_z*tRatio);
}
}
friend Quaternion<Type> Log(const Quaternion<Type> &q)
{
if (fabs(q.q_w)<1.0) {
Type tAngle = acos(q.q_w);
Type tSin = sin(tAngle);
if (fabs(tSin)>=0.001) {
Type tRatio = tAngle/tSin;
return Quaternion<Type>(Type(0), q.q_x*tRatio, q.q_y*tRatio, q.q_z*tRatio);
}
}
return Quaternion<Type>(Type(0), q.q_x, q.q_y, q.q_z);
}
2016-03-11 14:57:17 +01:00
// spherical linear interpolation
friend Quaternion<Type> Slerp(Type tT,
const Quaternion<Type> &q1, const Quaternion<Type> &q2)
{
Type tCos = q1%q2;
Quaternion<Type> qTemp;
if (tCos<Type(0)) {
tCos = -tCos;
qTemp = -q2;
} else {
qTemp = q2;
}
Type tF1, tF2;
if ((Type(1)-tCos) > Type(0.001)) {
// standard case (slerp)
Type tAngle = acos(tCos);
Type tSin = sin(tAngle);
tF1 = sin((Type(1)-tT)*tAngle)/tSin;
tF2 = sin(tT*tAngle)/tSin;
} else {
// linear interpolation
tF1 = Type(1)-tT;
tF2 = tT;
}
return q1*tF1 + qTemp*tF2;
}
2016-03-11 14:57:17 +01:00
// spherical quadratic interpolation
friend Quaternion<Type> Squad(Type tT,
2016-03-11 14:57:17 +01:00
const Quaternion<Type> &q1, const Quaternion<Type> &q2,
const Quaternion<Type> &qa, const Quaternion<Type> &qb)
{
return Slerp(2*tT*(1-tT),Slerp(tT,q1,q2),Slerp(tT,qa,qb));
}
2016-03-11 14:57:17 +01:00
};
// inline functions implementation
/*
* Default constructor.
*/
template<class Type>
inline Quaternion<Type>::Quaternion(void) {};
/* Constructor from three values. */
template<class Type>
inline Quaternion<Type>::Quaternion(Type w, Type x, Type y, Type z)
: q_w(w), q_x(x), q_y(y), q_z(z) {};
// unary minus (additive inversion)
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator-(void) const {
return Quaternion<Type>(-q_w, -q_x, -q_y, -q_z);
}
// conjugation
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator~(void) const {
return Quaternion<Type>(q_w, -q_x, -q_y, -q_z);
}
// multiplicative inversion
template<class Type>
inline Quaternion<Type> Quaternion<Type>::Inv(void) const {
return (~(*this))/Norm();
}
// multiplication/division by a scalar
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator*(Type t) const {
return Quaternion<Type>(q_w*t, q_x*t, q_y*t, q_z*t);
}
template<class Type>
inline Quaternion<Type> &Quaternion<Type>::operator*=(Type t) {
q_w*=t; q_x*=t; q_y*=t; q_z*=t;
return *this;
}
#if 0
2016-03-11 14:57:17 +01:00
template<class Type>
inline Quaternion<Type> operator*(Type t, Quaternion<Type> q) {
return Quaternion<Type>(q.q_w*t, q.q_x*t, q.q_y*t, q.q_z*t);
}
#endif
2016-03-11 14:57:17 +01:00
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator/(Type t) const {
return Quaternion<Type>(q_w/t, q_x/t, q_y/t, q_z/t);
}
template<class Type>
inline Quaternion<Type> &Quaternion<Type>::operator/=(Type t) {
q_w/=t; q_x/=t; q_y/=t; q_z/=t;
return *this;
}
// addition/substraction
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator+(const Quaternion<Type> &q2) const {
return Quaternion<Type>(q_w+q2.q_w, q_x+q2.q_x, q_y+q2.q_y, q_z+q2.q_z);
}
template<class Type>
inline Quaternion<Type> &Quaternion<Type>::operator+=(const Quaternion<Type> &q2) {
q_w+=q2.q_w; q_x+=q2.q_x; q_y+=q2.q_y; q_z+=q2.q_z;
return *this;
}
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator-(const Quaternion<Type> &q2) const {
return Quaternion<Type>(q_w-q2.q_w, q_x-q2.q_x, q_y-q2.q_y, q_z-q2.q_z);
}
template<class Type>
inline Quaternion<Type> &Quaternion<Type>::operator-=(const Quaternion<Type> &q2) {
q_w-=q2.q_w; q_x-=q2.q_x; q_y-=q2.q_y; q_z-=q2.q_z;
return *this;
}
// multiplication
template<class Type>
inline Quaternion<Type> Quaternion<Type>::operator*(const Quaternion<Type> &q2) const {
return Quaternion<Type>(
q_w*q2.q_w - q_x*q2.q_x - q_y*q2.q_y - q_z*q2.q_z,
q_w*q2.q_x + q_x*q2.q_w + q_y*q2.q_z - q_z*q2.q_y,
q_w*q2.q_y - q_x*q2.q_z + q_y*q2.q_w + q_z*q2.q_x,
q_w*q2.q_z + q_x*q2.q_y - q_y*q2.q_x + q_z*q2.q_w);
}
template<class Type>
inline Quaternion<Type> &Quaternion<Type>::operator*=(const Quaternion<Type> &q2) {
*this = (*this)*q2;
return *this;
}
// dot product
template<class Type>
inline Type Quaternion<Type>::operator%(const Quaternion<Type> &q2) const {
return q_w*q2.q_w + q_x*q2.q_x + q_y*q2.q_y + q_z*q2.q_z;
}
// quaternion norm (euclidian length of a 4d vector)
template<class Type>
inline Type Quaternion<Type>::Norm(void) const {
return (Type)sqrt(q_w*q_w + q_x*q_x + q_y*q_y + q_z*q_z);
}
#if 0
2016-03-11 14:57:17 +01:00
// transcendental functions
template<class Type>
inline Quaternion<Type> Exp(const Quaternion<Type> &q)
{
Type tAngle = (Type)sqrt(q.q_x*q.q_x + q.q_y*q.q_y + q.q_z*q.q_z);
Type tSin = sin(tAngle);
Type tCos = cos(tAngle);
if (fabs(tSin)<0.001) {
return Quaternion<Type>(tCos, q.q_x, q.q_y, q.q_z);
} else {
Type tRatio = tSin/tAngle;
return Quaternion<Type>(tCos, q.q_x*tRatio, q.q_y*tRatio, q.q_z*tRatio);
}
}
// transcendental functions
template<class Type>
inline Quaternion<Type> Log(const Quaternion<Type> &q)
{
if (fabs(q.q_w)<1.0) {
Type tAngle = acos(q.q_w);
Type tSin = sin(tAngle);
if (fabs(tSin)>=0.001) {
Type tRatio = tAngle/tSin;
return Quaternion<Type>(Type(0), q.q_x*tRatio, q.q_y*tRatio, q.q_z*tRatio);
}
}
return Quaternion<Type>(Type(0), q.q_x, q.q_y, q.q_z);
}
// spherical linear interpolation
template<class Type>
inline Quaternion<Type> Slerp(Type tT,
const Quaternion<Type> &q1, const Quaternion<Type> &q2)
{
Type tCos = q1%q2;
Quaternion<Type> qTemp;
if (tCos<Type(0)) {
tCos = -tCos;
qTemp = -q2;
} else {
qTemp = q2;
}
Type tF1, tF2;
if ((Type(1)-tCos) > Type(0.001)) {
// standard case (slerp)
Type tAngle = acos(tCos);
Type tSin = sin(tAngle);
tF1 = sin((Type(1)-tT)*tAngle)/tSin;
tF2 = sin(tT*tAngle)/tSin;
} else {
// linear interpolation
tF1 = Type(1)-tT;
tF2 = tT;
}
return q1*tF1 + qTemp*tF2;
}
// spherical quadratic interpolation
template<class Type>
inline Quaternion<Type> Squad(Type tT,
const Quaternion<Type> &q1, const Quaternion<Type> &q2,
const Quaternion<Type> &qa, const Quaternion<Type> &qb)
{
return Slerp(2*tT*(1-tT),Slerp(tT,q1,q2),Slerp(tT,qa,qb));
}
#endif
2016-03-11 14:57:17 +01:00
// conversion from euler angles
template<class Type>
void Quaternion<Type>::FromEuler(const Vector<Type, 3> &a)
{
Quaternion<Type> qH;
Quaternion<Type> qP;
Quaternion<Type> qB;
qH.q_w = Cos(a(1)/2);
qH.q_x = 0;
qH.q_y = Sin(a(1)/2);
qH.q_z = 0;
qP.q_w = Cos(a(2)/2);
qP.q_x = Sin(a(2)/2);
qP.q_y = 0;
qP.q_z = 0;
qB.q_w = Cos(a(3)/2);
qB.q_x = 0;
qB.q_y = 0;
qB.q_z = Sin(a(3)/2);
(*this) = qH*qP*qB;
}
// Check for almost, not really, but should be 0.0 values...
template<class Type>
Type Quaternion<Type>::EPS(Type orig) const
{
if ((orig <= 10e-6) && (orig >= -10e-6))
return(0.0);
return(orig);
}
2016-03-11 14:57:17 +01:00
// conversion to matrix
template<class Type>
void Quaternion<Type>::ToMatrix(Matrix<Type, 3, 3> &m) const
{
Type xx = 2*q_x*q_x; Type xy = 2*q_x*q_y; Type xz = 2*q_x*q_z;
Type yy = 2*q_y*q_y; Type yz = 2*q_y*q_z; Type zz = 2*q_z*q_z;
Type wx = 2*q_w*q_x; Type wy = 2*q_w*q_y; Type wz = 2*q_w*q_z;
m(1,1) = EPS(1.0-(yy+zz)); m(1,2) = EPS(xy-wz); m(1,3) = EPS(xz+wy);
m(2,1) = EPS(xy+wz); m(2,2) = EPS(1.0-(xx+zz)); m(2,3) = EPS(yz-wx);
m(3,1) = EPS(xz-wy); m(3,2) = EPS(yz+wx); m(3,3) = EPS(1.0-(xx+yy));
2016-03-11 14:57:17 +01:00
}
// conversion from matrix
template<class Type>
void Quaternion<Type>::FromMatrix(Matrix<Type, 3, 3> &m)
{
Type trace = m(1,1)+m(2,2)+m(3,3);
Type root;
if ( trace > 0.0 )
{
// |w| > 1/2, may as well choose w > 1/2
root = sqrt(trace+1.0); // 2w
q_w = 0.5*root;
root = 0.5/root; // 1/(4w)
q_x = (m(3,2)-m(2,3))*root;
q_y = (m(1,3)-m(3,1))*root;
q_z = (m(2,1)-m(1,2))*root;
}
else
{
// |w| <= 1/2
static int next[3] = { 1, 2, 0 };
int i = 0;
if ( m(2,2) > m(1,1) )
i = 1;
if ( m(3,3) > m(i+1,i+1) )
i = 2;
int j = next[i];
int k = next[j];
root = sqrt(m(i+1,i+1)-m(j+1,j+1)-m(k+1,k+1)+1.0);
Type* quat[3] = { &q_x, &q_y, &q_z };
*quat[i] = 0.5*root;
root = 0.5/root;
q_w = (m(k+1,j+1)-m(j+1,k+1))*root;
*quat[j] = (m(j+1,i+1)+m(i+1,j+1))*root;
*quat[k] = (m(k+1,i+1)+m(i+1,k+1))*root;
}
}
// conversion to/from axis-angle
template<class Type>
void Quaternion<Type>::FromAxisAngle(const Vector<Type, 3> &n, Type a)
{
Type tSin = sin(a/2);
Type tCos = cos(a/2);
q_x = n(1)*tSin;
q_y = n(2)*tSin;
q_z = n(3)*tSin;
q_w = tCos;
}
template<class Type>
void Quaternion<Type>::ToAxisAngle(Vector<Type, 3> &n, Type &a)
{
Type tCos = q_w;
Type tSin = sqrt(Type(1)-tCos*tCos);
a = 2*acos(tCos);
// if angle is not zero
if (Abs(tSin)>=0.001) {
n(1) = q_x / tSin;
n(2) = q_y / tSin;
n(3) = q_z / tSin;
// if angle is zero
} else {
n(1) = Type(1);
n(2) = Type(0);
n(3) = Type(0);
}
}
#endif /* include-once check. */