devmaster-cvs

[<ape...@us...>]

Update of /cvsroot/devmaster/engine/src/core/Math
In directory sc8-pr-cvs1:/tmp/cvs-serv12976/src/core/Math

Added Files:
	MathEngine.cpp MathEngine.h Matrix3.cpp Matrix3.h 
	Quaternion.cpp Quaternion.h Vector2.cpp Vector2.h Vector3.cpp 
	Vector3.h 
Log Message:
the math engine added.

--- NEW FILE: MathEngine.cpp ---
#include <float.h>
#include <stdlib.h>
#include "EngineMath.h"

const float Math::MAX_FLOAT = FLT_MAX;
const float Math::PI = 4.0f*Math::ATan(1.0f);
const float Math::TWO_PI = 2.0f*PI;
const float Math::HALF_PI = 0.5f*PI;
const float Math::INV_TWO_PI = 1.0f/TWO_PI;
const float Math::DEG_TO_RAD = Math::PI/180.0f;
const float Math::RAD_TO_DEG = 1.0f/Math::DEG_TO_RAD;

//----------------------------------------------------------------------------
float Math::UnitRandom (float seed)
{
    if ( seed > 0.0f )
        srand((unsigned int)seed);

     return float(rand())/float(RAND_MAX);
}
//----------------------------------------------------------------------------
float Math::SymmetricRandom (float seed)
{
    if ( seed > 0.0f )
        srand((unsigned int)seed);

    return 2.0f*float(rand())/float(RAND_MAX) - 1.0f;
}
//----------------------------------------------------------------------------
float Math::IntervalRandom (float min, float max, float seed)
{
    if ( seed > 0.0f )
        srand((unsigned int)seed);

    return min + (max-min)*float(rand())/float(RAND_MAX);
}
//----------------------------------------------------------------------------
float Math::FastSin0 (float angle)
{
    float fASqr = angle*angle;
    float fResult = 7.61e-03f;
    fResult *= fASqr;
    fResult -= 1.6605e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    fResult *= angle;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastSin1 (float angle)
{
    float fASqr = angle*angle;
    float fResult = -2.39e-08f;
    fResult *= fASqr;
    fResult += 2.7526e-06f;
    fResult *= fASqr;
    fResult -= 1.98409e-04f;
    fResult *= fASqr;
    fResult += 8.3333315e-03f;
    fResult *= fASqr;
    fResult -= 1.666666664e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    fResult *= angle;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastCos0 (float angle)
{
    float fASqr = angle*angle;
    float fResult = 3.705e-02f;
    fResult *= fASqr;
    fResult -= 4.967e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastCos1 (float angle)
{
    float fASqr = angle*angle;
    float fResult = -2.605e-07f;
    fResult *= fASqr;
    fResult += 2.47609e-05f;
    fResult *= fASqr;
    fResult -= 1.3888397e-03f;
    fResult *= fASqr;
    fResult += 4.16666418e-02f;
    fResult *= fASqr;
    fResult -= 4.999999963e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastTan0 (float angle)
{
    float fASqr = angle*angle;
    float fResult = 2.033e-01f;
    fResult *= fASqr;
    fResult += 3.1755e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    fResult *= angle;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastTan1 (float angle)
{
    float fASqr = angle*angle;
    float fResult = 9.5168091e-03f;
    fResult *= fASqr;
    fResult += 2.900525e-03f;
    fResult *= fASqr;
    fResult += 2.45650893e-02f;
    fResult *= fASqr;
    fResult += 5.33740603e-02f;
    fResult *= fASqr;
    fResult += 1.333923995e-01f;
    fResult *= fASqr;
    fResult += 3.333314036e-01f;
    fResult *= fASqr;
    fResult += 1.0f;
    fResult *= angle;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastInvSin (float value)
{
    float fRoot = Math::Sqrt(1.0f-value);
    float fResult = -0.0187293f;
    fResult *= value;
    fResult += 0.0742610f;
    fResult *= value;
    fResult -= 0.2121144f;
    fResult *= value;
    fResult += 1.5707288f;
    fResult = HALF_PI - fRoot*fResult;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastInvCos (float value)
{
    float fRoot = Math::Sqrt(1.0f-value);
    float fResult = -0.0187293f;
    fResult *= value;
    fResult += 0.0742610f;
    fResult *= value;
    fResult -= 0.2121144f;
    fResult *= value;
    fResult += 1.5707288f;
    fResult *= fRoot;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastInvTan0 (float value)
{
    float fVSqr = value*value;
    float fResult = 0.0208351f;
    fResult *= fVSqr;
    fResult -= 0.085133f;
    fResult *= fVSqr;
    fResult += 0.180141f;
    fResult *= fVSqr;
    fResult -= 0.3302995f;
    fResult *= fVSqr;
    fResult += 0.999866f;
    fResult *= value;
    return fResult;
}
//----------------------------------------------------------------------------
float Math::FastInvTan1 (float value)
{
    float fVSqr = value*value;
    float fResult = 0.0028662257f;
    fResult *= fVSqr;
    fResult -= 0.0161657367f;
    fResult *= fVSqr;
    fResult += 0.0429096138f;
    fResult *= fVSqr;
    fResult -= 0.0752896400f;
    fResult *= fVSqr;
    fResult += 0.1065626393f;
    fResult *= fVSqr;
    fResult -= 0.1420889944f;
    fResult *= fVSqr;
    fResult += 0.1999355085f;
    fResult *= fVSqr;
    fResult -= 0.3333314528f;
    fResult *= fVSqr;
    fResult += 1.0f;
    fResult *= value;
    return fResult;
}
//----------------------------------------------------------------------------
--- NEW FILE: MathEngine.h ---
#ifndef __MATH_H
#define __MATH_H

#include <math.h>

class Math
{
public:
    // Return -1 if the input is negative, 0 if the input is zero, and +1
    // if the input is positive.
    static int Sign (int iValue)
	{
		if ( iValue > 0 )
			return 1;

		if ( iValue < 0 )
			return -1;

		return 0;
	}
    static float Sign (float value)
	{
		if ( value > 0.0f )
			return 1.0f;

		if ( value < 0.0f )
			return -1.0f;

		return 0.0f;
	}

    // Just computes value*value.
    static float Sqr (float value)
	{
		return value*value;
	}

    // Generate a random number in [0,1).  The random number generator may
    // be seeded by a first call to UnitRandom with a positive seed.
    static float UnitRandom (float seed = 0.0f);

    // Generate a random number in [-1,1).  The random number generator may
    // be seeded by a first call to SymmetricRandom with a positive seed.
    static float SymmetricRandom (float seed = 0.0f);

    // Generate a random number in [min,max).  The random number generator may
    // be seeded by a first call to IntervalRandom with a positive seed.
    static float IntervalRandom (float min, float max, float seed = 0.0f);

    // Fast evaluation of sin(angle) by polynomial approximations.  The angle
    // must be in [0,pi/2].  The maximum absolute error is about 1.7e-04 for
    // FastSin0 and about 2.3e-09 for FastSin1.  The speed up is about 2 for
    // FastSin0 and about 1.5 for FastSin1.
    static float FastSin0 (float angle);
    static float FastSin1 (float angle);

    // Fast evaluation of cos(angle) by polynomial approximations.  The angle
    // must be in [0,pi/2].  The maximum absolute error is about 1.2e-03 for
    // FastCos0 and about 2.3e-09 for FastCos1.  The speed up is about 2 for
    // FastCos0 and about 1.5 for FastCos1.
    static float FastCos0 (float angle);
    static float FastCos1 (float angle);

    // Fast evaluation of tan(angle) by polynomial approximations.  The angle
    // must be in [0,pi/4].  The maximum absolute error is about 8.1e-04 for
    // FastTan0 and about 1.9e-08 for FastTan1.  The speed up is about 2.5 for
    // FastTan0 and about 1.75 for FastTan1.
    static float FastTan0 (float angle);
    static float FastTan1 (float angle);

    // Fast evaluation of asin(value) by a sqrt times a polynomial.  The value
    // must be in [0,1].  The maximum absolute error is about 6.8e-05 and the
    // speed up is about 2.5.
    static float FastInvSin (float value);

    // Fast evaluation of acos(value) by a sqrt times a polynomial.  The value
    // must be in [0,1].  The maximum absolute error is about 6.8e-05 and the
    // speed up is about 2.5.
    static float FastInvCos (float value);

    // Fast evaluation of atan(value) by polynomial approximations.  The value
    // must be in [-1,1].  The maximum absolute error is about 1.2e-05 for
    // FastInvTan0 and about 1.43-08 for FastInvTan1.  The speed up is about
    // 2.2 for FastInvTan0 and about 1.5 for FastInvTan1.
    static float FastInvTan0 (float value);
    static float FastInvTan1 (float value);

    // Wrappers for acos and asin, but the input value is clamped to [-1,1]
    // to avoid silent returns of NaN.
    static float ACos (float value)
	{
		if ( -1.0f < value )
		{
			if ( value < 1.0f )
				return (float)acos(value);
			else
				return 0.0f;
		}
		else
		{
			return PI;
		}
	}
    static float ASin (float value)
	{
		if ( -1.0f < value )
		{
			if ( value < 1.0f )
				return (float)asin(value);
			else
				return -HALF_PI;
		}
		else
		{
			return HALF_PI;
		}
	}

    // Wrapper for 1/sqrt to allow a fast implementation to replace it at a
    // later date.
	static float InvSqrt (float value)				{ return (float)(1.0/sqrt(value)); }

    // Wrappers to hide 'double foo(double)' versus 'float foof(float)'.
	static float ATan (float value)					{ return (float)atan(value); }
	static float ATan2 (float fY, float fX)			{ return (float)atan2(fY,fX); }
	static float Ceil (float value)					{ return (float)ceil(value); }
    static float Cos (float value)					{ return (float)cos(value); }
    static float Exp (float value)					{ return (float)exp(value); }
    static float Abs (float value)					{ return (float)fabs(value); }
    static float Floor (float value)					{ return (float)floor(value); }
    static float Log (float value)					{ return (float)log(value); }
    static float Pow (float base, float exponent)	{ return (float)pow(base,exponent); }
    static float Sin (float value)					{ return (float)sin(value); }
    static float Sqrt (float value)					{ return (float)sqrt(value); }
    static float Tan (float value)					{ return (float)tan(value); }

    // common constants
	static const float MAX_FLOAT;
    static const float PI;
    static const float TWO_PI;
    static const float HALF_PI;
    static const float INV_TWO_PI;
    static const float DEG_TO_RAD;
    static const float RAD_TO_DEG;
};

#endif



--- NEW FILE: Matrix3.cpp ---
#include <memory.h>
#include <assert.h>
#include "Matrix3.h"

const float Matrix3::EPSILON = 1e-06f;
const Matrix3 Matrix3::ZERO(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f);
const Matrix3 Matrix3::IDENTITY(1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f);
const float Matrix3::svdEpsilon = 1e-04f;
const int Matrix3::svdMaxIterations = 32;

//----------------------------------------------------------------------------
Matrix3::Matrix3 (const float entry[3][3])
{
    memcpy(m,entry,9*sizeof(float));
}
//----------------------------------------------------------------------------
Matrix3::Matrix3 (const Matrix3& matrix)
{
    memcpy(m,matrix.m,9*sizeof(float));
[...1451 lines suppressed...]
    // make eigenvectors form a right--handed system
    Vector3 kCross = eigenvector[1].Cross(eigenvector[2]);
    float det = eigenvector[0].Dot(kCross);
    if ( det < 0.0f )
    {
        eigenvector[2][0] = - eigenvector[2][0];
        eigenvector[2][1] = - eigenvector[2][1];
        eigenvector[2][2] = - eigenvector[2][2];
    }
}
//----------------------------------------------------------------------------
void Matrix3::TensorProduct (const Vector3& u, const Vector3& v, Matrix3& product)
{
    for (int row = 0; row < 3; row++)
    {
        for (int col = 0; col < 3; col++)
            product[row][col] = u[row]*v[col];
    }
}
//----------------------------------------------------------------------------
--- NEW FILE: Matrix3.h ---
#ifndef __MATRIX3_H
#define __MATRIX3_H

#include "Vector3.h"

// NOTE.  The (x,y,z) coordinate system is assumed to be right-handed.
// Coordinate axis rotation matrices are of the form
//   RX =    1       0       0
//           0     cos(t) -sin(t)
//           0     sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the yz-plane
//   RY =  cos(t)    0     sin(t)
//           0       1       0
//        -sin(t)    0     cos(t)
// where t > 0 indicates a counterclockwise rotation in the zx-plane
//   RZ =  cos(t) -sin(t)    0
//         sin(t)  cos(t)    0
//           0       0       1
// where t > 0 indicates a counterclockwise rotation in the xy-plane.


class Matrix3
{
public:
    // construction
	Matrix3 () { /* For efficiency reasons, do not initialize matrix. */ }
    Matrix3 (const float entry[3][3]);
    Matrix3 (const Matrix3& matrix);
    Matrix3 (float fEntry00, float fEntry01, float fEntry02,
             float fEntry10, float fEntry11, float fEntry12,
             float fEntry20, float fEntry21, float fEntry22);

    // member access, allows use of construct mat[r][c]
	float* operator[] (int row) const				{ return (float*)&m[row][0]; }
	operator float* ()								{ return &m[0][0]; }
    Vector3 GetColumn (int col) const;	

    // assignment and comparison
    Matrix3& operator= (const Matrix3& matrix);
    bool operator== (const Matrix3& matrix) const;
    bool operator!= (const Matrix3& matrix) const;

    // arithmetic operations
    Matrix3 operator+ (const Matrix3& matrix) const;
    Matrix3 operator- (const Matrix3& matrix) const;
    Matrix3 operator* (const Matrix3& matrix) const;
    Matrix3 operator- () const;

    // matrix * vector [3x3 * 3x1 = 3x1]
    Vector3 operator* (const Vector3& vector) const;

    // vector * matrix [1x3 * 3x3 = 1x3]
    friend Vector3 operator* (const Vector3& vector,
        const Matrix3& matrix);

    // matrix * scalar
    Matrix3 operator* (float scalar) const;

    // scalar * matrix
    friend Matrix3 operator* (float scalar, const Matrix3& matrix);

    // M0.TransposeTimes(M1) = M0^t*M1 where M0^t is the transpose of M0
    Matrix3 TransposeTimes (const Matrix3& mat) const;

    // M0.TimesTranspose(M1) = M0*M1^t where M1^t is the transpose of M1
    Matrix3 TimesTranspose (const Matrix3& mat) const;

    // utilities
    Matrix3 Transpose () const;
    bool Inverse (Matrix3& inverse, float tolerance = 1e-06f) const;
    Matrix3 Inverse (float tolerance = 1e-06f) const;
    float Determinant () const;

    // SLERP (spherical linear interpolation) without quaternions.  Computes
    // R(t) = R0*(Transpose(R0)*R1)^t.  If Q is a rotation matrix with
    // unit-length axis U and angle A, then Q^t is a rotation matrix with
    // unit-length axis U and rotation angle t*A.
    static Matrix3 Slerp (float T, const Matrix3& R0, const Matrix3& R1);

    // singular value decomposition
    void SingularValueDecomposition (Matrix3& L, Vector3& S,
        Matrix3& R) const;
    void SingularValueComposition (const Matrix3& L,
        const Vector3& S, const Matrix3& R);

    // Gram-Schmidt orthonormalization (applied to columns of rotation matrix)
    void Orthonormalize ();

    // orthogonal Q, diagonal D, upper triangular U stored as (u01,u02,u12)
    void QDUDecomposition (Matrix3& Q, Vector3& D, Vector3& U) const;

    float SpectralNorm () const;

    // matrix must be orthonormal
    void ToAxisAngle (Vector3& axis, float& radians) const;
    void FromAxisAngle (const Vector3& axis, float radians);

    // The matrix must be orthonormal.  The decomposition is yaw*pitch*roll
    // where yaw is rotation about the Up vector, pitch is rotation about the
    // Right axis, and roll is rotation about the Direction axis.
    bool ToEulerAnglesXYZ (float& yAngle, float& pAngle, float& rAngle) const;
    bool ToEulerAnglesXZY (float& yAngle, float& pAngle, float& rAngle) const;
    bool ToEulerAnglesYXZ (float& yAngle, float& pAngle, float& rAngle) const;
    bool ToEulerAnglesYZX (float& yAngle, float& pAngle, float& rAngle) const;
    bool ToEulerAnglesZXY (float& yAngle, float& pAngle, float& rAngle) const;
    bool ToEulerAnglesZYX (float& yAngle, float& pAngle, float& rAngle) const;
    void FromEulerAnglesXYZ (float yAngle, float pAngle, float rAngle);
    void FromEulerAnglesXZY (float yAngle, float pAngle, float rAngle);
    void FromEulerAnglesYXZ (float yAngle, float pAngle, float rAngle);
    void FromEulerAnglesYZX (float yAngle, float pAngle, float rAngle);
    void FromEulerAnglesZXY (float yAngle, float pAngle, float rAngle);
    void FromEulerAnglesZYX (float yAngle, float pAngle, float rAngle);

    // eigensolver, matrix must be symmetric
    void EigenSolveSymmetric (float eigenvalue[3], Vector3 eigenvector[3]) const;

    static void TensorProduct (const Vector3& rkU, const Vector3& rkV, Matrix3& rkProduct);

    static const float EPSILON;
    static const Matrix3 ZERO;
    static const Matrix3 IDENTITY;

protected:
    // support for eigensolver
    void Tridiagonal (float diag[3], float subDiag[3]);
    bool QLAlgorithm (float diag[3], float subDiag[3]);

    // support for singular value decomposition
    static const float svdEpsilon;
    static const int svdMaxIterations;
    static void Bidiagonalize (Matrix3& a, Matrix3& l, Matrix3& r);
    static void GolubKahanStep (Matrix3& a, Matrix3& l, Matrix3& r);

    // support for spectral norm
    static float MaxCubicRoot (float coeff[3]);

    float m[3][3];
};

#endif
--- NEW FILE: Quaternion.cpp ---
#include "Quaternion.h"

static float epsilon = 1e-03f;
Quaternion Quaternion::ZERO(0.0f,0.0f,0.0f,0.0f);
Quaternion Quaternion::IDENTITY(1.0f,0.0f,0.0f,0.0f);

//----------------------------------------------------------------------------
Quaternion::Quaternion (float W, float X, float Y, float Z)
{
    w = W;
    x = X;
    y = Y;
    z = Z;
}
//----------------------------------------------------------------------------
Quaternion::Quaternion (const Quaternion& q)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
}
//----------------------------------------------------------------------------
void Quaternion::FromRotationMatrix (const Matrix3& rot)
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".

    float trace = rot[0][0]+rot[1][1]+rot[2][2];
    float root;

    if ( trace > 0.0f )
    {
        // |w| > 1/2, may as well choose w > 1/2
		root = Math::Sqrt(trace + 1.0f);  // 2w
        w = 0.5f*root;
        root = 0.5f/root;  // 1/(4w)
        x = (rot[2][1]-rot[1][2])*root;
        y = (rot[0][2]-rot[2][0])*root;
        z = (rot[1][0]-rot[0][1])*root;
    }
    else
    {
        // |w| <= 1/2
        static int s_iNext[3] = { 1, 2, 0 };
        int i = 0;
        if ( rot[1][1] > rot[0][0] )
            i = 1;
        if ( rot[2][2] > rot[i][i] )
            i = 2;
        int j = s_iNext[i];
        int k = s_iNext[j];

        root = Math::Sqrt(rot[i][i]-rot[j][j]-rot[k][k] + 1.0f);
        float* apkQuat[3] = { &x, &y, &z };
        *apkQuat[i] = 0.5f*root;
        root = 0.5f/root;
        w = (rot[k][j]-rot[j][k])*root;
        *apkQuat[j] = (rot[j][i]+rot[i][j])*root;
        *apkQuat[k] = (rot[k][i]+rot[i][k])*root;
    }
}
//----------------------------------------------------------------------------
void Quaternion::ToRotationMatrix (Matrix3& rot) const
{
    float fTx  = 2.0f*x;
    float fTy  = 2.0f*y;
    float fTz  = 2.0f*z;
    float fTwx = fTx*w;
    float fTwy = fTy*w;
    float fTwz = fTz*w;
    float fTxx = fTx*x;
    float fTxy = fTy*x;
    float fTxz = fTz*x;
    float fTyy = fTy*y;
    float fTyz = fTz*y;
    float fTzz = fTz*z;

    rot[0][0] = 1.0f-(fTyy+fTzz);
    rot[0][1] = fTxy-fTwz;
    rot[0][2] = fTxz+fTwy;
    rot[1][0] = fTxy+fTwz;
    rot[1][1] = 1.0f-(fTxx+fTzz);
    rot[1][2] = fTyz-fTwx;
    rot[2][0] = fTxz-fTwy;
    rot[2][1] = fTyz+fTwx;
    rot[2][2] = 1.0f-(fTxx+fTyy);
}
//----------------------------------------------------------------------------
void Quaternion::FromAngleAxis (const float& angle, const Vector3& axis)
{
    // assert:  axis[] is unit length
    //
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    float fHalfAngle = 0.5f*angle;
    float fSin = Math::Sin(fHalfAngle);
    w = Math::Cos(fHalfAngle);
    x = fSin*axis.x;
    y = fSin*axis.y;
    z = fSin*axis.z;
}
//----------------------------------------------------------------------------
void Quaternion::ToAngleAxis (float& angle, Vector3& axis) const
{
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    float fSqrLength = x*x+y*y+z*z;
    if ( fSqrLength > 0.0f )
    {
        angle = 2.0f*Math::ACos(w);
        float fInvLength = Math::InvSqrt(fSqrLength);
        axis.x = x*fInvLength;
        axis.y = y*fInvLength;
        axis.z = z*fInvLength;
    }
    else
    {
        // angle is 0 (mod 2*pi), so any axis will do
        angle = 0.0f;
        axis.x = 1.0f;
        axis.y = 0.0f;
        axis.z = 0.0f;
    }
}
//----------------------------------------------------------------------------
void Quaternion::FromAxes (const Vector3* axis)
{
    Matrix3 rot;

    for (int iCol = 0; iCol < 3; iCol++)
    {
        rot[0][iCol] = axis[iCol].x;
        rot[1][iCol] = axis[iCol].y;
        rot[2][iCol] = axis[iCol].z;
    }

    FromRotationMatrix(rot);
}
//----------------------------------------------------------------------------
void Quaternion::ToAxes (Vector3* axis) const
{
    Matrix3 rot;

    ToRotationMatrix(rot);

    for (int iCol = 0; iCol < 3; iCol++)
    {
        axis[iCol].x = rot[0][iCol];
        axis[iCol].y = rot[1][iCol];
        axis[iCol].z = rot[2][iCol];
    }
}
//----------------------------------------------------------------------------
Quaternion& Quaternion::operator= (const Quaternion& q)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
    return *this;
}
//----------------------------------------------------------------------------
Quaternion Quaternion::operator+ (const Quaternion& q) const
{
    return Quaternion(w+q.w,x+q.x,y+q.y,z+q.z);
}
//----------------------------------------------------------------------------
Quaternion Quaternion::operator- (const Quaternion& q) const
{
    return Quaternion(w-q.w,x-q.x,y-q.y,z-q.z);
}
//----------------------------------------------------------------------------
Quaternion Quaternion::operator* (const Quaternion& q) const
{
    // NOTE:  Multiplication is not generally commutative, so in most
    // cases p*q != q*p.

    return Quaternion
    (
        w*q.w-x*q.x-y*q.y-z*q.z,
        w*q.x+x*q.w+y*q.z-z*q.y,
        w*q.y+y*q.w+z*q.x-x*q.z,
        w*q.z+z*q.w+x*q.y-y*q.x
    );
}
//----------------------------------------------------------------------------
Quaternion Quaternion::operator* (float scalar) const
{
    return Quaternion(scalar*w,scalar*x,scalar*y,scalar*z);
}
//----------------------------------------------------------------------------
Quaternion operator* (float scalar, const Quaternion& q)
{
    return Quaternion(scalar*q.w,scalar*q.x,scalar*q.y, scalar*q.z);
}
//----------------------------------------------------------------------------
Quaternion Quaternion::operator- () const
{
    return Quaternion(-w,-x,-y,-z);
}
//----------------------------------------------------------------------------
float Quaternion::Dot (const Quaternion& q) const
{
    return w*q.w+x*q.x+y*q.y+z*q.z;
}
//----------------------------------------------------------------------------
float Quaternion::Norm () const
{
    return w*w+x*x+y*y+z*z;
}
//----------------------------------------------------------------------------
Quaternion Quaternion::Inverse () const
{
    float fNorm = w*w+x*x+y*y+z*z;
    if ( fNorm > 0.0f )
    {
        float fInvNorm = 1.0f/fNorm;
        return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
    }
    else
    {
        // return an invalid result to flag the error
        return ZERO;
    }
}
//----------------------------------------------------------------------------
Quaternion Quaternion::UnitInverse () const
{
    // assert:  'this' is unit length
    return Quaternion(w,-x,-y,-z);
}
//----------------------------------------------------------------------------
Quaternion Quaternion::Exp () const
{
    // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
    // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

    float fAngle = Math::Sqrt(x*x+y*y+z*z);
    float fSin = Math::Sin(fAngle);

    Quaternion kResult;
    kResult.w = Math::Cos(fAngle);

    if ( Math::Abs(fSin) >= epsilon )
    {
        float fCoeff = fSin/fAngle;
        kResult.x = fCoeff*x;
        kResult.y = fCoeff*y;
        kResult.z = fCoeff*z;
    }
    else
    {
        kResult.x = x;
        kResult.y = y;
        kResult.z = z;
    }

    return kResult;
}
//----------------------------------------------------------------------------
Quaternion Quaternion::Log () const
{
    // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
    // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

    Quaternion kResult;
    kResult.w = 0.0f;

    if ( Math::Abs(w) < 1.0f )
    {
        float fAngle = Math::ACos(w);
        float fSin = Math::Sin(fAngle);
        if ( Math::Abs(fSin) >= epsilon )
        {
            float fCoeff = fAngle/fSin;
            kResult.x = fCoeff*x;
            kResult.y = fCoeff*y;
            kResult.z = fCoeff*z;
            return kResult;
        }
    }

    kResult.x = x;
    kResult.y = y;
    kResult.z = z;

    return kResult;
}
//----------------------------------------------------------------------------
Vector3 Quaternion::operator* (const Vector3& vector) const
{
    // Given a vector u = (x0,y0,z0) and a unit length quaternion
    // q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
    // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
    // multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
    // Note that q^{-1} = <w,-x,-y,-z>, so no float work is required to
    // invert q.  Now
    //
    //   q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
    //     = q*(x0*i+y0*j+z0*k)*q^{-1}
    //     = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
    //
    // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
    // of the rotation matrix computed in Quaternion::ToRotationMatrix.
    // The vector v is obtained as the product of that rotation matrix with
    // vector u.  As such, the quaternion representation of a rotation
    // matrix requires less space than the matrix and more time to compute
    // the rotated vector.  Typical space-time tradeoff...

    Matrix3 rot;
    ToRotationMatrix(rot);
    return rot*vector;
}
//----------------------------------------------------------------------------
Quaternion Quaternion::Slerp (float t, const Quaternion& p, const Quaternion& q)
{
    float fCos = p.Dot(q);
    float fAngle = Math::ACos(fCos);

    if ( Math::Abs(fAngle) < epsilon )
        return p;

    float fSin = Math::Sin(fAngle);
    float fInvSin = 1.0f/fSin;
    float fCoeff0 = Math::Sin((1.0f-t)*fAngle)*fInvSin;
    float fCoeff1 = Math::Sin(t*fAngle)*fInvSin;
    return fCoeff0*p + fCoeff1*q;
}
//----------------------------------------------------------------------------
Quaternion Quaternion::SlerpExtraSpins (float t, const Quaternion& p, const Quaternion& q, int iExtraSpins)
{
    float fCos = p.Dot(q);
    float fAngle = Math::ACos(fCos);

    if ( Math::Abs(fAngle) < epsilon )
        return p;

    float fSin = Math::Sin(fAngle);
    float fPhase = Math::PI*iExtraSpins*t;
    float fInvSin = 1.0f/fSin;
    float fCoeff0 = Math::Sin((1.0f-t)*fAngle - fPhase)*fInvSin;
    float fCoeff1 = Math::Sin(t*fAngle + fPhase)*fInvSin;
    return fCoeff0*p + fCoeff1*q;
}
//----------------------------------------------------------------------------
void Quaternion::Intermediate (const Quaternion& q0, const Quaternion& q1, const Quaternion& q2, Quaternion& a, Quaternion& b)
{
    // assert:  q0, q1, q2 are unit quaternions

    Quaternion kQ0inv = q0.UnitInverse();
    Quaternion kQ1inv = q1.UnitInverse();
    Quaternion rkP0 = kQ0inv*q1;
    Quaternion rkP1 = kQ1inv*q2;
    Quaternion kArg = 0.25f*(rkP0.Log()-rkP1.Log());
    Quaternion kMinusArg = -kArg;

    a = q1*kArg.Exp();
    b = q1*kMinusArg.Exp();
}
//----------------------------------------------------------------------------
Quaternion Quaternion::Squad (float t, const Quaternion& p, const Quaternion& a, const Quaternion& b, const Quaternion& q)
{
    float fSlerpT = 2.0f*t*(1.0f-t);
    Quaternion kSlerpP = Slerp(t,p,q);
    Quaternion kSlerpQ = Slerp(t,a,b);
    return Slerp(fSlerpT,kSlerpP,kSlerpQ);
}
//----------------------------------------------------------------------------
--- NEW FILE: Quaternion.h ---
#ifndef __QUATERNION_H
#define __QUATERNION_H

#include "Matrix3.h"

class Quaternion
{
public:
    float x, y, z, w;

    // construction and destruction
    Quaternion (float W = 1.0f, float X = 0.0f, float Y = 0.0f,
        float Z = 0.0f);
    Quaternion (const Quaternion& q);

    // conversion between quaternions, matrices, and angle-axes
    void FromRotationMatrix (const Matrix3& rot);
    void ToRotationMatrix (Matrix3& rot) const;
    void FromAngleAxis (const float& angle, const Vector3& axis);
    void ToAngleAxis (float& angle, Vector3& axis) const;
    void FromAxes (const Vector3* axis);
    void ToAxes (Vector3* axis) const;

    // arithmetic operations
    Quaternion& operator= (const Quaternion& q);
    Quaternion operator+ (const Quaternion& q) const;
    Quaternion operator- (const Quaternion& q) const;
    Quaternion operator* (const Quaternion& q) const;
    Quaternion operator* (float scalar) const;
    friend Quaternion operator* (float scalar, const Quaternion& q);
    Quaternion operator- () const;

    // functions of a quater...
 
[truncated message content]