367 lines
8.1 KiB
C
367 lines
8.1 KiB
C
|
#ifndef BLUECORE_QUATERNION_H
|
||
|
|
#define BLUECORE_QUATERNION_H
|
||
|
|
|
||
|
|
#include "Vector.h"
|
||
|
|
|
||
|
|
namespace BlueCore
|
||
|
|
{
|
||
|
|
template <class T>
|
||
|
|
class QuaternionTemplate
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
T w, x, y, z;
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* constructor
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate()
|
||
|
|
{
|
||
|
|
w = static_cast<T>(1.0);
|
||
|
|
x = static_cast<T>(0.0);
|
||
|
|
y = static_cast<T>(0.0);
|
||
|
|
z = static_cast<T>(0.0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* contructor
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate( S w, S x, S y, S z )
|
||
|
|
{
|
||
|
|
this->w = static_cast<T>(w);
|
||
|
|
this->x = static_cast<T>(x);
|
||
|
|
this->y = static_cast<T>(y);
|
||
|
|
this->z = static_cast<T>(z);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* contructor
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate( Vector3Template<S> axis, S angle )
|
||
|
|
{
|
||
|
|
T half_angle = static_cast<T>(angle)/static_cast<T>(2.0);
|
||
|
|
T sin_half_angle = static_cast<T>( sin( half_angle ) );
|
||
|
|
|
||
|
|
w = static_cast<T>( cos( half_angle ) );
|
||
|
|
x = sin_half_angle * static_cast<T>(axis.x);
|
||
|
|
y = sin_half_angle * static_cast<T>(axis.y);
|
||
|
|
z = sin_half_angle * static_cast<T>(axis.z);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* contructor
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate( S h, S a, S b )
|
||
|
|
{
|
||
|
|
T c1 = static_cast<T>( cos(h / 2.0) );
|
||
|
|
T c2 = static_cast<T>( cos(a / 2.0) );
|
||
|
|
T c3 = static_cast<T>( cos(b / 2.0) );
|
||
|
|
T s1 = static_cast<T>( sin(h / 2.0) );
|
||
|
|
T s2 = static_cast<T>( sin(a / 2.0) );
|
||
|
|
T s3 = static_cast<T>( sin(b / 2.0) );
|
||
|
|
|
||
|
|
w = c1 * c2 * c3 - s1 * s2 * s3;
|
||
|
|
x = s1 * s2 * c3 + c1 * c2 * s3;
|
||
|
|
y = s1 * c2 * c3 + c1 * s2 * s3;
|
||
|
|
z = c1 * s2 * c3 - s1 * c2 * s3;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* identity
|
||
|
|
*/
|
||
|
|
inline void identity()
|
||
|
|
{
|
||
|
|
w = static_cast<T>(1.0);
|
||
|
|
x = static_cast<T>(0.0);
|
||
|
|
y = static_cast<T>(0.0);
|
||
|
|
z = static_cast<T>(0.0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator +
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> operator + (
|
||
|
|
const QuaternionTemplate<S> &a ) const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>(
|
||
|
|
w + static_cast<T>(a.w),
|
||
|
|
x + static_cast<T>(a.x),
|
||
|
|
y + static_cast<T>(a.y),
|
||
|
|
z + static_cast<T>(a.z) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator +=
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> &operator += (
|
||
|
|
const QuaternionTemplate<S> &a )
|
||
|
|
{
|
||
|
|
w += static_cast<T>(a.w);
|
||
|
|
x += static_cast<T>(a.x);
|
||
|
|
y += static_cast<T>(a.y);
|
||
|
|
z += static_cast<T>(a.z);
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator -
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> operator - (
|
||
|
|
const QuaternionTemplate<S> &a ) const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>(
|
||
|
|
w - static_cast<T>(a.w),
|
||
|
|
x - static_cast<T>(a.x),
|
||
|
|
y - static_cast<T>(a.y),
|
||
|
|
z - static_cast<T>(a.z) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator *
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> operator * (
|
||
|
|
const QuaternionTemplate<S> &a ) const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>(
|
||
|
|
w * static_cast<T>(a.w) -
|
||
|
|
x * static_cast<T>(a.x) -
|
||
|
|
y * static_cast<T>(a.y) -
|
||
|
|
z * static_cast<T>(a.z),
|
||
|
|
w * static_cast<T>(a.x) +
|
||
|
|
x * static_cast<T>(a.w) +
|
||
|
|
y * static_cast<T>(a.z) -
|
||
|
|
z * static_cast<T>(a.y),
|
||
|
|
w * static_cast<T>(a.y) -
|
||
|
|
x * static_cast<T>(a.z) +
|
||
|
|
y * static_cast<T>(a.w) +
|
||
|
|
z * static_cast<T>(a.x),
|
||
|
|
w * static_cast<T>(a.z) +
|
||
|
|
x * static_cast<T>(a.y) -
|
||
|
|
y * static_cast<T>(a.x) +
|
||
|
|
z * static_cast<T>(a.w) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator *=
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> &operator *=(
|
||
|
|
const QuaternionTemplate<S> &a )
|
||
|
|
{
|
||
|
|
w = w * static_cast<T>(a.w) - x * static_cast<T>(a.x) -
|
||
|
|
y * static_cast<T>(a.y) - z * static_cast<T>(a.z);
|
||
|
|
x = w * static_cast<T>(a.x) + x * static_cast<T>(a.w) +
|
||
|
|
y * static_cast<T>(a.z) - z * static_cast<T>(a.y);
|
||
|
|
y = w * static_cast<T>(a.y) - x * static_cast<T>(a.z) +
|
||
|
|
y * static_cast<T>(a.w) + z * static_cast<T>(a.x);
|
||
|
|
z = w * static_cast<T>(a.z) + x * static_cast<T>(a.y) -
|
||
|
|
y * static_cast<T>(a.x) + z * static_cast<T>(a.w);
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator -=
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> &operator -= (
|
||
|
|
const QuaternionTemplate<S> &a )
|
||
|
|
{
|
||
|
|
w -= static_cast<T>(a.w);
|
||
|
|
x -= static_cast<T>(a.x);
|
||
|
|
y -= static_cast<T>(a.y);
|
||
|
|
z -= static_cast<T>(a.z);
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* operator =
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline QuaternionTemplate<T> &operator = (
|
||
|
|
const QuaternionTemplate<S> &a )
|
||
|
|
{
|
||
|
|
w = static_cast<T>(a.w);
|
||
|
|
x = static_cast<T>(a.x);
|
||
|
|
y = static_cast<T>(a.y);
|
||
|
|
z = static_cast<T>(a.z);
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* unit
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate<T> unit() const
|
||
|
|
{
|
||
|
|
T d = 1/sqrt( w*w + x*x + y*y + z*z );
|
||
|
|
return QuaternionTemplate<T>( w * d, x * d, y * d, z * d );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* inversed
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate<T> inversed() const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>( w, -x, -y, -z );
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* inversed
|
||
|
|
*/
|
||
|
|
inline void inverse()
|
||
|
|
{
|
||
|
|
x = -x;
|
||
|
|
y = -y;
|
||
|
|
z = -z;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* normalize
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate<T> &normalize()
|
||
|
|
{
|
||
|
|
T d = 1/sqrt( w*w + x*x + y*y + z*z );
|
||
|
|
w *= d;
|
||
|
|
x *= d;
|
||
|
|
y *= d;
|
||
|
|
z *= d;
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* apply
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline const Vector3Template<T> apply(
|
||
|
|
const Vector3Template<S> &a ) const
|
||
|
|
{
|
||
|
|
T xx = x*x, xy = x*y, xz = x*z, xw = x*w,
|
||
|
|
yy = y*y, yz = y*z, yw = y*w,
|
||
|
|
zz = z*z, zw = z*w;
|
||
|
|
|
||
|
|
return Vector3Template<T>(
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
|
||
|
|
static_cast<T>(a.y) * ( xy - zw ) +
|
||
|
|
static_cast<T>(a.z) * ( xz + yw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
|
||
|
|
static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
|
||
|
|
static_cast<T>(a.z) * ( yz - xw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
|
||
|
|
static_cast<T>(a.y) * ( yz + xw ) +
|
||
|
|
static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* apply
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline const Vector3Template<T> operator * (
|
||
|
|
const Vector3Template<S> &a ) const
|
||
|
|
{
|
||
|
|
T xx = x*x, xy = x*y, xz = x*z, xw = x*w,
|
||
|
|
yy = y*y, yz = y*z, yw = y*w,
|
||
|
|
zz = z*z, zw = z*w;
|
||
|
|
|
||
|
|
return Vector3Template<T>(
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
|
||
|
|
static_cast<T>(a.y) * ( xy - zw ) +
|
||
|
|
static_cast<T>(a.z) * ( xz + yw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
|
||
|
|
static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
|
||
|
|
static_cast<T>(a.z) * ( yz - xw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
|
||
|
|
static_cast<T>(a.y) * ( yz + xw ) +
|
||
|
|
static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* applyInversed
|
||
|
|
*/
|
||
|
|
template <class S>
|
||
|
|
inline Vector3Template<T> applyInversed(
|
||
|
|
const Vector3Template<S> &a ) const
|
||
|
|
{
|
||
|
|
T xx = x*x, xy = x*y, xz = x*z, xw = -x*w,
|
||
|
|
yy = y*y, yz = y*z, yw = -y*w,
|
||
|
|
zz = z*z, zw = -z*w;
|
||
|
|
|
||
|
|
return Vector3Template<T>(
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
|
||
|
|
static_cast<T>(a.y) * ( xy - zw ) +
|
||
|
|
static_cast<T>(a.z) * ( xz + yw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
|
||
|
|
static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
|
||
|
|
static_cast<T>(a.z) * ( yz - xw ) ),
|
||
|
|
|
||
|
|
2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
|
||
|
|
static_cast<T>(a.y) * ( yz + xw ) +
|
||
|
|
static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* transform from ode to gl coodinates
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate<T> toGl() const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>( w, x, z, -y );
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* transform from gl to ode coodinates
|
||
|
|
*/
|
||
|
|
inline QuaternionTemplate<T> toOde() const
|
||
|
|
{
|
||
|
|
return QuaternionTemplate<T>( w, x, -z, y );
|
||
|
|
}
|
||
|
|
|
||
|
|
inline QuaternionTemplate<T> slerp( const QuaternionTemplate<T> &q, const Scalar &t )
|
||
|
|
{
|
||
|
|
Scalar phi = acos(w*q.w + x*q.x + y*q.y + z*q.z);
|
||
|
|
Scalar s = 1 / sin(phi);
|
||
|
|
Scalar a = sin(phi*(1-t)) * s;
|
||
|
|
Scalar b = sin(phi*t) * s;
|
||
|
|
|
||
|
|
return QuaternionTemplate<T>( a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z );
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
typedef QuaternionTemplate<float> QuaternionFloat;
|
||
|
|
typedef QuaternionTemplate<double> QuaternionDouble;
|
||
|
|
typedef QuaternionTemplate<Scalar> Quaternion;
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|