894 lines
27 KiB
C++
894 lines
27 KiB
C++
//////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Crytek Common Source code
|
|
//
|
|
// File:Cry_Quat.h
|
|
// Description: Common quaternion class
|
|
//
|
|
// History:
|
|
// -Feb 27,2003: Created by Ivo Herzeg
|
|
//
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
#ifndef _CRYQUAT_H
|
|
#define _CRYQUAT_H
|
|
|
|
#include "platform.h"
|
|
#include "Cry_Vector2.h"
|
|
#include "Cry_Vector3.h"
|
|
#include "Cry_Matrix.h"
|
|
|
|
|
|
#if defined(LINUX)
|
|
#undef assert
|
|
#define assert(exp) (void)( (exp) || (printf("Assert: ' %s ' has failed\n", #exp), 0) )
|
|
#endif
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// Typedefs //
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
typedef Quaternion_tpl<f32> CryQuat;
|
|
#ifndef MAX_API_NUM
|
|
typedef Quaternion_tpl<f32> Quat;
|
|
typedef Quaternion_tpl<real> Quat_f64;
|
|
#endif
|
|
typedef Quaternion_tpl<f32> quaternionf;
|
|
typedef Quaternion_tpl<real> quaternion;
|
|
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// Definitions //
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
#define M00 data[SI*0+SJ*0]
|
|
#define M10 data[SI*1+SJ*0]
|
|
#define M20 data[SI*2+SJ*0]
|
|
#define M30 data[SI*3+SJ*0]
|
|
#define M01 data[SI*0+SJ*1]
|
|
#define M11 data[SI*1+SJ*1]
|
|
#define M21 data[SI*2+SJ*1]
|
|
#define M31 data[SI*3+SJ*1]
|
|
#define M02 data[SI*0+SJ*2]
|
|
#define M12 data[SI*1+SJ*2]
|
|
#define M22 data[SI*2+SJ*2]
|
|
#define M32 data[SI*3+SJ*2]
|
|
#define M03 data[SI*0+SJ*3]
|
|
#define M13 data[SI*1+SJ*3]
|
|
#define M23 data[SI*2+SJ*3]
|
|
#define M33 data[SI*3+SJ*3]
|
|
|
|
|
|
|
|
#define m_00 m.data[SI1*0+SJ1*0]
|
|
#define m_10 m.data[SI1*1+SJ1*0]
|
|
#define m_20 m.data[SI1*2+SJ1*0]
|
|
#define m_30 m.data[SI1*3+SJ1*0]
|
|
#define m_01 m.data[SI1*0+SJ1*1]
|
|
#define m_11 m.data[SI1*1+SJ1*1]
|
|
#define m_21 m.data[SI1*2+SJ1*1]
|
|
#define m_31 m.data[SI1*3+SJ1*1]
|
|
#define m_02 m.data[SI1*0+SJ1*2]
|
|
#define m_12 m.data[SI1*1+SJ1*2]
|
|
#define m_22 m.data[SI1*2+SJ1*2]
|
|
#define m_32 m.data[SI1*3+SJ1*2]
|
|
#define m_03 m.data[SI1*0+SJ1*3]
|
|
#define m_13 m.data[SI1*1+SJ1*3]
|
|
#define m_23 m.data[SI1*2+SJ1*3]
|
|
#define m_33 m.data[SI1*3+SJ1*3]
|
|
|
|
|
|
#if defined(LINUX)
|
|
template<class F,int SI,int SJ>
|
|
Quaternion_tpl<F> GetQuatFromMat33(const Matrix33_tpl<F,SI,SJ>& m);
|
|
#endif
|
|
|
|
|
|
//----------------------------------------------------------------------
|
|
// Quaternion
|
|
//----------------------------------------------------------------------
|
|
template <class F> struct Quaternion_tpl
|
|
{
|
|
|
|
Vec3_tpl<F> v; F w;
|
|
|
|
//-------------------------------
|
|
|
|
//constructors
|
|
Quaternion_tpl() { w=1; v(0,0,0); }
|
|
Quaternion_tpl( F W,F X,F Y,F Z ) { w = W; v.x=X; v.y=Y; v.z=Z; }
|
|
Quaternion_tpl( const F angle, const Vec3_tpl<F> &axis);
|
|
|
|
|
|
template<class F1,int SI,int SJ>
|
|
explicit ILINE Quaternion_tpl(const Matrix33_tpl<F1,SI,SJ>& m) { *this=GetQuatFromMat33(m); }
|
|
template<class T>
|
|
explicit ILINE Quaternion_tpl(const Matrix34_tpl<T>& m) { *this=GetQuatFromMat33(Matrix33(m)); }
|
|
template<class F1,int SI,int SJ>
|
|
explicit ILINE Quaternion_tpl(const Matrix44_tpl<F1,SI,SJ>& m) { *this=GetQuatFromMat33(Matrix33_tpl<f32,3,1>(m)); }
|
|
|
|
|
|
//CONSTRUCTOR: implement the copy/casting/assignement constructor:
|
|
template <class T>
|
|
ILINE Quaternion_tpl( const Quaternion_tpl<T>& q ) { w=q.w; v.x=q.v.x; v.y=q.v.y; v.z=q.v.z; }
|
|
|
|
|
|
//assignement of equal types
|
|
ILINE Quaternion_tpl& operator=(const Quaternion_tpl<F>& q) {
|
|
w=q.w; v=q.v; return *this;
|
|
}
|
|
//double=f32
|
|
template<typename U> ILINE Quaternion_tpl& operator=(const U& q) {
|
|
w=(F)q.w; v.x=(F)q.v.x; v.y=(F)q.v.y; v.z=(F)q.v.z; return *this;
|
|
}
|
|
|
|
/* declared as static but never used or implemented as static member functions, Linux gcc don't like it too
|
|
#ifndef WIN64
|
|
//dot-product
|
|
template<typename F1,typename F2> static f32 operator | (const Quaternion_tpl<F1>& q, const Quaternion_tpl<F2>& p);
|
|
|
|
//multiplication operator
|
|
template<typename F1,typename F2> static Quaternion_tpl<F1> operator * (const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
template<typename F1,typename F2> static void operator*=(Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
|
|
//division operator
|
|
template<typename F1,typename F2> static Quaternion_tpl<F1> operator / (const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
template<typename F1,typename F2> static void operator /= (Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
|
|
//addition operator
|
|
template<typename F1,typename F2> static Quaternion_tpl<F1> operator+(const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
template<typename F1,typename F2> static void operator+=(Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
|
|
//subtraction operator
|
|
template<typename F1,typename F2> static Quaternion_tpl<F1> operator - (const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
template<typename F1,typename F2> static void operator-=(Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p);
|
|
|
|
//rotate a vector by a quaternion
|
|
template<typename F1,typename F2> static Vec3_tpl<F2> operator*(const Quaternion_tpl<F1> &q, const Vec3_tpl<F2> &v);
|
|
template<typename F1,typename F2> static Vec3_tpl<F1> operator*(const Vec3_tpl<F1> &v, const Quaternion_tpl<F2> &q);
|
|
#endif
|
|
*/
|
|
//multiplication by a scalar
|
|
//friend Quaternion_tpl<F> operator * ( const Quaternion_tpl<F> &q, f32 t );
|
|
//friend Quaternion_tpl<F> operator * ( f32 t, const Quaternion_tpl<F> &q );
|
|
void operator *= (F op) { w*=op; v*=op; }
|
|
|
|
//negate quaternion. dont confuse this with quaternion-inversion.
|
|
Quaternion_tpl<F> operator - () const { return Quaternion_tpl<F>( -w,-v ); };
|
|
|
|
Quaternion_tpl<F> operator ! () const;
|
|
|
|
|
|
|
|
|
|
|
|
void SetIdentity(void);
|
|
static Quaternion_tpl<F> GetIdentity(void);
|
|
|
|
void SetRotationAA(F rad, const Vec3_tpl<F> &axis);
|
|
static Quaternion_tpl<F> GetRotationAA(F rad, const Vec3_tpl<F> &axis);
|
|
|
|
void SetRotationAA(F cosha, F sinha, const Vec3_tpl<F> &axis);
|
|
static Quaternion_tpl<F> GetRotationAA(F cosha, F sinha, const Vec3_tpl<F> &axis);
|
|
|
|
void SetRotationV0V1(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1);
|
|
static Quaternion_tpl<F> GetRotationV0V1(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1);
|
|
|
|
void SetRotationXYZ(const Ang3 &a);
|
|
static Quaternion_tpl<F> GetRotationXYZ(const Ang3 &a);
|
|
|
|
|
|
void Invert( void );
|
|
static Quaternion_tpl<F> GetInverted(Quaternion_tpl<F>& q);
|
|
|
|
void Normalize(void);
|
|
static Quaternion_tpl<F> GetNormalized( const Quaternion_tpl<F> &q );
|
|
|
|
static F GetLength( const Quaternion_tpl<F> &q );
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*!
|
|
* Constructor using a scalar and a vector
|
|
*
|
|
* Example:
|
|
* Vec3 vec;
|
|
* Quaternion quat = quaternionf( 0.5f, vec );
|
|
*/
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F>::Quaternion_tpl( const F angle, const Vec3_tpl<F> &axis) { w=angle; v=axis; }
|
|
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* The "inner product" or "dot product" operation.
|
|
*
|
|
* calculate the "inner product" between two Quaternion.
|
|
* If both Quaternion are unit-quaternions, the result is the cosine: p*q=cos(angle)
|
|
*
|
|
* Example:
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* f32 cosine = ( p | q );
|
|
*
|
|
*/
|
|
template<typename F1,typename F2>
|
|
ILINE f32 operator | (const Quaternion_tpl<F1>& q, const Quaternion_tpl<F2>& p) {
|
|
return (q.v.x*p.v.x + q.v.y*p.v.y + q.v.z*p.v.z + q.w*p.w);
|
|
}
|
|
|
|
|
|
/*!
|
|
* multiplication operator
|
|
*
|
|
* Example 1:
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion result=p*q;
|
|
*
|
|
* Example 2: (self-multiplication)
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion p*=q;
|
|
*
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Quaternion_tpl<F1> operator * (const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
return Quaternion_tpl<F1>(q.w*p.w-(q.v|p.v), q.v*p.w + q.w*p.v + (q.v%p.v) );
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE void operator *= (Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
F1 s0=q.w; q.w=q.w*p.w-(q.v|p.v); q.v=p.v*s0+q.v*p.w+(q.v%p.v);
|
|
}
|
|
|
|
/*!
|
|
* division operator
|
|
*
|
|
* Example 1:
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion result=p/q;
|
|
*
|
|
* Example 2: (self-division)
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion p/=q;
|
|
*
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Quaternion_tpl<F1> operator / (const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
return (!p*q);
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE void operator /= (Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
q=(!p*q);
|
|
}
|
|
|
|
|
|
/*!
|
|
* addition operator
|
|
*
|
|
* Example:
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion result=p+q;
|
|
*
|
|
* Example:(self-addition operator)
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion p-=q;
|
|
*
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Quaternion_tpl<F1> operator+(const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
return Quaternion_tpl<F1>( q.w+p.w, q.v+p.v );
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE void operator+=(Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
q.w+=p.w; q.v+=p.v;
|
|
}
|
|
|
|
|
|
|
|
/*!
|
|
* subtraction operator
|
|
*
|
|
* Example:
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion result=p-q;
|
|
*
|
|
* Example: (self-subtraction operator)
|
|
* Quaternion p(1,0,0,0),q(1,0,0,0);
|
|
* Quaternion p-=q;
|
|
*
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Quaternion_tpl<F1> operator-(const Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
return Quaternion_tpl<F1>( q.w-p.w, q.v-p.v);
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE void operator-=(Quaternion_tpl<F1> &q, const Quaternion_tpl<F2> &p) {
|
|
q.w-=p.w; q.v-=p.v;
|
|
}
|
|
|
|
|
|
//! Scale quaternion free function.
|
|
template <typename F1>
|
|
ILINE Quaternion_tpl<F1> operator * ( const Quaternion_tpl<F1> &q, f32 t ) {
|
|
return Quaternion_tpl<F1>( q.w*t, q.v*t );
|
|
};
|
|
//! Scale quaternion free function.
|
|
template <typename F1>
|
|
ILINE Quaternion_tpl<F1> operator *( f32 t, const Quaternion_tpl<F1> &q ) {
|
|
return Quaternion_tpl<F1>( t*q.w, t*q.v );
|
|
};
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* multiplication operator with a Vec3 (3D rotations with quaternions)
|
|
*
|
|
* Example:
|
|
* Quaternion q(1,0,0,0);
|
|
* Vec3 v(33,44,55);
|
|
* Vec3 result = q*v;
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F2> operator*(const Quaternion_tpl<F1> &q, const Vec3_tpl<F2> &v) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(q|q)))<0.05); //check if unit-quaternion
|
|
#endif
|
|
F1 vxvx=q.v.x*q.v.x;
|
|
F1 vzvz=q.v.z*q.v.z;
|
|
F1 vyvy=q.v.y*q.v.y;
|
|
F1 vxvy=q.v.x*q.v.y;
|
|
F1 vxvz=q.v.x*q.v.z;
|
|
F1 vyvz=q.v.y*q.v.z;
|
|
F1 svx=q.w*q.v.x,svy=q.w*q.v.y,svz=q.w*q.v.z;
|
|
Vec3_tpl<F2> res;
|
|
res.x = v.x*(1-(vyvy+vzvz)*2) + v.y*(vxvy-svz)*2 + v.z*(vxvz+svy)*2;
|
|
res.y = v.x*(vxvy+svz)*2 + v.y*(1-(vxvx+vzvz)*2) + v.z*(vyvz-svx)*2;
|
|
res.z = v.x*(vxvz-svy)*2 + v.y*(vyvz+svx)*2 + v.z*(1-(vxvx+vyvy)*2);
|
|
return res;
|
|
}
|
|
|
|
|
|
/*!
|
|
* multiplication operator with a Vec3 (3D rotations with quaternions)
|
|
*
|
|
* Example:
|
|
* Quaternion q(1,0,0,0);
|
|
* Vec3 v(33,44,55);
|
|
* Vec3 result = v*q;
|
|
*/
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1> operator*(const Vec3_tpl<F1> &v, const Quaternion_tpl<F2> &q) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(q|q)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
F2 vxvx=q.v.x*q.v.x;
|
|
F2 vzvz=q.v.z*q.v.z;
|
|
F2 vyvy=q.v.y*q.v.y;
|
|
F2 vxvy=q.v.x*q.v.y;
|
|
F2 vxvz=q.v.x*q.v.z;
|
|
F2 vyvz=q.v.y*q.v.z;
|
|
F2 svx=q.w*q.v.x,svy=q.w*q.v.y,svz=q.w*q.v.z;
|
|
Vec3_tpl<F1> res;
|
|
res.x = v.x*(1-(vyvy+vzvz)*2) + v.y*(vxvy+svz)*2 + v.z*(vxvz-svy)*2;
|
|
res.y = v.x*(vxvy-svz)*2 + v.y*(1-(vxvx+vzvz)*2) + v.z*(vyvz+svx)*2;
|
|
res.z = v.x*(vxvz+svy)*2 + v.y*(vyvz-svx)*2 + v.z*(1-(vxvx+vyvy)*2);
|
|
return res;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* invert quaternion.
|
|
*
|
|
* Example 1:
|
|
* Quaternion q=GetRotationXYZ<f32>(Ang3(1,2,3));
|
|
* Quaternion result = !q;
|
|
*
|
|
* Quaternion result = GetInverted(q);
|
|
*
|
|
* q.Invert();
|
|
*
|
|
*/
|
|
template<class F> ILINE Quaternion_tpl<F> Quaternion_tpl<F>::operator ! () const {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(*this|*this)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
return Quaternion_tpl(w,-v);
|
|
}
|
|
template<class F> ILINE void Quaternion_tpl<F>::Invert( void ) { *this=!*this; }
|
|
template<class F> ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetInverted(Quaternion_tpl<F>& q) { return !q; }
|
|
template<class F> ILINE Quaternion_tpl<F> GetInverted(Quaternion_tpl<F>& q) { return !q; }
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* Create rotation-quaternion around an arbitrary axis (=CVector).
|
|
* This is an implemetation of "Eulers Theorem". the axis is assumed to be nomalised.
|
|
*
|
|
* Example:
|
|
* Quaternion_tpl<f32> q=GetRotationAA( 2.14255f, GetNormalized(Vec3(1,2,3)) );
|
|
* q.SetRotationAA( 2.14255f, GetNormalized(Vec3(1,2,3)) );
|
|
*/
|
|
template<class F>
|
|
ILINE void Quaternion_tpl<F>::SetRotationAA(F rad, const Vec3_tpl<F> &axis) { F cs[2]; sincos_tpl( rad*(F)0.5, cs); SetRotationAA(cs[0],cs[1], axis); }
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetRotationAA(F rad, const Vec3_tpl<F> &axis) { Quaternion_tpl<F> q; q.SetRotationAA(rad,axis); return q; }
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> GetRotationAA(F rad, const Vec3_tpl<F> &axis) { Quaternion_tpl<F> q; q.SetRotationAA(rad,axis); return q; }
|
|
|
|
|
|
template<class F>ILINE void Quaternion_tpl<F>::SetRotationAA(F cosha, F sinha, const Vec3_tpl<F> &axis) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(axis|axis)))<0.001); //check if unit-vector
|
|
#endif
|
|
w=cosha; v=axis*sinha;
|
|
}
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetRotationAA(F cosha, F sinha, const Vec3_tpl<F> &axis) { Quaternion_tpl<F> q; q.SetRotationAA(cosha,sinha,axis); return q; }
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> GetRotationAA(F cosha, F sinha, const Vec3_tpl<F> &axis) { Quaternion_tpl<F> q; q.SetRotationAA(cosha,sinha,axis); return q; }
|
|
|
|
|
|
/*!
|
|
*
|
|
* Create rotation-quaternion that rotates from one vector to another.
|
|
* Both vectors are assumed to be nomalised.
|
|
*
|
|
* Example:
|
|
* Quaternion_tpl<f32> q=GetRotationV0V1( GetNormalized(Vec3(3,1,7)), GetNormalized(Vec3(1,2,3)) );
|
|
* q.SetRotationV0V1( GetNormalized(Vec3(3,1,7)), GetNormalized(Vec3(1,2,3)) );
|
|
*/
|
|
template<class F>
|
|
void Quaternion_tpl<F>::SetRotationV0V1(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1)
|
|
{
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(v0|v0)))<0.001); //check if unit-vector
|
|
assert((fabs_tpl(1-(v1|v1)))<0.001); //check if unit-vector
|
|
#endif
|
|
w=sqrt_tpl(max((F)0.0,(1+v0*v1)*(F)0.5));
|
|
if (w>0.001) {
|
|
v=v0^v1; v*=(F)0.5/w;
|
|
} else {
|
|
//v=v0.orthogonal().normalized(); w=0;
|
|
v=GetOrthogonal(v0).normalized(); w=0;
|
|
}
|
|
}
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetRotationV0V1(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1) { Quaternion_tpl<F> q; q.SetRotationV0V1(v0,v1); return q; }
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> GetRotationV0V1(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1) { Quaternion_tpl<F> q; q.SetRotationV0V1(v0,v1); return q; }
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* Create rotation-quaternion that around the fixed coordinate axes.
|
|
*
|
|
* Example:
|
|
* Quaternion_tpl<f32> q=GetRotationXYZ( Ang3(1,2,3) );
|
|
* q.SetRotationXYZ( Ang3(1,2,3) );
|
|
*/
|
|
template<class F>
|
|
ILINE void Quaternion_tpl<F>::SetRotationXYZ(const Ang3 &a) {
|
|
F csx[2]; sincos_tpl( a.x*(F)0.5, csx);
|
|
F csy[2]; sincos_tpl( a.y*(F)0.5, csy);
|
|
F csz[2]; sincos_tpl( a.z*(F)0.5, csz);
|
|
w = cx*cy*cz + sx*sy*sz;
|
|
v.x = cz*cy*sx - sz*sy*cx;
|
|
v.y = cz*sy*cx + sz*cy*sx;
|
|
v.z = sz*cy*cx - cz*sy*sx;
|
|
}
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetRotationXYZ(const Ang3 &a) { Quaternion_tpl<F> q; q.SetRotationXYZ(a); return q; }
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> GetRotationXYZ(const Ang3 &a) { Quaternion_tpl<F> q; q.SetRotationXYZ(a); return q; }
|
|
|
|
|
|
|
|
/*!
|
|
* set identity quaternion
|
|
*
|
|
* Example:
|
|
* Quaternion q;
|
|
* q.Identity();
|
|
*/
|
|
template <class F>
|
|
ILINE void Quaternion_tpl<F>::SetIdentity(void) { w=1; v.x=0, v.y=0; v.z=0; }
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetIdentity(void) { return Quaternion_tpl(1,0,0,0); }
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> GetIdentity(void) { return Quaternion_tpl<F>(1,0,0,0); }
|
|
|
|
|
|
/*!
|
|
* normalize quaternion.
|
|
*
|
|
* Example 1:
|
|
* Quaternion q;
|
|
* q.Normalize();
|
|
*
|
|
* Example 2:
|
|
* Quaternion q;
|
|
* Quaternion qn=GetNormalized( q );
|
|
*
|
|
*/
|
|
template <class F>
|
|
ILINE void Quaternion_tpl<F>::Normalize(void) {
|
|
#if !defined(LINUX)
|
|
assert((*this|*this)>0.00001f);
|
|
#endif
|
|
F d = GetLength(*this);
|
|
d=(F)1/d; w*=d; v*=d;
|
|
}
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> Quaternion_tpl<F>::GetNormalized( const Quaternion_tpl<F> &q ) { Quaternion_tpl<F> t=q; t.Normalize(); return t; }
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> GetNormalized( const Quaternion_tpl<F> &q ) { Quaternion_tpl<F> t=q; t.Normalize(); return t; }
|
|
|
|
|
|
/*!
|
|
* get length of quaternion.
|
|
*
|
|
* Example 1:
|
|
* f32 l=GetLength(q);
|
|
*
|
|
*/
|
|
template <class F>
|
|
ILINE F Quaternion_tpl<F>::GetLength( const Quaternion_tpl<F>& q ) { return sqrt_tpl(q|q); }
|
|
template <class F>
|
|
ILINE F GetLength( const Quaternion_tpl<F>& q ) { return sqrt_tpl(q|q); }
|
|
|
|
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* linear-interpolation between quaternions (lerp)
|
|
*
|
|
* Example:
|
|
* CQuaternion result,p,q;
|
|
* result=qlerp( p, q, 0.5f );
|
|
*
|
|
*/
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> qlerp( const Quaternion_tpl<F> &p, const Quaternion_tpl<F> &q, f32 t ) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(p|p)))<0.001); //check if unit-quaternion
|
|
assert((fabs_tpl(1-(q|q)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
return p*(1.0f-t) + q*t;
|
|
}
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* spherical-interpolation between quaternions (geometrical slerp)
|
|
*
|
|
* Example:
|
|
* Quaternion_tpl<f32> result,p,q;
|
|
* result=qslerp_g( p, q, 0.5f );
|
|
*
|
|
*/
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> Slerp( const Quaternion_tpl<F> &p, const Quaternion_tpl<F> &tq, f32 t ) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(p|p)))<0.001); //check if unit-quaternion
|
|
assert((fabs_tpl(1-(tq|tq)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
Quaternion_tpl<F> q=tq;
|
|
|
|
// calculate cosine using the "inner product" between two quaternions: p*q=cos(radiant)
|
|
f32 cosine = (p|q);
|
|
|
|
// Problem 1: given any two quaternions, there exist two possible arcs, along which one can move.
|
|
// One of them goes around the long way and this is the one that we wish to avoid.
|
|
if( cosine < 0.0 ) { cosine=-cosine; q=-q; }
|
|
|
|
//Problem 2: we explore the special cases where the both quaternions are very close together,
|
|
//in which case we approximate using the more economical LERP and avoid "divisions by zero" since sin(Angle) = 0 as Angle=0
|
|
if(cosine>=0.999f) {
|
|
return qlerp(p,q,t);
|
|
} else {
|
|
//perform qslerp_g: because of the "qlerp"-check above, a "division by zero" is not possible
|
|
f32 angle = cry_acosf(cosine);
|
|
f32 isine = 1.0f/sin_tpl(angle);
|
|
f32 scale_0 = sin_tpl((1.0f - t) * angle);
|
|
f32 scale_1 = sin_tpl(t * angle);
|
|
return (p*scale_0 + q*scale_1) * isine;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//! Spherical linear interpolation of quaternions.
|
|
//! @param a Source quaternion.
|
|
//! @param b Target quaternion.
|
|
//! @param t Interpolation time.
|
|
|
|
//! Spherical linear interpolation of quaternions.
|
|
/*template <class F>
|
|
ILINE Quaternion_tpl<F> Slerp(const Quaternion_tpl<F>& a, const Quaternion_tpl<F>& b, f32 t )
|
|
{
|
|
assert((fabs_tpl(1-(a|a)))<0.001); //check if unit-quaternion
|
|
assert((fabs_tpl(1-(b|b)))<0.001); //check if unit-quaternion
|
|
|
|
Quaternion_tpl<F> q;
|
|
|
|
f32 f1,f2; // interpolation coefficions.
|
|
f32 angle; // angle between A and B
|
|
f32 oosin_a, cos_a; // sine, cosine of angle
|
|
|
|
Quaternion_tpl<F> to;
|
|
|
|
cos_a = a|b;
|
|
if (cos_a < 0) {
|
|
cos_a = -cos_a; //????????
|
|
to = -b;
|
|
} else {
|
|
to = b;
|
|
}
|
|
|
|
if (1.0f - fabs(cos_a) < 0.001f) {
|
|
f1 = 1.0f - t;
|
|
f2 = t;
|
|
} else { // normal case
|
|
angle = (f32)acos(cos_a);
|
|
oosin_a = 1.0f / (f32)sin(angle);
|
|
f1 = (f32)sin( (1.0f - t)*angle ) * oosin_a;
|
|
f2 = (f32)sin( t*angle ) * oosin_a;
|
|
}
|
|
|
|
q.w = f1*a.w + f2*to.w;
|
|
q.v = f1*a.v + f2*to.v;
|
|
|
|
return q;
|
|
}*/
|
|
|
|
|
|
|
|
//! squad(p,a,b,q,t) = slerp( slerp(p,q,t),slerp(a,b,t), 2(1-t)t).
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> Squad( const Quaternion_tpl<F> p,const Quaternion_tpl<F> a,const Quaternion_tpl<F> b,const Quaternion_tpl<F> q,f32 t )
|
|
{
|
|
f32 k = 2.0f * (1.0f - t)*t;
|
|
//return long_slerp( long_slerp(p,q,t),long_slerp(a,b,t), k );
|
|
return Slerp( Slerp(p,q,t),Slerp(a,b,t), k );
|
|
}
|
|
|
|
//! Quaternion interpolation for angles > 2PI.
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> SquadRev( f32 angle, // angle of rotation
|
|
const Vec3& axis, // the axis of rotation
|
|
const Quaternion_tpl<F>& p, // start quaternion
|
|
const Quaternion_tpl<F>& a, // start tangent quaternion
|
|
const Quaternion_tpl<F>& b, // end tangent quaternion
|
|
const Quaternion_tpl<F>& q, // end quaternion
|
|
f32 t ) // Time parameter, in range [0,1]
|
|
{
|
|
f32 s,v;
|
|
f32 omega = 0.5f*angle;
|
|
f32 nrevs = 0;
|
|
Quaternion_tpl<F> r,pp,qq;
|
|
|
|
if (omega < (gf_PI - 0.00001f)) {
|
|
return Squad( p,a,b,q,t );
|
|
}
|
|
|
|
while (omega > (gf_PI - 0.00001f)) {
|
|
omega -= gf_PI;
|
|
nrevs += 1.0f;
|
|
}
|
|
if (omega < 0) omega = 0;
|
|
s = t*angle/gf_PI; // 2t(omega+Npi)/pi
|
|
|
|
if (s < 1.0f) {
|
|
pp = p*Quaternion_tpl<f32>(0.0f,axis);//pp = p.Orthog( axis );
|
|
r = Squad(p,a,pp,pp,s); // in first 90 degrees.
|
|
} else {
|
|
v = s + 1.0f - 2.0f*(nrevs+(omega/gf_PI));
|
|
if (v <= 0.0f) {
|
|
// middle part, on great circle(p,q).
|
|
while (s >= 2.0f) s -= 2.0f;
|
|
pp = p*Quaternion_tpl<f32>(0.0f,axis);//pp = p.Orthog(axis);
|
|
r = Slerp(p,pp,s);
|
|
} else {
|
|
// in last 90 degrees.
|
|
qq = -q*Quaternion_tpl<f32>(0.0f,axis);
|
|
r = Squad(qq,qq,b,q,v);
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
|
|
|
|
// Exponent of Quaternion.
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> exp(const Quaternion_tpl<F>& q) {
|
|
double d = sqrt( double(q.v.x*q.v.x + q.v.y*q.v.y + q.v.z*q.v.z) );
|
|
if (d > 1e-4) {
|
|
double m = sin(d)/d;
|
|
return Quaternion_tpl<F>( (F)cos(d),F(q.v.x*m),F(q.v.y*m),F(q.v.z*m));
|
|
} else return Quaternion_tpl<F> (F(1-d*d), F(q.v.x), F(q.v.y), F(q.v.z) ); // cos(d) ~= 1-d^2 when d->0
|
|
}
|
|
|
|
// logarithm of a quaternion, imaginary part (the real part of the logarithm is always 0)
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> log (const Quaternion_tpl<F>& q) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(q|q)))<0.01); //check if unit-quaternion
|
|
#endif
|
|
double d = sqrt( double(q.v.x*q.v.x + q.v.y*q.v.y + q.v.z*q.v.z ));
|
|
if (d > 1e-7) {
|
|
d = atan2 ( d, (double)q.w ) / d;
|
|
return Quaternion_tpl<F>(0, F(q.v.x*d),F(q.v.y*d), F(q.v.z*d) );
|
|
} else return Quaternion_tpl<F>(0,0,0,0);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
//! Logarithm of Quaternion difference.
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> LnDif( const Quaternion_tpl<F> &q1,const Quaternion_tpl<F> &q2 ){
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(q1|q1)))<0.001); //check if unit-quaternion
|
|
assert((fabs_tpl(1-(q2|q2)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
return log(q2/q1);
|
|
//Quaternion_tpl<F> r = q2/q1;
|
|
//return log(r);
|
|
}
|
|
|
|
/*
|
|
//! Compute a, the term used in Boehm-type interpolation.
|
|
//! a[n] = q[n]* qexp(-(1/4)*( ln(qinv(q[n])*q[n+1]) +ln( qinv(q[n])*q[n-1] )))
|
|
template <class F>
|
|
ILINE Quaternion_tpl<F> CompQuatA( const Quaternion_tpl<F>& qprev,const Quaternion_tpl<F>& q,const Quaternion_tpl<F>& qnext ) {
|
|
Quaternion_tpl<F> qm,qp,r;
|
|
qm = LnDif( q,qprev );
|
|
qp = LnDif( q,qnext );
|
|
r = (-0.25f)*(Quaternion_tpl<F>(qm.v.x+qp.v.x,qm.v.y+qp.v.y,qm.v.z+qp.v.z,qm.w+qp.w));
|
|
return q*r.Exp();
|
|
}*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* spherical-interpolation between quaternions (algebraic slerp_a)
|
|
* I have included this function just for the sake of completeness, because
|
|
* its the only useful application to check if exp & log really work.
|
|
* Both slerp-functions are returning the same result.
|
|
*
|
|
* Example:
|
|
* Quaternion result,p,q;
|
|
* result=qslerp_a( p, q, 0.5f );
|
|
*
|
|
*/
|
|
template<class F>
|
|
ILINE Quaternion_tpl<F> qslerp_a( const Quaternion_tpl<F> &p, const Quaternion_tpl<F> &tq, F t ) {
|
|
#if !defined(LINUX)
|
|
assert((fabs_tpl(1-(p|p)))<0.001); //check if unit-quaternion
|
|
assert((fabs_tpl(1-(tq|tq)))<0.001); //check if unit-quaternion
|
|
#endif
|
|
Quaternion_tpl<F> q=tq;
|
|
|
|
// calculate cosine using the "inner product" between two quaternions: p*q=cos(radiant)
|
|
f32 cosine = (p|q);
|
|
|
|
// Problem: given any two quaternions, there exist two possible arcs, along which one can move.
|
|
// One of them goes around the long way and this is the one that we wish to avoid.
|
|
if( cosine < 0.0 ) { cosine=-cosine; q=-q; }
|
|
|
|
//perform qslerp_a:
|
|
return exp( (log (p* !q)) * (1-t)) * q; //algebraic slerp (1)
|
|
|
|
//...and some more slerp-functions producing all the same result
|
|
//return p * exp( (log (invert(p)*q)) * t); //algebraic slerp (2)
|
|
//return exp( (log (q*invert(p))) * t) * p; //algebraic slerp (3)
|
|
//return q * exp( (log (invert(q)*p)) * (1-t)); //algebraic slerp (4)
|
|
}
|
|
|
|
|
|
/*!
|
|
*
|
|
* quaternion copy constructor; Quaternion q=mat33
|
|
* We take only the rotation of the 3x3 part
|
|
*
|
|
* Example 1:
|
|
* \code
|
|
* Matrix33 mat33;
|
|
* mat33.rotationXYZ33(0.5f, 2.5f, 1.5f);
|
|
* Quaternion q=mat33;
|
|
* \endcode
|
|
*
|
|
* Example 2:
|
|
* \code
|
|
* CMatrix34 mat34;
|
|
* mat34.rotationXYZ34(0.5f, 2.5f, 1.5f);
|
|
* Quaternion q=Matrix33(mat34);
|
|
* \endcode
|
|
*/
|
|
template<class F,int SI,int SJ>
|
|
inline Quaternion_tpl<F> GetQuatFromMat33(const Matrix33_tpl<F,SI,SJ>& m) {
|
|
Quaternion_tpl<f32> q;
|
|
//check if we have an orthonormal-base (assuming we are using a right-handed coordinate system)
|
|
//assert removed by ivo: it was impossible to load a level bacause of this asser!
|
|
//but be warned, if you convert a non-uniform-scaled matrix into a quaternion
|
|
//you get a worthless bullshit-quaternion!
|
|
//vlad: still impossible to load
|
|
//assert( IsEquivalent(m.GetOrtX(),m.GetOrtY()%m.GetOrtZ(),0.1f) );
|
|
//assert( IsEquivalent(m.GetOrtY(),m.GetOrtZ()%m.GetOrtX(),0.1f) );
|
|
//assert( IsEquivalent(m.GetOrtZ(),m.GetOrtX()%m.GetOrtY(),0.1f) );
|
|
|
|
F tr,s,p;
|
|
tr = m.M00 + m.M11 + m.M22;
|
|
|
|
//check the diagonal
|
|
if(tr > 0.0) {
|
|
s=sqrt_tpl(tr+1.0f); p=0.5f/s;
|
|
q.w=s*0.5f; q.v((m.M21-m.M12)*p,(m.M02-m.M20)*p,(m.M10-m.M01)*p); return q;
|
|
}
|
|
//diagonal is negativ. now we have to find the biggest element on the diagonal
|
|
//check if "m00" is the biggest element
|
|
if ( (m.M00>=m.M11) && (m.M00>=m.M22) ) {
|
|
s=p=sqrt_tpl(m.M00-m.M11-m.M22+1.0f); if (s) { p=0.5f/s; }
|
|
q.w=(m.M21-m.M12)*p; q.v(s*0.5f,(m.M10+m.M01)*p,(m.M20+m.M02)*p); return q;
|
|
}
|
|
//check if "m11" is the biggest element
|
|
if ( (m.M11>=m.M00) && (m.M11>=m.M22) ) {
|
|
s=p=sqrt_tpl(m.M11-m.M22-m.M00+1.0f); if (s) { p=0.5f/s; }
|
|
q.w=(m.M02-m.M20)*p; q.v((m.M01+m.M10)*p,s*0.5f,(m.M21+m.M12)*p); return q;
|
|
}
|
|
//check if "m22" is the biggest element
|
|
if ( (m.M22>=m.M00) && (m.M22>=m.M11) ) {
|
|
s=p=sqrt_tpl(m.M22-m.M00-m.M11+1.0f); if (s) { p=0.5f/s; }
|
|
q.w=(m.M10-m.M01)*p; q.v((m.M02+m.M20)*p,(m.M12+m.M21)*p,s*0.5f); return q;
|
|
}
|
|
#if !defined(LINUX)
|
|
assert(0);
|
|
#endif
|
|
return q;//if it ends here, then we have no valid rotation matrix
|
|
}
|
|
|
|
|
|
#endif // _Quaternion_tpl_H
|