+++ /dev/null
-//------------------------------------------------------------------------------
-// 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
+++ /dev/null
-//------------------------------------------------------------------------------
-// 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);
-}
-
-
+++ /dev/null
-//------------------------------------------------------------------------------
-// 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);
-}
projects / simgear.git / commitdiff