Removed unnecessary files.
This commit is contained in:
curt
@@ -1,136 +0,0 @@
|
|||||||
//------------------------------------------------------------------------------
|
|
||||||
// File : SkyRenderableInstanceGeneric.cpp
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// SkyWorks : Copyright 2002 Mark J. Harris and
|
|
||||||
// The University of North Carolina at Chapel Hill
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Permission to use, copy, modify, distribute and sell this software and its
|
|
||||||
// documentation for any purpose is hereby granted without fee, provided that
|
|
||||||
// the above copyright notice appear in all copies and that both that copyright
|
|
||||||
// notice and this permission notice appear in supporting documentation.
|
|
||||||
// Binaries may be compiled with this software without any royalties or
|
|
||||||
// restrictions.
|
|
||||||
//
|
|
||||||
// The author(s) and The University of North Carolina at Chapel Hill make no
|
|
||||||
// representations about the suitability of this software for any purpose.
|
|
||||||
// It is provided "as is" without express or
|
|
||||||
// implied warranty.
|
|
||||||
/**
|
|
||||||
* @file SkyRenderableInstanceGeneric.cpp
|
|
||||||
*
|
|
||||||
* A basic implementation of SkyRenderableInstance
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "glvu.hpp"
|
|
||||||
#include "SkyUtil.hpp"
|
|
||||||
#include "SkyRenderable.hpp"
|
|
||||||
#include "SkyMinMaxBox.hpp"
|
|
||||||
#include "SkyRenderableInstanceGeneric.hpp"
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric(SkyRenderable *object)
|
|
||||||
* @brief Constructor, store the renderable and set the position to identity
|
|
||||||
*/
|
|
||||||
SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric(SkyRenderable *pObject)
|
|
||||||
: SkyRenderableInstance(),
|
|
||||||
_pObj(pObject)
|
|
||||||
{
|
|
||||||
_pBV = pObject->CopyBoundingVolume();
|
|
||||||
}
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric(SkyRenderable *object, const Vec3f &position, const Mat33f &rotation, const float scale)
|
|
||||||
* @brief Constructor, stores the instance information given
|
|
||||||
*/
|
|
||||||
SkyRenderableInstanceGeneric::SkyRenderableInstanceGeneric(SkyRenderable *pObject,
|
|
||||||
const Vec3f &position,
|
|
||||||
const Mat33f &rotation,
|
|
||||||
const float scale)
|
|
||||||
: SkyRenderableInstance(position, rotation, scale),
|
|
||||||
_pObj(pObject)
|
|
||||||
{
|
|
||||||
_pBV = pObject->CopyBoundingVolume();
|
|
||||||
}
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::~SkyRenderableInstanceGeneric
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::~SkyRenderableInstanceGeneric()
|
|
||||||
* @brief Destructor
|
|
||||||
*/
|
|
||||||
SkyRenderableInstanceGeneric::~SkyRenderableInstanceGeneric()
|
|
||||||
{
|
|
||||||
_pObj = NULL;
|
|
||||||
SAFE_DELETE(_pBV);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::SetRenderable
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::SetRenderable(SkyRenderable *pRenderable)
|
|
||||||
* @brief Set the renderable for this instance.
|
|
||||||
*/
|
|
||||||
void SkyRenderableInstanceGeneric::SetRenderable(SkyRenderable *pRenderable)
|
|
||||||
{
|
|
||||||
_pObj = pRenderable;
|
|
||||||
SAFE_DELETE(_pBV);
|
|
||||||
_pBV = pRenderable->CopyBoundingVolume();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::Display
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::Display()
|
|
||||||
* @brief Displays the instance by calling the renderable's display function
|
|
||||||
*/
|
|
||||||
SKYRESULT SkyRenderableInstanceGeneric::Display()
|
|
||||||
{
|
|
||||||
// Get and set the world space transformation
|
|
||||||
Mat44f mat;
|
|
||||||
GetModelToWorldTransform(mat);
|
|
||||||
|
|
||||||
glMatrixMode(GL_MODELVIEW);
|
|
||||||
glPushMatrix();
|
|
||||||
glMultMatrixf(mat.M);
|
|
||||||
|
|
||||||
//FAIL_RETURN_MSG(_pObj->Display(*(GLVU::GetCurrent()->GetCurrentCam()), this),
|
|
||||||
// "SkyRenderableInstanceGeneric:Display(), error returned from object's display");
|
|
||||||
|
|
||||||
glMatrixMode(GL_MODELVIEW);
|
|
||||||
glPopMatrix();
|
|
||||||
|
|
||||||
return SKYRESULT_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Function : SkyRenderableInstanceGeneric::ViewFrustumCull
|
|
||||||
// Description :
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* @fn SkyRenderableInstanceGeneric::ViewFrustumCull(const Camera &cam)
|
|
||||||
* @brief View frustum cull the object given its world position
|
|
||||||
*/
|
|
||||||
bool SkyRenderableInstanceGeneric::ViewFrustumCull(const Camera &cam)
|
|
||||||
{
|
|
||||||
Mat44f xform;
|
|
||||||
GetModelToWorldTransform(xform);
|
|
||||||
_bCulled = (_pBV == NULL) ? false : _pBV->ViewFrustumCull(cam, xform);
|
|
||||||
return _bCulled;
|
|
||||||
}
|
|
||||||
@@ -1,166 +0,0 @@
|
|||||||
//------------------------------------------------------------------------------
|
|
||||||
// File : quat.hpp
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// GLVU : Copyright 1997 - 2002
|
|
||||||
// The University of North Carolina at Chapel Hill
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Permission to use, copy, modify, distribute and sell this software and its
|
|
||||||
// documentation for any purpose is hereby granted without fee, provided that
|
|
||||||
// the above copyright notice appear in all copies and that both that copyright
|
|
||||||
// notice and this permission notice appear in supporting documentation.
|
|
||||||
// Binaries may be compiled with this software without any royalties or
|
|
||||||
// restrictions.
|
|
||||||
//
|
|
||||||
// The University of North Carolina at Chapel Hill makes no representations
|
|
||||||
// about the suitability of this software for any purpose. It is provided
|
|
||||||
// "as is" without express or implied warranty.
|
|
||||||
|
|
||||||
/**************************************************************************
|
|
||||||
|
|
||||||
quat.hpp
|
|
||||||
|
|
||||||
A quaternion template class
|
|
||||||
|
|
||||||
---------------------------------------------------------------------
|
|
||||||
|
|
||||||
Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
|
|
||||||
|
|
||||||
Modification History:
|
|
||||||
- Oct 2000, Bill Baxter. Adapted to GLVU coding conventions and
|
|
||||||
templetized. Also implemented many methods that were declared but
|
|
||||||
not defined. Added some methods from quatlib and other sources.
|
|
||||||
|
|
||||||
**************************************************************************/
|
|
||||||
|
|
||||||
#ifndef _QUAT_H_
|
|
||||||
#define _QUAT_H_
|
|
||||||
|
|
||||||
#include "vec3f.hpp"
|
|
||||||
#include "mat44.hpp"
|
|
||||||
#include "mat33.hpp"
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
* Quaternion *
|
|
||||||
****************************************************************/
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
class Quat
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
typedef Vec3<Type> qvec;
|
|
||||||
|
|
||||||
Type s; /* scalar component */
|
|
||||||
qvec v; /* quat vector component */
|
|
||||||
|
|
||||||
/* Constructors */
|
|
||||||
|
|
||||||
Quat(void);
|
|
||||||
Quat(Type s, Type x, Type y, Type z);
|
|
||||||
Quat(Type x, Type y, Type z); // s=0
|
|
||||||
Quat(Type s, const qvec& v);
|
|
||||||
Quat(const qvec& v, Type s = 0.0);
|
|
||||||
Quat(const Type *d); /* copy from four-element Type array s,x,y,z */
|
|
||||||
Quat(const Quat &q); /* copy from other Quat */
|
|
||||||
|
|
||||||
/* Setters */
|
|
||||||
|
|
||||||
void Set( Type x, Type y, Type z );
|
|
||||||
void Set( Type s, Type x, Type y, Type z );
|
|
||||||
void Set( Type s, const qvec& v );
|
|
||||||
void Set( const qvec& v, Type s=0 );
|
|
||||||
|
|
||||||
/* Operators */
|
|
||||||
|
|
||||||
Quat &operator = ( const Quat &v ); /* assignment of a Quat */
|
|
||||||
Quat &operator += ( const Quat &v ); /* incrementation by a Quat */
|
|
||||||
Quat &operator -= ( const Quat &v ); /* decrementation by a Quat */
|
|
||||||
Quat &operator *= ( const Type d ); /* multiplication by a scalar */
|
|
||||||
Quat &operator *= ( const Quat &v ); /* quat product (this*v) */
|
|
||||||
Quat &operator /= ( const Type d ); /* division by a scalar */
|
|
||||||
Type &operator [] ( int i); /* indexing s=0,x=1,y=2,z=3 */
|
|
||||||
|
|
||||||
/* special functions */
|
|
||||||
|
|
||||||
Type Length(void) const; /* length of a Quat */
|
|
||||||
Type LengthSqr(void) const; /* squared length of a Quat */
|
|
||||||
Type Norm(void) const; /* also squared length of a Quat */
|
|
||||||
Quat &Normalize(void); /* normalize a Quat */
|
|
||||||
Quat &Invert(void); /* q = q^-1 */
|
|
||||||
Quat &Conjugate(void); /* q = q* */
|
|
||||||
qvec Xform( const qvec &v ) const; /* q*v*q-1 */
|
|
||||||
Quat &Log(void); /* log(q) */
|
|
||||||
Quat &Exp(void); /* exp(q) */
|
|
||||||
qvec GetAxis( void ) const; /* Get rot axis */
|
|
||||||
Type GetAngle( void ) const; /* Get rot angle (radians) */
|
|
||||||
void SetAngle( Type rad_ang ); /* set rot angle (radians) */
|
|
||||||
void ScaleAngle( Type f ); /* scale rot angle */
|
|
||||||
void Print( ) const; /* print Quat */
|
|
||||||
|
|
||||||
/* Conversions */
|
|
||||||
Mat44<Type>& ToMat( Mat44<Type> &dest ) const; /* to 4x4 matrix */
|
|
||||||
Mat33<Type>& ToMat( Mat33<Type> &dest ) const; /* to 3x3 matrix */
|
|
||||||
Quat& FromMat( const Mat44<Type>& src ); /* from 4x4 rot matrix */
|
|
||||||
Quat& FromMat( const Mat33<Type>& src ); /* from 3x3 rot matrix */
|
|
||||||
|
|
||||||
void ToAngleAxis( Type &ang, qvec &ax ) const; /* to rot angle AND axis */
|
|
||||||
Quat& FromAngleAxis( Type ang, const qvec &ax );/*from rot angle AND axis */
|
|
||||||
Quat& FromTwoVecs(const qvec &a, const qvec& b); /* quat from a to b */
|
|
||||||
// to/from Euler Angles (XYZ-Fixed/ZYX-Relative, angles in radians)
|
|
||||||
// See quatimpl.hpp for more detailed comments.
|
|
||||||
Quat& FromEuler( Type yaw_Z, Type pitch_Y, Type roll_X);
|
|
||||||
void ToEuler(Type &yaw_Z, Type &pitch_Y, Type &roll_X) const;
|
|
||||||
|
|
||||||
|
|
||||||
// HELPERS
|
|
||||||
static Type DEG2RAD(Type d);
|
|
||||||
static Type RAD2DEG(Type d);
|
|
||||||
static Type Sin(double d);
|
|
||||||
static Type Cos(double d);
|
|
||||||
static Type ACos(double d);
|
|
||||||
static Type ASin(double d);
|
|
||||||
static Type ATan(double d);
|
|
||||||
static Type ATan2(double n, double d);
|
|
||||||
|
|
||||||
// CONSTANTS
|
|
||||||
static Type FUDGE();
|
|
||||||
static Quat ZERO();
|
|
||||||
static Quat IDENTITY();
|
|
||||||
};
|
|
||||||
|
|
||||||
/* Utility functions */
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& QuatSlerp(
|
|
||||||
Quat<Type> &dest, const Quat<Type>& from, const Quat<Type>& to, Type t );
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> QuatSlerp(const Quat<Type>& from, const Quat<Type>& to, Type t );
|
|
||||||
|
|
||||||
/* "Friends" */
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator -(const Quat<Type> &v); // -q1
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator +(const Quat<Type> &a, const Quat<Type> &b); // q1 + q2
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator -(const Quat<Type> &a, const Quat<Type> &b); // q1 - q2
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator *(const Quat<Type> &a, const Type d); // q1 * 3.0
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator *(const Type d, const Quat<Type> &a); // 3.0 * q1
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator *(const Quat<Type> &a, const Quat<Type> &b); // q1 * q2
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator /(const Quat<Type> &a, const Type d); // q1 / 3.0
|
|
||||||
template <class Type>
|
|
||||||
bool operator ==(const Quat<Type> &a, const Quat<Type> &b); // q1 == q2 ?
|
|
||||||
template <class Type>
|
|
||||||
bool operator !=(const Quat<Type> &a, const Quat<Type> &b); // q1 != q2 ?
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#include "quatimpl.hpp"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
typedef Quat<float> Quatf;
|
|
||||||
typedef Quat<double> Quatd;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
@@ -1,886 +0,0 @@
|
|||||||
//------------------------------------------------------------------------------
|
|
||||||
// File : quatimpl.hpp
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// GLVU : Copyright 1997 - 2002
|
|
||||||
// The University of North Carolina at Chapel Hill
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Permission to use, copy, modify, distribute and sell this software and its
|
|
||||||
// documentation for any purpose is hereby granted without fee, provided that
|
|
||||||
// the above copyright notice appear in all copies and that both that copyright
|
|
||||||
// notice and this permission notice appear in supporting documentation.
|
|
||||||
// Binaries may be compiled with this software without any royalties or
|
|
||||||
// restrictions.
|
|
||||||
//
|
|
||||||
// The University of North Carolina at Chapel Hill makes no representations
|
|
||||||
// about the suitability of this software for any purpose. It is provided
|
|
||||||
// "as is" without express or implied warranty.
|
|
||||||
|
|
||||||
/***********************************************************************
|
|
||||||
|
|
||||||
quatimpl.hpp
|
|
||||||
|
|
||||||
A quaternion template class
|
|
||||||
|
|
||||||
-------------------------------------------------------------------
|
|
||||||
|
|
||||||
Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
|
|
||||||
Oct 2000, Bill Baxter
|
|
||||||
|
|
||||||
Modification History:
|
|
||||||
- See main header file, quat.hpp
|
|
||||||
|
|
||||||
************************************************************************/
|
|
||||||
|
|
||||||
#include "quat.hpp"
|
|
||||||
#include <math.h>
|
|
||||||
#include <assert.h>
|
|
||||||
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// CONSTRUCTORS
|
|
||||||
//============================================================================
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( void )
|
|
||||||
{
|
|
||||||
// do nothing so default construction is fast
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( Type _s, Type x, Type y, Type z )
|
|
||||||
{
|
|
||||||
s = _s;
|
|
||||||
v.Set( x, y, z );
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( Type x, Type y, Type z )
|
|
||||||
{
|
|
||||||
s = 0.0;
|
|
||||||
v.Set( x, y, z );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( const qvec& _v, Type _s )
|
|
||||||
{
|
|
||||||
Set( _v, _s );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( Type _s, const qvec& _v )
|
|
||||||
{
|
|
||||||
Set( _v, _s );
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( const Type *d )
|
|
||||||
{
|
|
||||||
s = *d++;
|
|
||||||
v.Set(d);
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::Quat( const Quat &q )
|
|
||||||
{
|
|
||||||
s = q.s;
|
|
||||||
v = q.v;
|
|
||||||
}
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// SETTERS
|
|
||||||
//============================================================================
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::Set( Type _s, Type x, Type y, Type z )
|
|
||||||
{
|
|
||||||
s = _s;
|
|
||||||
v.Set(x,y,z);
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::Set( Type x, Type y, Type z )
|
|
||||||
{
|
|
||||||
s = 0.0;
|
|
||||||
v.Set(x,y,z);
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::Set( const qvec& _v, Type _s )
|
|
||||||
{
|
|
||||||
s = _s;
|
|
||||||
v = _v;
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::Set( Type _s, const qvec& _v )
|
|
||||||
{
|
|
||||||
s = _s;
|
|
||||||
v = _v;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// OPERATORS
|
|
||||||
//============================================================================
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::operator = (const Quat& q)
|
|
||||||
{
|
|
||||||
v = q.v; s = q.s; return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::operator += ( const Quat &q )
|
|
||||||
{
|
|
||||||
v += q.v; s += q.s; return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::operator -= ( const Quat &q )
|
|
||||||
{
|
|
||||||
v -= q.v; s -= q.s; return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> &Quat<Type>::operator *= ( const Type d )
|
|
||||||
{
|
|
||||||
v *= d; s *= d; return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> &Quat<Type>::operator *= ( const Quat& q )
|
|
||||||
{
|
|
||||||
#if 0
|
|
||||||
// Quaternion multiplication with
|
|
||||||
// temporary object construction minimized (hopefully)
|
|
||||||
Type ns = s*q.s - v*q.v;
|
|
||||||
qvec nv(v^q.v);
|
|
||||||
v *= q.s;
|
|
||||||
v += nv;
|
|
||||||
nv.Set(s*q.v);
|
|
||||||
v += nv;
|
|
||||||
s = ns;
|
|
||||||
return *this;
|
|
||||||
#else
|
|
||||||
// optimized (12 mults, and no compiler-generated temp objects)
|
|
||||||
Type A, B, C, D, E, F, G, H;
|
|
||||||
|
|
||||||
A = (s + v.x)*(q.s + q.v.x);
|
|
||||||
B = (v.z - v.y)*(q.v.y - q.v.z);
|
|
||||||
C = (s - v.x)*(q.v.y + q.v.z);
|
|
||||||
D = (v.y + v.z)*(q.s - q.v.x);
|
|
||||||
E = (v.x + v.z)*(q.v.x + q.v.y);
|
|
||||||
F = (v.x - v.z)*(q.v.x - q.v.y);
|
|
||||||
G = (s + v.y)*(q.s - q.v.z);
|
|
||||||
H = (s - v.y)*(q.s + q.v.z);
|
|
||||||
|
|
||||||
v.x = A - (E + F + G + H) * Type(0.5);
|
|
||||||
v.y = C + (E - F + G - H) * Type(0.5);
|
|
||||||
v.z = D + (E - F - G + H) * Type(0.5);
|
|
||||||
s = B + (-E - F + G + H) * Type(0.5);
|
|
||||||
|
|
||||||
return *this;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> &Quat<Type>::operator /= ( const Type d )
|
|
||||||
{
|
|
||||||
Type r = Type(1.0)/d;
|
|
||||||
v *= r;
|
|
||||||
s *= r;
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Type &Quat<Type>::operator [] ( int i)
|
|
||||||
{
|
|
||||||
switch (i) {
|
|
||||||
case 0: return s;
|
|
||||||
case 1: return v.x;
|
|
||||||
case 2: return v.y;
|
|
||||||
case 3: return v.z;
|
|
||||||
}
|
|
||||||
assert(false);
|
|
||||||
return s;
|
|
||||||
}
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// SPECIAL FUNCTIONS
|
|
||||||
//============================================================================
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::Length( void ) const
|
|
||||||
{
|
|
||||||
return Type( sqrt( v*v + s*s ) );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::LengthSqr( void ) const
|
|
||||||
{
|
|
||||||
return Norm();
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::Norm( void ) const
|
|
||||||
{
|
|
||||||
return v*v + s*s;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type>& Quat<Type>::Normalize( void )
|
|
||||||
{
|
|
||||||
*this *= Type(1.0) / Type(sqrt(v*v + s*s));
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type>& Quat<Type>::Invert( void )
|
|
||||||
{
|
|
||||||
Type scale = Type(1.0)/Norm();
|
|
||||||
v *= -scale;
|
|
||||||
s *= scale;
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type>& Quat<Type>::Conjugate( void )
|
|
||||||
{
|
|
||||||
v.x = -v.x;
|
|
||||||
v.y = -v.y;
|
|
||||||
v.z = -v.z;
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Xform
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Transform a vector by this quaternion using q * v * q^-1
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::qvec Quat<Type>::Xform( const qvec &vec ) const
|
|
||||||
{
|
|
||||||
/* copy vector into temp quaternion for multiply */
|
|
||||||
Quat vecQuat(vec);
|
|
||||||
/* invert multiplier */
|
|
||||||
Quat inverse(*this);
|
|
||||||
inverse.Invert();
|
|
||||||
|
|
||||||
/* do q * vec * q(inv) */
|
|
||||||
Quat tempVecQuat(*this * vecQuat);
|
|
||||||
tempVecQuat *= inverse;
|
|
||||||
|
|
||||||
/* return vector part */
|
|
||||||
return tempVecQuat.v;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Log
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Natural log of quat
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> &Quat<Type>::Log(void)
|
|
||||||
{
|
|
||||||
Type theta, scale;
|
|
||||||
|
|
||||||
scale = v.Length();
|
|
||||||
theta = ATan2(scale, s);
|
|
||||||
|
|
||||||
if (scale > 0.0)
|
|
||||||
scale = theta/scale;
|
|
||||||
|
|
||||||
v *= scale;
|
|
||||||
s = 0.0;
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Exp
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// e to the quat: e^quat
|
|
||||||
// -- assuming scalar part 0
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> &Quat<Type>::Exp(void)
|
|
||||||
{
|
|
||||||
Type scale;
|
|
||||||
Type theta = v.Length();
|
|
||||||
|
|
||||||
if (theta > FUDGE()) {
|
|
||||||
scale = Sin(theta)/theta ;
|
|
||||||
v *= scale;
|
|
||||||
}
|
|
||||||
|
|
||||||
s = Cos(theta) ;
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// SetAngle (radians)
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::SetAngle( Type f )
|
|
||||||
{
|
|
||||||
qvec axis(GetAxis());
|
|
||||||
f *= Type(0.5);
|
|
||||||
s = Cos( f );
|
|
||||||
v = axis * Sin( f );
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// ScaleAngle
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
inline void Quat<Type>::ScaleAngle( Type f )
|
|
||||||
{
|
|
||||||
SetAngle( f * GetAngle() );
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// GetAngle (radians)
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// get rot angle in radians. Assumes s is between -1 and 1, which will always
|
|
||||||
// be the case for unit quaternions.
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::GetAngle( void ) const
|
|
||||||
{
|
|
||||||
return ( Type(2.0) * ACos( s ) );
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// GetAxis
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>::qvec Quat<Type>::GetAxis( void ) const
|
|
||||||
{
|
|
||||||
Type scale;
|
|
||||||
|
|
||||||
scale = Sin( acos( s ) ) ;
|
|
||||||
if ( scale < FUDGE() && scale > -FUDGE() )
|
|
||||||
return qvec( 0.0, 0.0, 0.0 );
|
|
||||||
else
|
|
||||||
return v / scale;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Print
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
inline void Quat<Type>::Print( ) const
|
|
||||||
{
|
|
||||||
printf( "(%3.2f, <%3.2f %3.2f %3.2f>)\n", s, v.x, v.y, v.z );
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// CONVERSIONS
|
|
||||||
//============================================================================
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Mat44<Type>& Quat<Type>::ToMat( Mat44<Type>& dest ) const
|
|
||||||
{
|
|
||||||
Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
||||||
qvec a, c, b, d;
|
|
||||||
|
|
||||||
t = Type(2.0) / (v*v + s*s);
|
|
||||||
const Type ONE(1.0);
|
|
||||||
|
|
||||||
xs = v.x*t; ys = v.y*t; zs = v.z*t;
|
|
||||||
wx = s*xs; wy = s*ys; wz = s*zs;
|
|
||||||
xx = v.x*xs; xy = v.x*ys; xz = v.x*zs;
|
|
||||||
yy = v.y*ys; yz = v.y*zs; zz = v.z*zs;
|
|
||||||
|
|
||||||
dest.Set( ONE-(yy+zz), xy-wz, xz+wy, 0.0,
|
|
||||||
xy+wz, ONE-(xx+zz), yz-wx, 0.0,
|
|
||||||
xz-wy, yz+wx, ONE-(xx+yy), 0.0,
|
|
||||||
0.0, 0.0, 0.0, ONE );
|
|
||||||
|
|
||||||
return dest;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Mat33<Type>& Quat<Type>::ToMat( Mat33<Type>& dest ) const
|
|
||||||
{
|
|
||||||
Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
||||||
qvec a, c, b, d;
|
|
||||||
|
|
||||||
t = Type(2.0) / Norm();
|
|
||||||
const Type ONE(1.0);
|
|
||||||
|
|
||||||
xs = v.x*t; ys = v.y*t; zs = v.z*t;
|
|
||||||
wx = s*xs; wy = s*ys; wz = s*zs;
|
|
||||||
xx = v.x*xs; xy = v.x*ys; xz = v.x*zs;
|
|
||||||
yy = v.y*ys; yz = v.y*zs; zz = v.z*zs;
|
|
||||||
|
|
||||||
dest.Set( ONE-(yy+zz), xy-wz, xz+wy,
|
|
||||||
xy+wz, ONE-(xx+zz), yz-wx,
|
|
||||||
xz-wy, yz+wx, ONE-(xx+yy) );
|
|
||||||
|
|
||||||
return dest;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// FromMat
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Convert rotation matrix to quaternion
|
|
||||||
// Results will be bad if matrix is not (very close to) orthonormal
|
|
||||||
// Modified from gamasutra.com article:
|
|
||||||
// http://www.gamasutra.com/features/programming/19980703/quaternions_07.htm
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::FromMat( const Mat44<Type>& m )
|
|
||||||
{
|
|
||||||
Type tr = m.Trace();
|
|
||||||
|
|
||||||
// check the diagonal
|
|
||||||
if (tr > 0.0) {
|
|
||||||
Type scale = Type( sqrt (tr) );
|
|
||||||
s = scale * Type(0.5);
|
|
||||||
scale = Type(0.5) / scale;
|
|
||||||
v.x = (m(1,2) - m(2,1)) * scale;
|
|
||||||
v.y = (m(2,0) - m(0,2)) * scale;
|
|
||||||
v.z = (m(0,1) - m(1,0)) * scale;
|
|
||||||
} else {
|
|
||||||
// diagonal is negative or zero
|
|
||||||
int i, j, k;
|
|
||||||
i = 0;
|
|
||||||
if (m(1,1) > m(0,0)) i = 1;
|
|
||||||
if (m(2,2) > m(i,i)) i = 2;
|
|
||||||
int nxt[3] = {1, 2, 0};
|
|
||||||
j = nxt[i];
|
|
||||||
k = nxt[j];
|
|
||||||
|
|
||||||
Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
|
|
||||||
|
|
||||||
v[i] = scale * Type(0.5);
|
|
||||||
|
|
||||||
if (scale != 0.0) scale = Type(0.5) / scale;
|
|
||||||
|
|
||||||
s = (m(j,k) - m(k,j)) * scale;
|
|
||||||
v[j] = (m(i,j) + m(j,i)) * scale;
|
|
||||||
v[k] = (m(i,k) + m(k,i)) * scale;
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::FromMat( const Mat33<Type>& m )
|
|
||||||
{
|
|
||||||
Type tr = m.Trace();
|
|
||||||
|
|
||||||
// check the diagonal
|
|
||||||
if (tr > 0.0) {
|
|
||||||
Type scale = Type( sqrt (tr + Type(1.0)) );
|
|
||||||
s = scale * Type(0.5);
|
|
||||||
scale = Type(0.5) / scale;
|
|
||||||
v.x = (m(1,2) - m(2,1)) * scale;
|
|
||||||
v.y = (m(2,0) - m(0,2)) * scale;
|
|
||||||
v.z = (m(0,1) - m(1,0)) * scale;
|
|
||||||
} else {
|
|
||||||
// diagonal is negative or zero
|
|
||||||
int i, j, k;
|
|
||||||
i = 0;
|
|
||||||
if (m(1,1) > m(0,0)) i = 1;
|
|
||||||
if (m(2,2) > m(i,i)) i = 2;
|
|
||||||
int nxt[3] = {1, 2, 0};
|
|
||||||
j = nxt[i];
|
|
||||||
k = nxt[j];
|
|
||||||
|
|
||||||
Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
|
|
||||||
|
|
||||||
v[i] = scale * Type(0.5);
|
|
||||||
|
|
||||||
if (scale != 0.0) scale = Type(0.5) / scale;
|
|
||||||
|
|
||||||
s = (m(j,k) - m(k,j)) * scale;
|
|
||||||
v[j] = (m(i,j) + m(j,i)) * scale;
|
|
||||||
v[k] = (m(i,k) + m(k,i)) * scale;
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// ToAngleAxis (radians)
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Convert to angle & axis representation
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::ToAngleAxis( Type &angle, qvec &axis ) const
|
|
||||||
{
|
|
||||||
Type cinv = ACos( s );
|
|
||||||
angle = Type(2.0) * cinv;
|
|
||||||
|
|
||||||
Type scale;
|
|
||||||
|
|
||||||
scale = Sin( cinv );
|
|
||||||
if ( scale < FUDGE() && scale > -FUDGE() )
|
|
||||||
axis = qvec::ZERO;
|
|
||||||
else {
|
|
||||||
axis = v;
|
|
||||||
axis /= scale;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// FromAngleAxis (radians)
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Convert to quat from angle & axis representation
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::FromAngleAxis( Type angle, const qvec &axis )
|
|
||||||
{
|
|
||||||
/* normalize vector */
|
|
||||||
Type length = axis.Length();
|
|
||||||
|
|
||||||
/* if zero vector passed in, just set to identity quaternion */
|
|
||||||
if ( length < FUDGE() )
|
|
||||||
{
|
|
||||||
*this = IDENTITY();
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
length = Type(1.0)/length;
|
|
||||||
angle *= 0.5;
|
|
||||||
v = axis;
|
|
||||||
v *= length;
|
|
||||||
v *= Sin(angle);
|
|
||||||
|
|
||||||
s = Cos(angle);
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// FromTwoVecs
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Return the quat that rotates vector a into vector b
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::FromTwoVecs(const qvec &a, const qvec& b)
|
|
||||||
{
|
|
||||||
qvec u1(a);
|
|
||||||
qvec u2(b);
|
|
||||||
double theta ; /* angle of rotation about axis */
|
|
||||||
double theta_complement ;
|
|
||||||
double crossProductMagnitude ;
|
|
||||||
|
|
||||||
|
|
||||||
// Normalize both vectors and take cross product to get rotation axis.
|
|
||||||
u1.Normalize();
|
|
||||||
u2.Normalize();
|
|
||||||
qvec axis( u1 ^ u2 );
|
|
||||||
|
|
||||||
|
|
||||||
// | u1 X u2 | = |u1||u2|sin(theta)
|
|
||||||
//
|
|
||||||
// Since u1 and u2 are normalized,
|
|
||||||
//
|
|
||||||
// theta = arcsin(|axis|)
|
|
||||||
crossProductMagnitude = axis.Length();
|
|
||||||
|
|
||||||
// Occasionally, even though the vectors are normalized, the
|
|
||||||
// magnitude will be calculated to be slightly greater than one. If
|
|
||||||
// this happens, just set it to 1 or asin() will barf.
|
|
||||||
if( crossProductMagnitude > Type(1.0) )
|
|
||||||
crossProductMagnitude = Type(1.0) ;
|
|
||||||
|
|
||||||
// Take arcsin of magnitude of rotation axis to compute rotation
|
|
||||||
// angle. Since crossProductMagnitude=[0,1], we will have
|
|
||||||
// theta=[0,pi/2].
|
|
||||||
theta = ASin( crossProductMagnitude ) ;
|
|
||||||
theta_complement = Type(3.14159265358979323846) - theta ;
|
|
||||||
|
|
||||||
// If cos(theta) < 0, use complement of theta as rotation angle.
|
|
||||||
if( u1 * u2 < 0.0 )
|
|
||||||
{
|
|
||||||
double tmp = theta;
|
|
||||||
theta = theta_complement ;
|
|
||||||
theta_complement = tmp;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if angle is 0, just return identity quaternion
|
|
||||||
if( theta < FUDGE() )
|
|
||||||
{
|
|
||||||
*this = IDENTITY();
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if( theta_complement < FUDGE() )
|
|
||||||
{
|
|
||||||
// The two vectors are opposed. Find some arbitrary axis vector.
|
|
||||||
// First try cross product with x-axis if u1 not parallel to x-axis.
|
|
||||||
if( (u1.y*u1.y + u1.z*u1.z) >= FUDGE() )
|
|
||||||
{
|
|
||||||
axis.Set( 0.0, u1.z, -u1.y ) ;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// u1 is parallel to to x-axis. Use z-axis as axis of rotation.
|
|
||||||
axis.Set(0.0, 0.0, 1.0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
axis.Normalize();
|
|
||||||
FromAngleAxis(Type(theta), axis);
|
|
||||||
Normalize();
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// FromEuler
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// converts 3 euler angles (in radians) to a quaternion
|
|
||||||
//
|
|
||||||
// angles are in radians; Assumes roll is rotation about X, pitch is
|
|
||||||
// rotation about Y, yaw is about Z. (So thinking of
|
|
||||||
// Z as up) Assumes order of yaw, pitch, roll applied as follows:
|
|
||||||
//
|
|
||||||
// p' = roll( pitch( yaw(p) ) )
|
|
||||||
//
|
|
||||||
// Where yaw, pitch, and roll are defined in the BODY coordinate sys.
|
|
||||||
// In other words these are ZYX-relative (or XYZ-fixed) Euler Angles.
|
|
||||||
//
|
|
||||||
// For a complete Euler angle implementation that handles all 24 angle
|
|
||||||
// sets, see "Euler Angle Conversion" by Ken Shoemake, in "Graphics
|
|
||||||
// Gems IV", Academic Press, 1994
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& Quat<Type>::FromEuler(Type yaw, Type pitch, Type roll)
|
|
||||||
{
|
|
||||||
Type cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
|
|
||||||
Type half_roll, half_pitch, half_yaw;
|
|
||||||
|
|
||||||
/* put angles into radians and divide by two, since all angles in formula
|
|
||||||
* are (angle/2)
|
|
||||||
*/
|
|
||||||
const Type HALF(0.5);
|
|
||||||
half_yaw = yaw * HALF;
|
|
||||||
half_pitch = pitch * HALF;
|
|
||||||
half_roll = roll * HALF;
|
|
||||||
|
|
||||||
cosYaw = Cos(half_yaw);
|
|
||||||
sinYaw = Sin(half_yaw);
|
|
||||||
|
|
||||||
cosPitch = Cos(half_pitch);
|
|
||||||
sinPitch = Sin(half_pitch);
|
|
||||||
|
|
||||||
cosRoll = Cos(half_roll);
|
|
||||||
sinRoll = Sin(half_roll);
|
|
||||||
|
|
||||||
Type cpcy = cosPitch * cosYaw;
|
|
||||||
Type spsy = sinPitch * sinYaw;
|
|
||||||
|
|
||||||
v.x = sinRoll * cpcy - cosRoll * spsy;
|
|
||||||
v.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
|
||||||
v.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
|
||||||
s = cosRoll * cpcy + sinRoll * spsy;
|
|
||||||
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// ToEuler
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// converts a quaternion to 3 euler angles (in radians)
|
|
||||||
//
|
|
||||||
// See FromEuler for details of which set of Euler Angles are returned
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
void Quat<Type>::ToEuler(Type& yaw, Type& pitch, Type& roll) const
|
|
||||||
{
|
|
||||||
// This is probably wrong
|
|
||||||
Mat33<Type> M;
|
|
||||||
ToMat(M);
|
|
||||||
const int i = 0, j = 1, k = 2;
|
|
||||||
double cy = sqrt(M(i,i)*M(i,i) + M(i,j)*M(i,j));
|
|
||||||
if (cy > FUDGE()) {
|
|
||||||
roll = ATan2(M(j,k), M(k,k));
|
|
||||||
pitch = ATan2(-M(i,k), cy);
|
|
||||||
yaw = ATan2(M(i,j), M(i,i));
|
|
||||||
} else {
|
|
||||||
roll = ATan2(-M(k,j), M(j,j));
|
|
||||||
pitch = ATan2(-M(i,k), cy);
|
|
||||||
yaw = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// QUAT FRIENDS
|
|
||||||
//============================================================================
|
|
||||||
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator + (const Quat<Type> &a, const Quat<Type> &b)
|
|
||||||
{
|
|
||||||
return Quat<Type>( a.s+b.s, a.v+b.v );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator - (const Quat<Type> &a, const Quat<Type> &b)
|
|
||||||
{
|
|
||||||
return Quat<Type>( a.s-b.s, a.v-b.v );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator - (const Quat<Type> &a )
|
|
||||||
{
|
|
||||||
return Quat<Type>( -a.s, -a.v );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator * ( const Quat<Type> &a, const Quat<Type> &b)
|
|
||||||
{
|
|
||||||
#if 0
|
|
||||||
// 16 mults
|
|
||||||
return Quat<Type>( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
|
|
||||||
#else
|
|
||||||
// optimized (12 mults, and no compiler-generated temp objects)
|
|
||||||
Type A, B, C, D, E, F, G, H;
|
|
||||||
|
|
||||||
A = (a.s + a.v.x)*(b.s + b.v.x);
|
|
||||||
B = (a.v.z - a.v.y)*(b.v.y - b.v.z);
|
|
||||||
C = (a.s - a.v.x)*(b.v.y + b.v.z);
|
|
||||||
D = (a.v.y + a.v.z)*(b.s - b.v.x);
|
|
||||||
E = (a.v.x + a.v.z)*(b.v.x + b.v.y);
|
|
||||||
F = (a.v.x - a.v.z)*(b.v.x - b.v.y);
|
|
||||||
G = (a.s + a.v.y)*(b.s - b.v.z);
|
|
||||||
H = (a.s - a.v.y)*(b.s + b.v.z);
|
|
||||||
|
|
||||||
return Quat<Type>(
|
|
||||||
B + (-E - F + G + H) * Type(0.5),
|
|
||||||
A - (E + F + G + H) * Type(0.5),
|
|
||||||
C + (E - F + G - H) * Type(0.5),
|
|
||||||
D + (E - F - G + H) * Type(0.5));
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator * ( const Quat<Type> &a, const Type t)
|
|
||||||
{
|
|
||||||
return Quat<Type>( a.v * t, a.s * t );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator * ( const Type t, const Quat<Type> &a )
|
|
||||||
{
|
|
||||||
return Quat<Type>( a.v * t, a.s * t );
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type> operator / ( const Quat<Type> &a, const Type t )
|
|
||||||
{
|
|
||||||
return Quat<Type>( a.v / t, a.s / t );
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
bool operator == (const Quat<Type> &a, const Quat<Type> &b)
|
|
||||||
{
|
|
||||||
return (a.s == b.s && a.v == b.v);
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
bool operator != (const Quat<Type> &a, const Quat<Type> &b)
|
|
||||||
{
|
|
||||||
return (a.s != b.s || a.v != b.v);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//============================================================================
|
|
||||||
// UTILS
|
|
||||||
//============================================================================
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::DEG2RAD(Type d)
|
|
||||||
{
|
|
||||||
return d * Type(0.0174532925199432957692369076848861);
|
|
||||||
}
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::RAD2DEG(Type d)
|
|
||||||
{
|
|
||||||
return d * Type(57.2957795130823208767981548141052);
|
|
||||||
}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::Sin(double d) { return Type(sin(d)); }
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::Cos(double d) { return Type(cos(d)); }
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::ACos(double d) { return Type(acos(d)); }
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::ASin(double d) { return Type(asin(d)); }
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::ATan(double d) { return Type(atan(d)); }
|
|
||||||
template <class Type>
|
|
||||||
inline Type Quat<Type>::ATan2(double n, double d) {return Type(atan2(n,d));}
|
|
||||||
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type> Quat<Type>::ZERO() {return Quat(0,0,0,0); }
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type> Quat<Type>::IDENTITY() {return Quat(1,0,0,0); }
|
|
||||||
|
|
||||||
template<class Type>
|
|
||||||
inline Type Quat<Type>::FUDGE() { return 1e-6; }
|
|
||||||
template<>
|
|
||||||
inline double Quat<double>::FUDGE() { return 1e-10; }
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// QuatSlerp
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
template <class Type>
|
|
||||||
Quat<Type>& QuatSlerp(
|
|
||||||
Quat<Type> &dest,
|
|
||||||
const Quat<Type> &from, const Quat<Type> &to, Type t )
|
|
||||||
{
|
|
||||||
#if 0
|
|
||||||
// compact mathematical version
|
|
||||||
// exp(t*log(to*from^-1))*from
|
|
||||||
Quat<Type> fminv(from);
|
|
||||||
Quat<Type> tofrom(to*fminv.Invert());
|
|
||||||
Quat<Type> slerp = t*tofrom.Log();
|
|
||||||
slerp.Exp();
|
|
||||||
slerp *= from;
|
|
||||||
return slerp;
|
|
||||||
#endif
|
|
||||||
Quat<Type> to1;
|
|
||||||
double omega, cosom, sinom, scale0, scale1;
|
|
||||||
|
|
||||||
/* calculate cosine */
|
|
||||||
cosom = from.v * to.v + from.s + to.s;
|
|
||||||
|
|
||||||
/* Adjust signs (if necessary) so we take shorter path */
|
|
||||||
if ( cosom < 0.0 ) {
|
|
||||||
cosom = -cosom;
|
|
||||||
to1 = -to;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
to1 = to;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Calculate coefficients */
|
|
||||||
if ((1.0 - cosom) > Quat<Type>::FUDGE ) {
|
|
||||||
/* standard case (slerp) */
|
|
||||||
omega = acos( cosom );
|
|
||||||
sinom = sin( omega );
|
|
||||||
scale0 = sin((1.0 - t) * omega) / sinom;
|
|
||||||
scale1 = sin(t * omega) / sinom;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
/* 'from' and 'to' are very close - just do linear interpolation */
|
|
||||||
scale0 = 1.0 - t;
|
|
||||||
scale1 = t;
|
|
||||||
}
|
|
||||||
|
|
||||||
dest = from;
|
|
||||||
dest *= Type(scale0);
|
|
||||||
dest += Type(scale1) * to1;
|
|
||||||
return dest;
|
|
||||||
}
|
|
||||||
|
|
||||||
// This version creates more temporary objects
|
|
||||||
template <class Type>
|
|
||||||
inline Quat<Type> QuatSlerp(
|
|
||||||
const Quat<Type>& from, const Quat<Type>& to, Type t )
|
|
||||||
{
|
|
||||||
Quat<Type> q;
|
|
||||||
return QuatSlerp(q, from, to, t);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@@ -1,196 +0,0 @@
|
|||||||
//------------------------------------------------------------------------------
|
|
||||||
// File : quattest.cpp
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// GLVU : Copyright 1997 - 2002
|
|
||||||
// The University of North Carolina at Chapel Hill
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
// Permission to use, copy, modify, distribute and sell this software and its
|
|
||||||
// documentation for any purpose is hereby granted without fee, provided that
|
|
||||||
// the above copyright notice appear in all copies and that both that copyright
|
|
||||||
// notice and this permission notice appear in supporting documentation.
|
|
||||||
// Binaries may be compiled with this software without any royalties or
|
|
||||||
// restrictions.
|
|
||||||
//
|
|
||||||
// The University of North Carolina at Chapel Hill makes no representations
|
|
||||||
// about the suitability of this software for any purpose. It is provided
|
|
||||||
// "as is" without express or implied warranty.
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
// Quattest.cpp
|
|
||||||
// Quaternion coverage (and maybe functionality) test.
|
|
||||||
// The main objective is to just make sure every API gets called
|
|
||||||
// to flush out lurking template bugs that might not otherwise get tickled.
|
|
||||||
// A few checks to make sure things are functioning properly are also
|
|
||||||
// included.
|
|
||||||
//
|
|
||||||
// For the API coverage, if the thing simply compiles then it was a success.
|
|
||||||
//----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
|
|
||||||
#include "quat.hpp"
|
|
||||||
|
|
||||||
void coverage_test()
|
|
||||||
{
|
|
||||||
float x = 0.5f, y = 1.1f, z = -0.1f, s = 0.2f;
|
|
||||||
// TEST CONSTRUCTORS
|
|
||||||
Quatf q1;
|
|
||||||
Quatf q2 = Quatf(x, y, z, s);
|
|
||||||
Quatf q3(x, y, z);
|
|
||||||
Quatf q4(Vec3f(1,2,3));
|
|
||||||
Quatf q5(Vec3f(2,3,4), 2.0);
|
|
||||||
Quatf q6(1.0, Vec3f(-1,-2,-3));
|
|
||||||
float floatarray[4] = { 1, 2, 3, 4 };
|
|
||||||
Quatf q7(floatarray);
|
|
||||||
Quatf q8(q7);
|
|
||||||
|
|
||||||
// TEST SETTERS
|
|
||||||
q1.Set(1,2,3);
|
|
||||||
q2.Set(1,2,3,4);
|
|
||||||
q3.Set(Vec3f(1.0,1.0,1.0));
|
|
||||||
q3.Set(Vec3f(1.0,1.0,1.0),-0.5);
|
|
||||||
|
|
||||||
// TEST OPERATORS
|
|
||||||
// Quat &operator = ( const Quat &v ); /* assignment of a Quat */
|
|
||||||
q4 = q6;
|
|
||||||
// Quat &operator += ( const Quat &v ); /* incrementation by a Quat */
|
|
||||||
q3 += q2;
|
|
||||||
// Quat &operator -= ( const Quat &v ); /* decrementation by a Quat */
|
|
||||||
q3 -= q2;
|
|
||||||
// Quat &operator *= ( const Type d ); /* multiplication by a scalar */
|
|
||||||
q4 *= 0.5f;
|
|
||||||
// Quat &operator *= ( const Quat &v ); /* quat product (this*v) */
|
|
||||||
q2 *= q1;
|
|
||||||
// Quat &operator /= ( const Type d ); /* division by a scalar */
|
|
||||||
q5 /= 2.0f;
|
|
||||||
// Type &operator [] ( int i); /* indexing x=0, s=3 */
|
|
||||||
float c0 = q1[0];
|
|
||||||
float c1 = q1[1];
|
|
||||||
float c2 = q1[2];
|
|
||||||
float c3 = q1[3];
|
|
||||||
|
|
||||||
// TEST SPECIAL FUNCTIONS
|
|
||||||
|
|
||||||
// Type Length(void) const; /* length of a Quat */
|
|
||||||
float l = q4.Length();
|
|
||||||
// Type LengthSqr(void) const; /* squared length of a Quat */
|
|
||||||
l = q4.LengthSqr();
|
|
||||||
// Type LengthSqr(void) const; /* squared length of a Quat */
|
|
||||||
l = q4.Norm();
|
|
||||||
// Quat &Normalize(void); /* normalize a Quat */
|
|
||||||
Quatf q9 = q4.Normalize();
|
|
||||||
// Quat &Invert(void); /* q = q^-1 */
|
|
||||||
q9 = q4.Invert();
|
|
||||||
// Quat &Conjugate(void); /* q = q* */
|
|
||||||
q9 = q4.Conjugate();
|
|
||||||
// qvec Xform( const qvec &v ); /* q*v*q-1 */
|
|
||||||
Vec3f v1 = q4.Xform(Vec3f(1.0,1.0,1.0));
|
|
||||||
// Quat &Log(void); /* log(q) */
|
|
||||||
q9 = q4.Log();
|
|
||||||
// Quat &Exp(void); /* exp(q) */
|
|
||||||
q9 = q4.Exp();
|
|
||||||
// qvec GetAxis( void ) const; /* Get rot axis */
|
|
||||||
v1 = q5.GetAxis();
|
|
||||||
// Type GetAngle( void ) const; /* Get rot angle (radians) */
|
|
||||||
float a = q5.GetAngle();
|
|
||||||
// void SetAngle( Type rad_ang ); /* set rot angle (radians) */
|
|
||||||
q2.SetAngle(a);
|
|
||||||
// void ScaleAngle( Type f ); /* scale rot angle */
|
|
||||||
q2.ScaleAngle(1.5);
|
|
||||||
// void Print( ) const; /* print Quat */
|
|
||||||
q3.Print();
|
|
||||||
|
|
||||||
/* TEST CONVERSIONS */
|
|
||||||
Mat44f m44;
|
|
||||||
Mat33f m33;
|
|
||||||
//Mat44& ToMat( Mat44 &dest ) const;
|
|
||||||
//Mat33& ToMat( Mat33 &dest ) const;
|
|
||||||
// Quat& FromMat( const Mat44<Type>& src ) const;
|
|
||||||
// Quat& FromMat( const Mat33<Type>& src ) const;
|
|
||||||
q3.Normalize();
|
|
||||||
m44 = q3.ToMat(m44);
|
|
||||||
m33 = q3.ToMat(m33);
|
|
||||||
q5 = q3.FromMat(m44);
|
|
||||||
q5 = q3.FromMat(m33);
|
|
||||||
//void ToAngleAxis( Type &ang, qvec &ax ) const;
|
|
||||||
q3.ToAngleAxis( a, v1 );
|
|
||||||
//Quat& FromAngleAxis( Type ang, const qvec &ax );
|
|
||||||
q4 = q3.FromAngleAxis( a, v1 );
|
|
||||||
//Quat& FromTwoVecs(const qvec &a, const qvec& b);
|
|
||||||
Vec3f v2(-1,-2,-3);
|
|
||||||
q4 = q3.FromTwoVecs( v2, v1 );
|
|
||||||
//Quat& FromEuler( Type yaw, Type pitch, Type roll);
|
|
||||||
q4 = q3.FromEuler( 2.2f, 1.2f, -0.4f );
|
|
||||||
//void ToEuler(Type &yaw, Type &pitch, Type &roll) const;
|
|
||||||
float p=0.3f,r=-1.57f; y= 0.1f;
|
|
||||||
q3.ToEuler( y,p,r );
|
|
||||||
|
|
||||||
/* TEST FRIENDS */
|
|
||||||
|
|
||||||
//friend Quat operator - (const Quat &v); /* -q1 */
|
|
||||||
q1 = -q2;
|
|
||||||
//friend Quat operator + (const Quat &a, const Quat &b); /* q1 + q2 */
|
|
||||||
q1 = q2 + q3;
|
|
||||||
//friend Quat operator - (const Quat &a, const Quat &b); /* q1 - q2 */
|
|
||||||
q1 = q2 - q3;
|
|
||||||
//friend Quat operator * (const Quat &a, const Type d); /* q1 * 3.0 */
|
|
||||||
q1 = q2 * 0.2f;
|
|
||||||
//friend Quat operator * (const Type d, const Quat &a); /* 3.0 * q1 */
|
|
||||||
q1 = 0.2f * q2;
|
|
||||||
//friend Quat operator * (const Quat &a, const Quat &b); /* q1 * q2 */
|
|
||||||
q1 = q2 * q3;
|
|
||||||
//friend Quat operator / (const Quat &a, const Type d); /* q1 / 3.0 */
|
|
||||||
q1 = q2 / 1.2f;
|
|
||||||
//friend bool operator == (const Quat &a, const Quat &b); /* q1 == q2 ? */
|
|
||||||
bool eq = (q1 == q2);
|
|
||||||
//friend bool operator != (const Quat &a, const Quat &b); /* q1 != q2 ? */
|
|
||||||
bool neq = (q1 != q2);
|
|
||||||
|
|
||||||
// HELPERS
|
|
||||||
// static Type DEG2RAD(Type d);
|
|
||||||
// static Type RAD2DEG(Type d);
|
|
||||||
a = Quatf::RAD2DEG(a);
|
|
||||||
a = Quatf::DEG2RAD(a);
|
|
||||||
a = Quatf::Sin(a);
|
|
||||||
a = Quatf::Cos(a);
|
|
||||||
a = Quatf::ACos(a);
|
|
||||||
a = Quatf::ASin(a);
|
|
||||||
a = Quatf::ATan(a);
|
|
||||||
a = Quatf::ATan2(a, p);
|
|
||||||
|
|
||||||
// CONSTANTS
|
|
||||||
// static const Type FUDGE;
|
|
||||||
// static const Quat ZERO;
|
|
||||||
// static const Quat IDENTITY;
|
|
||||||
a = Quatf::FUDGE;
|
|
||||||
q3 = Quatf::ZERO();
|
|
||||||
q4 = Quatf::IDENTITY();
|
|
||||||
|
|
||||||
q1 = QuatSlerp(q1, q3, q4, 0.5f );
|
|
||||||
q1 = QuatSlerp(q3, q4, 0.5f );
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
Quatf StatQuat(Quatf::IDENTITY());
|
|
||||||
Quatf StatQuat2 = Quatf::IDENTITY();
|
|
||||||
|
|
||||||
void functional_test()
|
|
||||||
{
|
|
||||||
printf("The ZERO quat: ");
|
|
||||||
Quatf::ZERO().Print();
|
|
||||||
printf("The IDENTITY quat: ");
|
|
||||||
Quatf::IDENTITY().Print();
|
|
||||||
printf("Statically constructed copy of IDENTITY quat: ");
|
|
||||||
StatQuat.Print();
|
|
||||||
printf("A different static copy of IDENTITY quat: ");
|
|
||||||
StatQuat2.Print();
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
coverage_test();
|
|
||||||
functional_test();
|
|
||||||
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user