777 lines
25 KiB
C
777 lines
25 KiB
C
|
//-----------------------------------------------------------------------------
|
||
|
|
// Quaternion.h
|
||
|
|
//
|
||
|
|
// Author: Squirrel Eiserloh
|
||
|
|
//
|
||
|
|
// Copyright (C) 1997 by Ritual Entertainment, Inc.
|
||
|
|
// All rights reserved.
|
||
|
|
//
|
||
|
|
// This source is may not be distributed and/or modified without
|
||
|
|
// expressly written permission by Ritual Entertainment, Inc.
|
||
|
|
//
|
||
|
|
// DESCRIPTION:
|
||
|
|
// Header for (and inline implementation of) a basic Quaternion class.
|
||
|
|
// This should ultimately be part of a yet-to-be-written general-purpose
|
||
|
|
// Rotation class at some point, time-permitting.
|
||
|
|
//
|
||
|
|
#ifndef _QUATERNION_H_
|
||
|
|
#define _QUATERNION_H_
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include "vector.h"
|
||
|
|
//#include "Matrix.h"
|
||
|
|
|
||
|
|
#define UNUSED_ARG (void)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Quaternion
|
||
|
|
//
|
||
|
|
// ...in the form (x, y, z, w) such that q = xi + yj + zk + w, where:
|
||
|
|
//
|
||
|
|
// i x i = -1 i x j = k j x i = -k
|
||
|
|
// j x j = -1 and j x k = i and k x j = -i
|
||
|
|
// k = k = -1 k x i = j i x k = -j
|
||
|
|
//
|
||
|
|
// Note that i, j, and k are each imaginary numbers of different "flavors"
|
||
|
|
// representing mutually perpendicular unit vectors defining 3 of the
|
||
|
|
// 4 axes in quaternion 4-space. (The fourth axis is the real unit, 1.)
|
||
|
|
//
|
||
|
|
// In vector form, the quaternion would look like (s, v), where s is a
|
||
|
|
// scalar (equal to w) and v is a vector (x, y, z) in quaternion 4-space
|
||
|
|
// (giving position along the base unit axis vectors i, j, and k,
|
||
|
|
// respectively).
|
||
|
|
//
|
||
|
|
// The most useful quaternions are those that are of unit length
|
||
|
|
// (|q| = 1, or x*x + y*y + z*z + w*w = 1). This defines a set
|
||
|
|
// of points which make up a 4-dimensional "unit hypersphere",
|
||
|
|
// across the surface of which we will be interpolating.
|
||
|
|
//
|
||
|
|
// Note that Quaternion multiplication involves the vector cross
|
||
|
|
// product (v1 x v2), so it is NOT COMMUTATIVE. This means that
|
||
|
|
// q1 x q2 != q2 x q1. (Then again, matrix multiplication isn't
|
||
|
|
// commutative either, so suck it down.)
|
||
|
|
//
|
||
|
|
// "lhs" and "rhs" mean "left hand side" and "right hand side" for
|
||
|
|
// operator arguments, respectively.
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
class Quaternion
|
||
|
|
{
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Member variables
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
private:
|
||
|
|
float _x; // coefficient for the i imaginary term
|
||
|
|
float _y; // coefficient for the j imaginary term
|
||
|
|
float _z; // coefficient for the k imaginary term
|
||
|
|
float _w; // coefficient for the real term
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Accessors / Mutators
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
protected:
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Implementation Methods
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
private:
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Construction / Destruction
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
public:
|
||
|
|
~Quaternion();
|
||
|
|
explicit Quaternion(); // default constructor
|
||
|
|
Quaternion( const Quaternion& rhs ); // copy constructor
|
||
|
|
explicit Quaternion( const float x, const float y, const float z, const float w );
|
||
|
|
explicit Quaternion( const float w, const Vector& vec );
|
||
|
|
explicit Quaternion( const Vector& eulerAngles );
|
||
|
|
// explicit Quaternion( const Matrix3x3& rotationMatrix );
|
||
|
|
// explicit Quaternion( const Matrix4x4& transformMatrix );
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Interface Methods
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
public:
|
||
|
|
float CalcLength( void ) const;
|
||
|
|
float CalcLengthSquared( void ) const;
|
||
|
|
float Normalize( void );
|
||
|
|
|
||
|
|
void SetFromSV( const float w, const Vector& vec );
|
||
|
|
void SetFromXYZW( const float x, const float y, const float z, const float w );
|
||
|
|
void SetFromEuler( const Vector& eulerAngles );
|
||
|
|
// void SetFromMatrix3x3( const Matrix3x3& rotationMatrix );
|
||
|
|
// void SetFromMatrix4x4( const Matrix4x4& transformMatrix );
|
||
|
|
|
||
|
|
void GetToSV( float& w, Vector& vec ) const;
|
||
|
|
void GetToXYZW( float& x, float& y, float& z, float& w ) const;
|
||
|
|
void GetToEuler( Vector& eulerAngles ) const;
|
||
|
|
// void GetToMatrix3x3( Matrix3x3& rotationMatrix ) const;
|
||
|
|
// void GetToMatrix4x4( Matrix4x4& transformMatrix ) const;
|
||
|
|
|
||
|
|
/// Self-modifying operators
|
||
|
|
const Quaternion& operator = ( const Quaternion& rhs );
|
||
|
|
const Quaternion& operator += ( const Quaternion& rhs );
|
||
|
|
const Quaternion& operator -= ( const Quaternion& rhs );
|
||
|
|
const Quaternion& operator *= ( const float scale );
|
||
|
|
const Quaternion& operator *= ( const Quaternion& rhs );
|
||
|
|
const Quaternion& operator /= ( const float invScale );
|
||
|
|
const Quaternion operator - () const;
|
||
|
|
|
||
|
|
/// Construction operators
|
||
|
|
const Quaternion operator + ( const Quaternion& rhs ) const;
|
||
|
|
const Quaternion operator - ( const Quaternion& rhs ) const;
|
||
|
|
|
||
|
|
bool operator == ( const Quaternion& rhs ) const;
|
||
|
|
bool operator != ( const Quaternion& rhs ) const;
|
||
|
|
|
||
|
|
private:
|
||
|
|
const Quaternion operator * ( const float scale ) const; // multiply-by-right-scalar forbidden; use (float, Quaternion&) version instead
|
||
|
|
};
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// External Operators & Functions
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
const Quaternion operator * ( const Quaternion& lhs, const Quaternion& rhs );
|
||
|
|
const Quaternion operator * ( const float scale, const Quaternion& rhs );
|
||
|
|
const Quaternion operator / ( const Quaternion& lhs, const float invScale );
|
||
|
|
const Quaternion CalcSlerp( const Quaternion& q1, const Quaternion& q2, const float fraction );
|
||
|
|
const Quaternion CalcLerp( const Quaternion& q1, const Quaternion& q2, const float q2Fraction );
|
||
|
|
const Quaternion CalcNoLerp( const Quaternion& q1, const Quaternion& q2, const float q2Fraction );
|
||
|
|
float QuaternionDotProduct( const Quaternion& q1, const Quaternion& q2 );
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Destructor
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::~Quaternion()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Default constructor
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::Quaternion()
|
||
|
|
{
|
||
|
|
// Do nothing; this should be used only by static array declarations
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Copy Constructor( Quaternion )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::Quaternion( const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
SetFromXYZW( rhs._x, rhs._y, rhs._z, rhs._w );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Constructor( float, float, float, float )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::Quaternion( const float x, const float y, const float z, const float w )
|
||
|
|
{
|
||
|
|
SetFromXYZW( x, y, z, w );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Constructor( float, Vector )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::Quaternion( const float w, const Vector& vec )
|
||
|
|
{
|
||
|
|
SetFromSV( w, vec );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Constructor( Vector )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline Quaternion::Quaternion( const Vector& eulerAngles )
|
||
|
|
{
|
||
|
|
SetFromEuler( eulerAngles );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Constructor( Matrix3x3 )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline Quaternion::Quaternion( const Matrix3x3& rotationMatrix )
|
||
|
|
{
|
||
|
|
SetFromMatrix3x3( rotationMatrix );
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Constructor( Matrix4x4 )
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline Quaternion::Quaternion( const Matrix4x4& transformMatrix )
|
||
|
|
{
|
||
|
|
SetFromMatrix4x4( transformMatrix );
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// CalcLength()
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline float Quaternion::CalcLength( void ) const
|
||
|
|
{
|
||
|
|
float length = (float) sqrt( CalcLengthSquared() );
|
||
|
|
return( length );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// CalcLengthSquared()
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline float Quaternion::CalcLengthSquared( void ) const
|
||
|
|
{
|
||
|
|
float lengthSquared;
|
||
|
|
lengthSquared = (_x * _x) + (_y * _y) + (_z * _z) + (_w * _w);
|
||
|
|
return( lengthSquared );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// Normalize
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline float Quaternion::Normalize( void )
|
||
|
|
{
|
||
|
|
/// Get the length of the quaternion 4d vector
|
||
|
|
float length = CalcLength();
|
||
|
|
if( !length )
|
||
|
|
return( 0.0f );
|
||
|
|
|
||
|
|
/// Divide each component by <length>
|
||
|
|
*this /= length;
|
||
|
|
return( length );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// SetFromSV
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::SetFromSV( const float w, const Vector& vec )
|
||
|
|
{
|
||
|
|
SetFromXYZW( vec.x, vec.y, vec.z, w );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// SetFromXYZW
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::SetFromXYZW( const float x, const float y, const float z, const float w )
|
||
|
|
{
|
||
|
|
_x = x;
|
||
|
|
_y = y;
|
||
|
|
_z = z;
|
||
|
|
_w = w;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// SetFromEuler
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::SetFromEuler( const Vector& eulerAngles )
|
||
|
|
{
|
||
|
|
// FIXME: THIS IS TEMPORARY HACKED STUFF FOR PROOF OF CONCEPT ONLY!!!
|
||
|
|
float quat[ 4 ];
|
||
|
|
vec3_t eulerAng;
|
||
|
|
|
||
|
|
eulerAngles.copyTo( eulerAng );
|
||
|
|
EulerToQuat( eulerAng, quat );
|
||
|
|
|
||
|
|
_x = quat[ 0 ];
|
||
|
|
_y = quat[ 1 ];
|
||
|
|
_z = quat[ 2 ];
|
||
|
|
_w = quat[ 3 ];
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// SetFromMatrix3x3
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline void Quaternion::SetFromMatrix3x3( const Matrix3x3& rotationMatrix )
|
||
|
|
{
|
||
|
|
// FIXME: stub
|
||
|
|
UNUSED_ARG rotationMatrix;
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// SetFromMatrix4x4
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline void Quaternion::SetFromMatrix4x4( const Matrix4x4& transformMatrix )
|
||
|
|
{
|
||
|
|
// FIXME: stub
|
||
|
|
UNUSED_ARG transformMatrix;
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// GetToSV
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::GetToSV( float& w, Vector& vec ) const
|
||
|
|
{
|
||
|
|
GetToXYZW( vec.x, vec.y, vec.z, w );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// GetToXYZW
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::GetToXYZW( float& x, float& y, float& z, float& w ) const
|
||
|
|
{
|
||
|
|
x = _x;
|
||
|
|
y = _y;
|
||
|
|
z = _z;
|
||
|
|
w = _w;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// GetToEuler
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline void Quaternion::GetToEuler( Vector& eulerAngles ) const
|
||
|
|
{
|
||
|
|
// FIXME: THIS IS TEMPORARY HACKED STUFF FOR PROOF OF CONCEPT ONLY!!!
|
||
|
|
float matrix[ 3 ][ 3 ];
|
||
|
|
float quat[ 4 ];
|
||
|
|
vec3_t eulerAng;
|
||
|
|
|
||
|
|
quat[ 0 ] = _x;
|
||
|
|
quat[ 1 ] = _y;
|
||
|
|
quat[ 2 ] = _z;
|
||
|
|
quat[ 3 ] = _w;
|
||
|
|
|
||
|
|
QuatToMat( quat, matrix );
|
||
|
|
MatrixToEulerAngles( matrix, eulerAng );
|
||
|
|
eulerAngles.setXYZ( eulerAng[0], eulerAng[1], eulerAng[2] );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// GetToMatrix3x3
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline void Quaternion::GetToMatrix3x3( Matrix3x3& rotationMatrix ) const
|
||
|
|
{
|
||
|
|
// FIXME: stub
|
||
|
|
UNUSED_ARG rotationMatrix;
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// GetToMatrix4x4
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
/*
|
||
|
|
inline void Quaternion::GetToMatrix4x4( Matrix4x4& transformMatrix ) const
|
||
|
|
{
|
||
|
|
// FIXME: stub
|
||
|
|
UNUSED_ARG transformMatrix;
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator = (Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator = ( const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
if( this == &rhs )
|
||
|
|
return *this;
|
||
|
|
|
||
|
|
_x = rhs._x;
|
||
|
|
_y = rhs._y;
|
||
|
|
_z = rhs._z;
|
||
|
|
_w = rhs._w;
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator += (Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator += ( const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
_x += rhs._x;
|
||
|
|
_y += rhs._y;
|
||
|
|
_z += rhs._z;
|
||
|
|
_w += rhs._w;
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator -= (Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator -= ( const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
_x -= rhs._x;
|
||
|
|
_y -= rhs._y;
|
||
|
|
_z -= rhs._z;
|
||
|
|
_w -= rhs._w;
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator *= (float)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator *= ( const float scale )
|
||
|
|
{
|
||
|
|
_x *= scale;
|
||
|
|
_y *= scale;
|
||
|
|
_z *= scale;
|
||
|
|
_w *= scale;
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator *= (Quaternion)
|
||
|
|
//
|
||
|
|
// Quaternion multiplication is NOT COMMUTATIVE, and is defined as
|
||
|
|
// follows, where "s" is the scalar component (w) and "v" is the vector
|
||
|
|
// component (x,y,z) (and 'x' means "3d vector cross product", '|' means
|
||
|
|
// "3d vector dot product"):
|
||
|
|
//
|
||
|
|
// q1 * q2 = q3 = (s3, v3) != q2 * q1
|
||
|
|
//
|
||
|
|
// s3 = (s1 * s2) - (v1 | v2)
|
||
|
|
// v3 = (s1 * v2) + (s2 * v1) + (v1 x v2)
|
||
|
|
//
|
||
|
|
// i.e. Q(s1, v1) * Q(s2, v2) = Q(s3, v3)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator *= ( const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
float s1, s2, s3; // see above comment
|
||
|
|
Vector v1, v2, v3; // see above comment
|
||
|
|
|
||
|
|
/// Get both quaternions into (s,v) form
|
||
|
|
GetToSV( s1, v1 );
|
||
|
|
rhs.GetToSV( s2, v2 );
|
||
|
|
|
||
|
|
/// Calculate the new scalar term (s3)
|
||
|
|
s3 = (s1 * s2) - Vector::Dot( v1, v2 );
|
||
|
|
|
||
|
|
/// Calculate the new vector term (v3)
|
||
|
|
Vector v1CrossV2; // temp variable for cross-product result, since our Vector class sucks
|
||
|
|
v1CrossV2.CrossProduct( v1, v2 ); // our vector class sucks
|
||
|
|
v3 = (s1 * v2) + (s2 * v1) + v1CrossV2;
|
||
|
|
|
||
|
|
/// Set the new scalar and vector terms and return this
|
||
|
|
SetFromSV( s3, v3 );
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator /= (float)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion& Quaternion::operator /= ( const float invScale )
|
||
|
|
{
|
||
|
|
if( invScale )
|
||
|
|
{
|
||
|
|
_x /= invScale;
|
||
|
|
_y /= invScale;
|
||
|
|
_z /= invScale;
|
||
|
|
_w /= invScale;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
SetFromXYZW( 0.0f, 0.0f, 0.0f, 0.0f );
|
||
|
|
}
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator - (Quaternion) : unary minus
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion Quaternion::operator - () const
|
||
|
|
{
|
||
|
|
return Quaternion( -_x, -_y, -_z, -_w );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator == (Quaternion, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
//inline bool operator == ( const Quaternion& lhs, const Quaternion& rhs )
|
||
|
|
inline bool Quaternion::operator == ( const Quaternion& rhs ) const
|
||
|
|
{
|
||
|
|
if( _x != rhs._x )
|
||
|
|
return false;
|
||
|
|
|
||
|
|
if( _y != rhs._y )
|
||
|
|
return false;
|
||
|
|
|
||
|
|
if( _z != rhs._z )
|
||
|
|
return false;
|
||
|
|
|
||
|
|
if( _w != rhs._w )
|
||
|
|
return false;
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator != (Quaternion, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline bool Quaternion::operator != ( const Quaternion& rhs ) const
|
||
|
|
{
|
||
|
|
return( !(*this == rhs) );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator - (Quaternion, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion Quaternion::operator - ( const Quaternion& rhs ) const
|
||
|
|
{
|
||
|
|
Quaternion difference( *this );
|
||
|
|
difference -= rhs;
|
||
|
|
return( difference );
|
||
|
|
}
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// operator + (Quaternion, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion Quaternion::operator + ( const Quaternion& rhs ) const
|
||
|
|
//Quaternion Quaternion::operator + ( const Quaternion& rhs ) const
|
||
|
|
{
|
||
|
|
Quaternion sum( *this );
|
||
|
|
sum += rhs;
|
||
|
|
return( sum );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general operator * (Quaternion, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion operator * ( const Quaternion& lhs, const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
Quaternion product( lhs );
|
||
|
|
product *= rhs;
|
||
|
|
return product;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general operator * (float, Quaternion)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion operator * ( const float scale, const Quaternion& rhs )
|
||
|
|
{
|
||
|
|
Quaternion scaled( rhs );
|
||
|
|
scaled *= scale;
|
||
|
|
return scaled;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general operator / (Quaternion, float)
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion operator / ( const Quaternion& lhs, const float invScale )
|
||
|
|
{
|
||
|
|
Quaternion scaled( lhs );
|
||
|
|
if( invScale )
|
||
|
|
{
|
||
|
|
scaled /= invScale;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
scaled *= 0.0f;
|
||
|
|
}
|
||
|
|
return scaled;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general CalcLerp (Quaternion, Quaternion, float)
|
||
|
|
//
|
||
|
|
// Performs a hypervector linear interpolation - or "lerp" - of two
|
||
|
|
// Quaternions and returns the resulting (newly constructed) Quaternion.
|
||
|
|
//
|
||
|
|
// <q2Fraction> is a value in the range [0,1] representing how much of
|
||
|
|
// <q2> to use in the interpolation; q1Fraction = 1-<q2Fraction> is the
|
||
|
|
// weighting given to <q1>.
|
||
|
|
//
|
||
|
|
// NOTE: This interpolation is faster, but less accurate, than CalcSlerp().
|
||
|
|
// Use CalcSlerp() if the error incurred from CalcLerp() is noticeable.
|
||
|
|
//
|
||
|
|
// For optimization purposes, CalcLerp() assumes <q1> and <q2> are
|
||
|
|
// already normalized.
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion CalcLerp( const Quaternion& q1, const Quaternion& q2, const float q2Fraction )
|
||
|
|
{
|
||
|
|
const float q1Fraction = 1.0f - q2Fraction;
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are the same quaternion
|
||
|
|
if( &q1 == &q2 )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are data-identical
|
||
|
|
if( q1 == q2 )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
|
||
|
|
/// Check if <q2Fraction> is at (or beyond) a boundary condition
|
||
|
|
if( q2Fraction <= 0.0f )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
if( q2Fraction >= 1.0f )
|
||
|
|
return Quaternion( q2 );
|
||
|
|
|
||
|
|
/// Create a new quaternion which represents the weighted average of <q1> and <q2>
|
||
|
|
Quaternion lerped( (q1Fraction * q1) + (q2Fraction * q2) );
|
||
|
|
float lerpedLength = lerped.Normalize();
|
||
|
|
|
||
|
|
/// Check if the 4d vectors added up to 0.0 (degenerate case!)
|
||
|
|
if( !lerpedLength )
|
||
|
|
{
|
||
|
|
/// Return whichever of <q1> or <q2> the parameter is currently closest to
|
||
|
|
return CalcNoLerp( q1, q2, q2Fraction );
|
||
|
|
}
|
||
|
|
|
||
|
|
return lerped;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general CalcSlerp (Quaternion, Quaternion, float)
|
||
|
|
//
|
||
|
|
// Performs a (hyper)spherical linear interpolation - or "slerp" - of two
|
||
|
|
// Quaternions and returns the resulting (newly constructed) Quaternion.
|
||
|
|
//
|
||
|
|
// <q2Fraction> is a value in the range [0,1] representing how much of
|
||
|
|
// <q2> to use in the interpolation; q1Fraction = 1-<q2Fraction> is the
|
||
|
|
// weighting given to <q1>.
|
||
|
|
//
|
||
|
|
// For optimization purposes, CalcSlerp() assumes <q1> and <q2> are
|
||
|
|
// already normalized.
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion CalcSlerp( const Quaternion& q1, const Quaternion& q2, const float q2Fraction )
|
||
|
|
{
|
||
|
|
static const float SIN_OMEGA_EPSILON = 0.00001f;
|
||
|
|
const float q1Fraction = 1.0f - q2Fraction;
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are one and the same
|
||
|
|
if( &q1 == &q2 )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are data-identical
|
||
|
|
if( q1 == q2 )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
|
||
|
|
/// Check if <q2Fraction> is at (or beyond) a boundary condition
|
||
|
|
if( q2Fraction <= 0.0f )
|
||
|
|
return Quaternion( q1 );
|
||
|
|
if( q2Fraction >= 1.0f )
|
||
|
|
return Quaternion( q2 );
|
||
|
|
|
||
|
|
/// Calculate <cosOmega>, the dot product (cosine) of the angle between the two 4d hypervectors
|
||
|
|
float cosOmega = QuaternionDotProduct( q1, q2 );
|
||
|
|
|
||
|
|
/// Create a copy of q2 and invert it if <cosOmega> is negative (i.e. it's on the opposite side of the hypersphere)
|
||
|
|
Quaternion q2copy( q2 );
|
||
|
|
if( cosOmega < 0.0f )
|
||
|
|
{
|
||
|
|
/// Mirror the hypervector (and, therefore, the dot product) to be on the same side of the hypersphere as <q1>
|
||
|
|
cosOmega = -cosOmega;
|
||
|
|
q2copy *= -1.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Check if either <q1> or <q2> was not normalized
|
||
|
|
if( cosOmega > 1.0f )
|
||
|
|
{
|
||
|
|
/// FIXME: One or both of <q1> or <q2> were evidently not normalized; this should be an error condition!
|
||
|
|
/// Return whichever of <q1> or <q2> the parameter is currently closest to
|
||
|
|
return CalcNoLerp( q1, q2, q2Fraction );
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are close enough to use linear interpolation instead
|
||
|
|
if( (1.0f - cosOmega) > SIN_OMEGA_EPSILON )
|
||
|
|
{
|
||
|
|
return CalcLerp( q1, q2, q2Fraction );
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Check if <q1> and <q2> are nearly opposite on the hypersphere
|
||
|
|
if( (cosOmega + 1.0) < SIN_OMEGA_EPSILON )
|
||
|
|
{
|
||
|
|
// FIXME: how should this case be handled?
|
||
|
|
// Watt & Watt does some voodoo-math which is clearly incorrect...
|
||
|
|
return CalcNoLerp( q1, q2, q2Fraction );
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Calculate <omega>, the angle between <q1> and <q2>, based on the dot product (cosine) between <q1> and <q2>
|
||
|
|
const float omega = acos( cosOmega );
|
||
|
|
const float sinOmega = sin( omega );
|
||
|
|
|
||
|
|
/// Check if <sinOmega> is prohibitively small (since it will end up in the denominator later)
|
||
|
|
if( (sinOmega > -SIN_OMEGA_EPSILON) && (sinOmega < SIN_OMEGA_EPSILON) )
|
||
|
|
{
|
||
|
|
/// Return whichever of <q1> or <q2> the parameter is currently closest to
|
||
|
|
return CalcNoLerp( q1, q2, q2Fraction );
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Build a new quaternion, <slerped>, which uses normal (hyper)spherical linear interpolation
|
||
|
|
Quaternion slerped;
|
||
|
|
float scale1 = sin( q1Fraction * omega ) / sinOmega;
|
||
|
|
float scale2 = sin( q2Fraction * omega ) / sinOmega;
|
||
|
|
slerped = (scale1 * q1) + (scale2 * q2);
|
||
|
|
|
||
|
|
// FIXME: does <slerped> need to be renormalized at this point??
|
||
|
|
// (yes, but only because of floating-point drift, and it's the caller's reponsibility to do this)
|
||
|
|
|
||
|
|
return slerped;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// general CalcNoLerp (Quaternion, Quaternion, float)
|
||
|
|
//
|
||
|
|
// This is a fake / stub quaternion interpolation function; it simply
|
||
|
|
// returns a copy of <q1> or <q2> based on which one <q2Fraction>
|
||
|
|
// indicates it is closer to.
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline const Quaternion CalcNoLerp( const Quaternion& q1, const Quaternion& q2, const float q2Fraction )
|
||
|
|
{
|
||
|
|
if( q2Fraction < 0.5f )
|
||
|
|
{
|
||
|
|
return Quaternion( q1 );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
return Quaternion( q2 );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
// QuaternionDotProduct
|
||
|
|
//
|
||
|
|
// Calculates the dot product of <q1> and <q2> where both quaternions are
|
||
|
|
// taken as (probably unit) vectors in fourspace.
|
||
|
|
//---------------------------------------------------------------------------
|
||
|
|
inline float QuaternionDotProduct( const Quaternion& q1, const Quaternion& q2 )
|
||
|
|
{
|
||
|
|
float x1, y1, z1, w1;
|
||
|
|
float x2, y2, z2, w2;
|
||
|
|
|
||
|
|
q1.GetToXYZW( x1, y1, z1, w1 );
|
||
|
|
q2.GetToXYZW( x2, y2, z2, w2 );
|
||
|
|
|
||
|
|
float dotProduct = (x1*x2) + (y1*y2) + (z1*z2) + (w1*w1);
|
||
|
|
return dotProduct;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#endif // _QUATERNION_H_
|