Menu
▾
▴
[Devmaster-cvs] engine/src/core/Math MathEngine.cpp,NONE,1.1 MathEngine.h,NONE,1.1 Matrix3.cpp,NONE,1.1 Matrix3.h,NONE,1.1 Quaternion.cpp,NONE,1.1 Quaternion.h,NONE,1.1 Vector2.cpp,NONE,1.1 Vector2.h,NONE,1.1 Vector3.cpp,NONE,1.1 Vector3.h,NONE,1.1
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 quaternion
float Dot (const Quaternion& q) const; // dot product
float Norm () const; // squared-length
Quaternion Inverse () const; // apply to non-zero quaternion
Quaternion UnitInverse () const; // apply to unit-length quaternion
Quaternion Exp () const;
Quaternion Log () const;
// rotation of a vector by a quaternion
Vector3 operator* (const Vector3& vector) const;
// spherical linear interpolation
static Quaternion Slerp (float t, const Quaternion& p, const Quaternion& q);
static Quaternion SlerpExtraSpins (float t, const Quaternion& p, const Quaternion& q, int iExtraSpins);
// setup for spherical quadratic interpolation
static void Intermediate (const Quaternion& q0,
const Quaternion& q1, const Quaternion& q2,
Quaternion& a, Quaternion& b);
// spherical quadratic interpolation
static Quaternion Squad (float t, const Quaternion& p,
const Quaternion& a, const Quaternion& b,
const Quaternion& q);
// special values
static Quaternion ZERO;
static Quaternion IDENTITY;
};
#endif
--- NEW FILE: Vector2.cpp ---
#include "Vector2.h"
const Vector2 Vector2::ZERO(0.0f,0.0f);
const Vector2 Vector2::UNIT_X(1.0f,0.0f);
const Vector2 Vector2::UNIT_Y(0.0f,1.0f);
float Vector2::FUZZ = 0.0f;
//----------------------------------------------------------------------------
Vector2::Vector2 (float X, float Y)
{
x = X;
y = Y;
}
//----------------------------------------------------------------------------
Vector2::Vector2 (float coordinate[2])
{
x = coordinate[0];
y = coordinate[1];
}
//----------------------------------------------------------------------------
Vector2::Vector2 (const Vector2& vector)
{
x = vector.x;
y = vector.y;
}
//----------------------------------------------------------------------------
Vector2& Vector2::operator= (const Vector2& vector)
{
x = vector.x;
y = vector.y;
return *this;
}
//----------------------------------------------------------------------------
bool Vector2::operator== (const Vector2& vector) const
{
if ( FUZZ == 0.0f )
return x == vector.x && y == vector.y;
else
return Math::Abs(x-vector.x) <= FUZZ
&& Math::Abs(y-vector.y) <= FUZZ;
}
//----------------------------------------------------------------------------
bool Vector2::operator!= (const Vector2& vector) const
{
if ( FUZZ == 0.0f )
return x != vector.x || y != vector.y;
else
return Math::Abs(x-vector.x) > FUZZ
|| Math::Abs(y-vector.y) > FUZZ;
}
//----------------------------------------------------------------------------
bool Vector2::operator< (const Vector2& vector) const
{
float xtmp = vector.x, ytmp = vector.y;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
}
// compare y values
unsigned int uiTest0 = *(unsigned int*)&y;
unsigned int uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 < uiTest1;
}
//----------------------------------------------------------------------------
bool Vector2::operator<= (const Vector2& vector) const
{
float xtmp = vector.x, ytmp = vector.y;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
}
// compare y values
unsigned int uiTest0 = *(unsigned int*)&y;
unsigned int uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 <= uiTest1;
}
//----------------------------------------------------------------------------
bool Vector2::operator> (const Vector2& vector) const
{
float xtmp = vector.x, ytmp = vector.y;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
}
// compare y values
unsigned int uiTest0 = *(unsigned int*)&y;
unsigned int uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 > uiTest1;
}
//----------------------------------------------------------------------------
bool Vector2::operator>= (const Vector2& vector) const
{
float xtmp = vector.x, ytmp = vector.y;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
}
// compare y values
unsigned int uiTest0 = *(unsigned int*)&y;
unsigned int uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 >= uiTest1;
}
//----------------------------------------------------------------------------
Vector2 Vector2::operator+ (const Vector2& vector) const
{
return Vector2(x+vector.x,y+vector.y);
}
//----------------------------------------------------------------------------
Vector2 Vector2::operator- (const Vector2& vector) const
{
return Vector2(x-vector.x,y-vector.y);
}
//----------------------------------------------------------------------------
Vector2 Vector2::operator* (float scalar) const
{
return Vector2(scalar*x,scalar*y);
}
//----------------------------------------------------------------------------
Vector2 Vector2::operator- () const
{
return Vector2(-x,-y);
}
//----------------------------------------------------------------------------
Vector2 operator* (float scalar, const Vector2& vector)
{
return Vector2(scalar*vector.x,scalar*vector.y);
}
//----------------------------------------------------------------------------
Vector2 Vector2::operator/ (float scalar) const
{
Vector2 quot;
if ( scalar != 0.0f )
{
float invScalar = 1.0f/scalar;
quot.x = invScalar*x;
quot.y = invScalar*y;
return quot;
}
else
{
return Vector2(Math::MAX_FLOAT,Math::MAX_FLOAT);
}
}
//----------------------------------------------------------------------------
Vector2& Vector2::operator+= (const Vector2& vector)
{
x += vector.x;
y += vector.y;
return *this;
}
//----------------------------------------------------------------------------
Vector2& Vector2::operator-= (const Vector2& vector)
{
x -= vector.x;
y -= vector.y;
return *this;
}
//----------------------------------------------------------------------------
Vector2& Vector2::operator*= (float scalar)
{
x *= scalar;
y *= scalar;
return *this;
}
//----------------------------------------------------------------------------
Vector2& Vector2::operator/= (float scalar)
{
if ( scalar != 0.0f )
{
float invScalar = 1.0f/scalar;
x *= invScalar;
y *= invScalar;
}
else
{
x = Math::MAX_FLOAT;
y = Math::MAX_FLOAT;
}
return *this;
}
//----------------------------------------------------------------------------
float Vector2::Dot (const Vector2& vector) const
{
return x*vector.x + y*vector.y;
}
//----------------------------------------------------------------------------
float Vector2::Length () const
{
return Math::Sqrt(x*x +y*y);
}
//----------------------------------------------------------------------------
Vector2 Vector2::Cross () const
{
return Vector2(y,-x);
}
//----------------------------------------------------------------------------
Vector2 Vector2::UnitCross () const
{
Vector2 cross(y,-x);
cross.Normalize();
return cross;
}
//----------------------------------------------------------------------------
float Vector2::Normalize (float tolerance)
{
float length = Length();
if ( length > tolerance )
{
float invLength = 1.0f/length;
x *= invLength;
y *= invLength;
}
else
{
length = 0.0f;
}
return length;
}
//----------------------------------------------------------------------------
Vector2 Vector2::GetNormalized (float tolerance)
{
Vector2 vector = *this;
vector.Normalize();
return vector;
}
//----------------------------------------------------------------------------
void Vector2::Orthonormalize (Vector2 vector[/*2*/])
{
// If the input vectors are v0 and v1, then the Gram-Schmidt
// orthonormalization produces vectors u0 and u1 as follows,
//
// u0 = v0/|v0|
// u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
//
// where |A| indicates length of vector A and A*B indicates dot
// product of vectors A and B.
// compute u0
vector[0].Normalize();
// compute u1
float dot0 = vector[0].Dot(vector[1]);
vector[1] -= dot0*vector[0];
vector[1].Normalize();
}
--- NEW FILE: Vector2.h ---
#ifndef __VECTOR2_H
#define __VECTOR2_H
#include "EngineMath.h"
class Vector2
{
public:
Vector2 ()
{
// For efficiency in construction of large arrays of vectors, the
// default constructor does not initialize the vector.
}
Vector2 (float X, float Y);
Vector2 (float coordinate[2]);
Vector2 (const Vector2& vector);
// coordinates
float x, y;
// access vector V as V[0] = V.x, V[1] = V.y
//
// WARNING. These member functions rely on
// (1) Vector2 not having virtual functions
// (2) the data packed in a 2*sizeof(float) memory block
float& operator[] (int i) const { return ((float*)this)[i]; }
operator float* () { return (float*)this; }
// assignment
Vector2& operator= (const Vector2& vector);
// comparison (supports fuzzy arithmetic when FUZZ > 0)
bool operator== (const Vector2& vector) const;
bool operator!= (const Vector2& vector) const;
bool operator< (const Vector2& vector) const;
bool operator<= (const Vector2& vector) const;
bool operator> (const Vector2& vector) const;
bool operator>= (const Vector2& vector) const;
// arithmetic operations
Vector2 operator+ (const Vector2& vector) const;
Vector2 operator- (const Vector2& vector) const;
Vector2 operator* (float scalar) const;
Vector2 operator/ (float scalar) const;
Vector2 operator- () const;
friend Vector2 operator* (float scalar, const Vector2& vector);
// arithmetic updates
Vector2& operator+= (const Vector2& vector);
Vector2& operator-= (const Vector2& vector);
Vector2& operator*= (float scalar);
Vector2& operator/= (float scalar);
// vector operations
float Length () const;
float SquaredLength () const { return x*x + y*y; }
float Dot (const Vector2& vector) const;
float Normalize (float tolerance = 1e-06f);
Vector2 GetNormalized (float tolerance = 1e-06f);
Vector2 Cross () const; // returns (y,-x)
Vector2 UnitCross () const; // returns (y,-x)/sqrt(x*x+y*y)
// Gram-Schmidt orthonormalization.
static void Orthonormalize (Vector2 vector[/*2*/]);
// special points
static const Vector2 ZERO;
static const Vector2 UNIT_X;
static const Vector2 UNIT_Y;
// fuzzy arithmetic (set FUZZ > 0 to enable)
static float FUZZ;
};
#endif
--- NEW FILE: Vector3.cpp ---
// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000-2002. All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement. The various license agreements may be found at
// the Magic Software web site. This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf
#include "Vector3.h"
const Vector3 Vector3::ZERO(0.0f,0.0f,0.0f);
const Vector3 Vector3::UNIT_X(1.0f,0.0f,0.0f);
const Vector3 Vector3::UNIT_Y(0.0f,1.0f,0.0f);
const Vector3 Vector3::UNIT_Z(0.0f,0.0f,1.0f);
float Vector3::FUZZ = 0.0f;
//----------------------------------------------------------------------------
Vector3::Vector3 (float X, float Y, float Z)
{
x = X;
y = Y;
z = Z;
}
//----------------------------------------------------------------------------
Vector3::Vector3 (float coordinate[3])
{
x = coordinate[0];
y = coordinate[1];
z = coordinate[2];
}
//----------------------------------------------------------------------------
Vector3::Vector3 (const Vector3& vector)
{
x = vector.x;
y = vector.y;
z = vector.z;
}
//----------------------------------------------------------------------------
Vector3& Vector3::operator= (const Vector3& vector)
{
x = vector.x;
y = vector.y;
z = vector.z;
return *this;
}
//----------------------------------------------------------------------------
bool Vector3::operator== (const Vector3& vector) const
{
if ( FUZZ == 0.0f )
{
return x == vector.x && y == vector.y && z == vector.z;
}
else
{
return Math::Abs(x - vector.x) <= FUZZ
&& Math::Abs(y - vector.y) <= FUZZ
&& Math::Abs(z - vector.z) <= FUZZ;
}
}
//----------------------------------------------------------------------------
bool Vector3::operator!= (const Vector3& vector) const
{
if ( FUZZ == 0.0f )
{
return x != vector.x || y != vector.y || z != vector.z;
}
else
{
return Math::Abs(x - vector.x) > FUZZ
|| Math::Abs(y - vector.y) > FUZZ
|| Math::Abs(z - vector.z) > FUZZ;
}
}
//----------------------------------------------------------------------------
bool Vector3::operator< (const Vector3& vector) const
{
float xtmp = vector.x, ytmp = vector.y, ztmp = vector.z;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
if ( Math::Abs(z - ztmp) <= FUZZ )
ztmp = z;
}
// compare z values
unsigned int uiTest0 = *(unsigned int*)&z;
unsigned int uiTest1 = *(unsigned int*)&ztmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare y values
uiTest0 = *(unsigned int*)&y;
uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 < uiTest1;
}
//----------------------------------------------------------------------------
bool Vector3::operator<= (const Vector3& vector) const
{
float xtmp = vector.x, ytmp = vector.y, ztmp = vector.z;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
if ( Math::Abs(z - ztmp) <= FUZZ )
ztmp = z;
}
// compare z values
unsigned int uiTest0 = *(unsigned int*)&z;
unsigned int uiTest1 = *(unsigned int*)&ztmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare y values
uiTest0 = *(unsigned int*)&y;
uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 < uiTest1 )
return true;
if ( uiTest0 > uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 <= uiTest1;
}
//----------------------------------------------------------------------------
bool Vector3::operator> (const Vector3& vector) const
{
float xtmp = vector.x, ytmp = vector.y, ztmp = vector.z;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
if ( Math::Abs(z - ztmp) <= FUZZ )
ztmp = z;
}
// compare z values
unsigned int uiTest0 = *(unsigned int*)&z;
unsigned int uiTest1 = *(unsigned int*)&ztmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare y values
uiTest0 = *(unsigned int*)&y;
uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 > uiTest1;
}
//----------------------------------------------------------------------------
bool Vector3::operator>= (const Vector3& vector) const
{
float xtmp = vector.x, ytmp = vector.y, ztmp = vector.z;
if ( FUZZ > 0.0f )
{
if ( Math::Abs(x - xtmp) <= FUZZ )
xtmp = x;
if ( Math::Abs(y - ytmp) <= FUZZ )
ytmp = y;
if ( Math::Abs(z - ztmp) <= FUZZ )
ztmp = z;
}
// compare z values
unsigned int uiTest0 = *(unsigned int*)&z;
unsigned int uiTest1 = *(unsigned int*)&ztmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare y values
uiTest0 = *(unsigned int*)&y;
uiTest1 = *(unsigned int*)&ytmp;
if ( uiTest0 > uiTest1 )
return true;
if ( uiTest0 < uiTest1 )
return false;
// compare x values
uiTest0 = *(unsigned int*)&x;
uiTest1 = *(unsigned int*)&xtmp;
return uiTest0 >= uiTest1;
}
//----------------------------------------------------------------------------
Vector3 Vector3::operator+ (const Vector3& vector) const
{
return Vector3(x+vector.x,y+vector.y,z+vector.z);
}
//----------------------------------------------------------------------------
Vector3 Vector3::operator- (const Vector3& vector) const
{
return Vector3(x-vector.x,y-vector.y,z-vector.z);
}
//----------------------------------------------------------------------------
Vector3 Vector3::operator* (float scalar) const
{
return Vector3(scalar*x,scalar*y,scalar*z);
}
//----------------------------------------------------------------------------
Vector3 Vector3::operator/ (float scalar) const
{
Vector3 quot;
if ( scalar != 0.0f )
{
float invScalar = 1.0f/scalar;
quot.x = invScalar*x;
quot.y = invScalar*y;
quot.z = invScalar*z;
return quot;
}
else
{
return Vector3(Math::MAX_FLOAT,Math::MAX_FLOAT,Math::MAX_FLOAT);
}
}
//----------------------------------------------------------------------------
Vector3 Vector3::operator- () const
{
return Vector3(-x,-y,-z);
}
//----------------------------------------------------------------------------
Vector3 operator* (float scalar, const Vector3& vector)
{
return Vector3(scalar*vector.x,scalar*vector.y,
scalar*vector.z);
}
//----------------------------------------------------------------------------
Vector3& Vector3::operator+= (const Vector3& vector)
{
x += vector.x;
y += vector.y;
z += vector.z;
return *this;
}
//----------------------------------------------------------------------------
Vector3& Vector3::operator-= (const Vector3& vector)
{
x -= vector.x;
y -= vector.y;
z -= vector.z;
return *this;
}
//----------------------------------------------------------------------------
Vector3& Vector3::operator*= (float scalar)
{
x *= scalar;
y *= scalar;
z *= scalar;
return *this;
}
//----------------------------------------------------------------------------
Vector3& Vector3::operator/= (float scalar)
{
if ( scalar != 0.0f )
{
float invScalar = 1.0f/scalar;
x *= invScalar;
y *= invScalar;
z *= invScalar;
}
else
{
x = Math::MAX_FLOAT;
y = Math::MAX_FLOAT;
z = Math::MAX_FLOAT;
}
return *this;
}
//----------------------------------------------------------------------------
float Vector3::SquaredLength () const
{
return x*x + y*y + z*z;
}
//----------------------------------------------------------------------------
float Vector3::Length () const
{
return Math::Sqrt(x*x + y*y + z*z);
}
//----------------------------------------------------------------------------
float Vector3::Dot (const Vector3& vector) const
{
return x*vector.x + y*vector.y + z*vector.z;
}
//----------------------------------------------------------------------------
Vector3 Vector3::Cross (const Vector3& vector) const
{
return Vector3(y*vector.z-z*vector.y,z*vector.x-x*vector.z,
x*vector.y-y*vector.x);
}
//----------------------------------------------------------------------------
Vector3 Vector3::UnitCross (const Vector3& vector) const
{
Vector3 kCross(y*vector.z-z*vector.y,z*vector.x-x*vector.z,
x*vector.y-y*vector.x);
kCross.Normalize();
return kCross;
}
//----------------------------------------------------------------------------
float Vector3::Normalize (float tolerance)
{
float length = Length();
if ( length > tolerance )
{
float invLength = 1.0f/length;
x *= invLength;
y *= invLength;
z *= invLength;
}
else
{
length = 0.0f;
}
return length;
}
//----------------------------------------------------------------------------
Vector3 Vector3::GetNormalized (float tolerance)
{
Vector3 vector = *this;
vector.Normalize();
return vector;
}
//----------------------------------------------------------------------------
void Vector3::Orthonormalize (Vector3 vector[/*3*/])
{
// If the input vectors are v0, v1, and v2, then the Gram-Schmidt
// orthonormalization produces vectors u0, u1, and u2 as follows,
//
// u0 = v0/|v0|
// u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
// u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1|
//
// where |A| indicates length of vector A and A*B indicates dot
// product of vectors A and B.
// compute u0
vector[0].Normalize();
// compute u1
float fDot0 = vector[0].Dot(vector[1]);
vector[1] -= fDot0*vector[0];
vector[1].Normalize();
// compute u2
float fDot1 = vector[1].Dot(vector[2]);
fDot0 = vector[0].Dot(vector[2]);
vector[2] -= fDot0*vector[0] + fDot1*vector[1];
vector[2].Normalize();
}
//----------------------------------------------------------------------------
void Vector3::GenerateOrthonormalBasis (Vector3& u, Vector3& v,
Vector3& w, bool unitLengthW)
{
if ( !unitLengthW )
w.Normalize();
float invLength;
if ( Math::Abs(w.x) >= Math::Abs(w.y) )
{
// W.x or W.z is the largest magnitude component, swap them
invLength = Math::InvSqrt(w.x*w.x+w.z*w.z);
u.x = -w.z*invLength;
u.y = 0.0f;
u.z = +w.x*invLength;
}
else
{
// W.y or W.z is the largest magnitude component, swap them
invLength = Math::InvSqrt(w.y*w.y+w.z*w.z);
u.x = 0.0f;
u.y = +w.z*invLength;
u.z = -w.y*invLength;
}
v = w.Cross(u);
}
//----------------------------------------------------------------------------
--- NEW FILE: Vector3.h ---
#ifndef __VECTOR3_H
#define __VECTOR3_H
#include "EngineMath.h"
class Vector3
{
public:
// construction
Vector3 ()
{
// For efficiency in construction of large arrays of vectors, the
// default constructor does not initialize the vector.
}
Vector3 (float X, float Y, float Z);
Vector3 (float coordinate[3]);
Vector3 (const Vector3& vector);
// coordinates
float x, y, z;
// access vector V as V[0] = V.x, V[1] = V.y, V[2] = V.z
//
// WARNING. These member functions rely on
// (1) Vector3 not having virtual functions
// (2) the data packed in a 3*sizeof(float) memory block
float& operator[] (int i) const { return ((float*)this)[i]; }
operator float* () { return (float*)this; }
// assignment
Vector3& operator= (const Vector3& vector);
// comparison (supports fuzzy arithmetic when FUZZ > 0)
bool operator== (const Vector3& vector) const;
bool operator!= (const Vector3& vector) const;
bool operator< (const Vector3& vector) const;
bool operator<= (const Vector3& vector) const;
bool operator> (const Vector3& vector) const;
bool operator>= (const Vector3& vector) const;
// arithmetic operations
Vector3 operator+ (const Vector3& vector) const;
Vector3 operator- (const Vector3& vector) const;
Vector3 operator* (float scalar) const;
Vector3 operator/ (float scalar) const;
Vector3 operator- () const;
friend Vector3 operator* (float scalar, const Vector3& vector);
// arithmetic updates
Vector3& operator+= (const Vector3& vector);
Vector3& operator-= (const Vector3& vector);
Vector3& operator*= (float scalar);
Vector3& operator/= (float scalar);
// vector operations
float Length () const;
float SquaredLength () const;
float Dot (const Vector3& vector) const;
float Normalize (float tolerance = 1e-06f);
Vector3 GetNormalized (float tolerance = 1e-06f);
Vector3 Cross (const Vector3& vector) const;
Vector3 UnitCross (const Vector3& vector) const;
// Gram-Schmidt orthonormalization.
static void Orthonormalize (Vector3 vector[/*3*/]);
// Input W must be initialize to a nonzero vector, output is {U,V,W}
// an orthonormal basis. A hint is provided about whether or not W
// is already unit length.
static void GenerateOrthonormalBasis (Vector3& u, Vector3& v,
Vector3& w, bool unitLengthW = true);
// special points
static const Vector3 ZERO;
static const Vector3 UNIT_X;
static const Vector3 UNIT_Y;
static const Vector3 UNIT_Z;
// fuzzy arithmetic (set FUZZ > 0 to enable)
static float FUZZ;
};
#endif
|
