JurassicParkTrespasser/jp2_pc/Source/Lib/Transform5/Rotate.hpp

872 lines
23 KiB
C++
Raw Permalink Normal View History

2018-01-01 22:07:24 +00:00
/***********************************************************************************************
*
* Copyright <EFBFBD> DreamWorks Interactive. 1996
*
* Contents:
* CRotate3<TR>
* CPlacement3<TR>
*
* Bugs:
*
* To do:
* Incorporate fast sqrt routines when they have enough precision.
* Optimise conversion from Euler angles.
*
***********************************************************************************************
*
* $Log:: /JP2_PC/Source/Lib/Transform5/Rotate.hpp $
*
* 26 97-04-24 18:40 Speter
* Folded new changes from 4.2 version into this 5.0 specific version.
*
* 31 97-04-23 14:28 Speter
* Changed tolerance for normalisation asserts (sorry).
*
* 30 97-04-22 10:50 Speter
* Sped up Dir * Rotate function by avoiding dir renormalisation.
*
* 29 4/16/97 2:24p Agrant
* Restored SFrame to allow CRotate --> CMatrix conversion
*
* 28 97/03/24 14:54 Speter
* Removed constructors of CDirs and CRotates from ASCII chars; use d3ZAxis etc. constants.
* When we need optimisation for axes, we'll use derived classes which don't need a switch
* statement.
*
* 27 97/02/19 17:54 Speter
* Made direct constructors public.
*
* 26 97/02/07 19:15 Speter
* Removed SFrame3, associated CRotate3 functions, as they were inefficient and were replaced
* with equivalent CMatrix functions. Removed CMatrix3 to CRotate3 conversion, as it used
* SFrame3 and wasn't used.
*
* 25 97/01/26 20:00 Speter
* Added operator to interpolate rotation by a scalar.
* Sped up rotate vector, rotation to matrix, and rotation from vector operations.
* Aligned code.
*
* 24 12/20/96 3:52p Mlange
* Moved the CRotate3 constructor that takes a character axis and angle to the class
* declaration in the header file.
*
* 23 96/12/10 11:06 Speter
* Upped fMAX_DENORMALISATION to .01 from .0001.
*
* 22 12/09/96 8:46p Agrant
* Added assert in constructor for CRotate taking a CQuaternion
*
* 21 12/09/96 12:43p Agrant
* Removed the 4 TR constructor, paid 5 bucks.
*
* 20 12/06/96 9:51p Agrant
* Hacks to CRotate for physics integration.
* It turns out that we have two different Quaternions.
* Will fix it up soon.
*
* 19 96/12/04 20:19 Speter
* Changed v3T to v3Pos in all transforms. Changed r3T to r3Rot.
* Changed CPlacement3(CTranslate3) to CPlacement3(CVector3).
*
*
*
* 18 96/11/20 11:52 Speter
* Added constructor for CRotate3 from CMatrix3.
*
* 17 96/10/18 19:03 Speter
* Took out include of <new.h>, now that it's in Common.hpp.
*
* 16 96/09/25 19:50 Speter
* The Big Change.
* In transforms, replaced TReal with TR, TDefReal with TReal.
* Replaced all references to transform templates that have <TObjReal> with <>.
* Replaced TObjReal with TReal, and "or" prefix with "r".
* Replaced CObjPoint, CObjNormal, and CObjPlacement with equivalent transform types, and
* prefixes likewise.
* Removed some unnecessary casts to TReal.
* Finally, replaced VER_GENERAL_DEBUG_ASSERTS with VER_DEBUG.
*
* 15 96/09/12 16:27 Speter
* Replaced bFuzzyEquals with Fuzzy ==.
*
* 14 96/09/09 18:32 Speter
* Added SFrame3 structure, and CRotate3 constructor which rotates one frame to another.
*
* 13 96/08/21 18:22 Speter
* Changes from code review:
* Added default template parameter to all classes.
* Made all in-class friends inline.
* Removed CRotate3(char, CAngle) and CRotate3(char*, CAngle, CAngle, CAngle). Replaced with
* CRotate3(uint32, CAngle, CAngle = 0, CAngle = 0), where first param is a multi-byte
* character. Removed 's' and 'r' specifiers for static and rotating frames. Rotating frames
* must now be specified by reversing the order of axes. See notes.
* Added CRotate3 * CDir3 operators, to disambiguate between CPlacement3 * CDir3.
* Changed tLen2() to tLenSqr().
* Changed calculation slightly in Normalise(), added Asserts.
* Added SetOrigin() and AdjustOrigin() functions.
* Added many comments, fixed some prefixes.
*
* 12 96/08/06 18:23 Speter
* Reversed order of d3_to and d3_from in constructor.
* Added CPlacement3 constructor which takes a CTranslate3.
*
* 11 96/08/01 11:04 Speter
* Added automatic renormalisation.
*
* 10 96/07/23 11:01 Speter
* Added static denormalisation variables for debugging.
*
* 9 96/07/12 17:35 Speter
* Added Rotate3 constructor taking a rotation vector.
*
* 8 96/07/08 12:40 Speter
* Changed name of CNormal3 to CDir3 (more general).
* Added specific functions to transform directions, which do not translate positions.
*
* 7 96/06/26 22:07 Speter
* Added a bunch of comments, prettied things up nicely.
*
* 6 96/06/26 17:03 Speter
* Really fixed rotation.
*
* 5 96/06/26 16:43 Speter
* Fixed matrix and vector operations. They were transposed!
*
* 4 96/06/26 13:16 Speter
* Changed TGReal to TR and prefix gr to r.
*
* 3 96/06/25 14:35 Speter
* Finalised design of transform classes, with Position3 and Transform3.
*
* 2 96/06/20 17:13 Speter
* Converted into templates and made necessary changes.
*
* 1 96/06/20 15:26 Speter
* First version of new optimised transform library.
*
**********************************************************************************************/
#ifndef HEADER_LIB_TRANSFORM_ROTATE_HPP
#define HEADER_LIB_TRANSFORM_ROTATE_HPP
#include <math.h>
#include "Matrix.hpp"
#include "Translate.hpp"
#include "Lib/Math/FastTrig.hpp"
//
// A version flag to enable counting of quaternion multiplications.
// Set to 1 if we ever want to revisit quaternion renormalisation methods.
// (For now, quaternions are checked for denormalisation after every multiply.)
//
#define VER_QUATERNION_COUNT VER_DEBUG
// Threshold at which we renormalise the quaternion.
#define fMAX_QUAT_DENORMALISATION (fMAX_VECTOR_DENORMALISATION * 0.1)
//**********************************************************************************************
//
template<class TR = TReal> struct SFrame3
//
// Prefix: fr3
//
//**************************************
{
CDir3<TR> d3Z, d3Y; // The directions corresponding to the Z and Y axes.
SFrame3()
: d3Z(0,0,1), d3Y(0,1,0)
{
}
SFrame3(const CDir3<TR>& d3_z, const CDir3<TR>& d3_y)
: d3Z(d3_z), d3Y(d3_y)
{
// These directions must be perpendicular!
Assert(Fuzzy(d3Z * d3Y) == 0);
}
};
template <class TR = TReal>
class CQuaternion
{
public:
TR tE0; // Same as tC in CRotate
TR tE1; // X
TR tE2; // Y
TR tE3; // Z
CQuaternion(float e0, float e1, float e2, float e3) : tE0(e0), tE1(e1), tE2(e2), tE3(e3)
{};
CQuaternion()
{};
// CPlacement3(const CRotate3<TR>& r3, const CVector3<TR>& v3)
};
//**********************************************************************************************
//
template<class TR = TReal> class CRotate3
//
// Prefix: r3
//
// An arbitrary rotation (any axis and angle).
// Implemented as a quaternion.
//
// Note: Since we use a right-handed coordinate system, an angle of rotation about a vector
// specifies amount of clockwise rotation when looking in the direction of the vector.
//
//**************************************
{
public:
#if VER_DEBUG
// Several static variables for statistics gathering.
static int iMaxNormalisationCount, iAvgNormalisationCount, iMaxCheckCount;
static float fTotalNormalisationCount, fTotalNormalisations;
static int iMaxDenormalisationCount;
static TR tMaxDenormalisation;
#endif
protected:
TR tC; // Scalar part (cosine of half rotation angle).
CVector3<TR> v3S; // Vector part (sine of half rotation angle times vector).
#if VER_QUATERNION_COUNT
int iCount;
#endif
public:
//******************************************************************************************
//
// Constructors.
//
//******************************************************************************************
//
CRotate3()
//
// Identity rotation.
//
//**********************************
: tC((TR)1), v3S((TR)0, (TR)0, (TR)0)
{
InitCounter();
}
//******************************************************************************************
//
CRotate3
(
const CQuaternion<>& q
)
//**********************************
: v3S(q.tE1, q.tE2, q.tE3), tC(q.tE0)
{
Assert(bIsNormalised());
}
//******************************************************************************************
//
CRotate3<TR>(TR t_w, const CVector3<TR>& v3)
//
// Initialise quaternion components directly.
// Invoked by operator~ and .r3Rotate.
//
//**********************************
: tC(t_w), v3S(v3)
{
Assert(bIsNormalised());
}
//******************************************************************************************
//
CRotate3<TR>
(
TR t_w, TR t_x, TR t_y, TR t_z
#if VER_QUATERNION_COUNT
,
int i_count
#endif
)
//
// Initialise quaternion components even more directly.
// Invoked by quaternion multiplication operator.
// Therefore, we do renormalisation here.
//
//**********************************
: tC(t_w), v3S(t_x, t_y, t_z)
#if VER_QUATERNION_COUNT
,
iCount(i_count)
#endif
{
Normalise();
#if VER_DEBUG
TR t = Abs(tLenSqr() - (TR)1);
if (t > tMaxDenormalisation)
{
tMaxDenormalisation = t;
iMaxDenormalisationCount = iCount;
}
#endif
}
//******************************************************************************************
CRotate3
(
const CDir3<TR>& d3, // Axis about which to rotate.
CAngle ang // Angle of rotation (clockwise when looking down vector).
);
//**********************************
//******************************************************************************************
CRotate3
(
const CVector3<TR>& v3 // Rotation vector to apply.
// This is a vector whose direction is the axis of rotation,
// and whose magnitude specifies the amount of rotation,
// in radians.
);
//**********************************
//******************************************************************************************
CRotate3
(
const CDir3<TR>& d3_from, // Starting direction.
const CDir3<TR>& d3_to // Destination direction.
);
//
// Constructs a rotation that moves d3_from to d3_to, via the shortest path (great circle).
//
//**********************************
//******************************************************************************************
CRotate3
(
const SFrame3<TR>& fr3_from, // Starting frame.
const SFrame3<TR>& fr3_to // Ending frame.
);
//
// Constructs a rotation that moves the two axes in fr3_from to the corresponding axes
// in fr3_to. Unlike the constructor taking two directions, we do not have to choose an
// arbitrary (great circle) rotation. This rotation is unique.
//
//**********************************
//******************************************************************************************
CRotate3
(
const CMatrix3<TR>& mx3 // A transformation matrix.
);
//
// Constructs a rotation from a matrix.
//
//**********************************
//******************************************************************************************
//
// Conversion operators.
//
// Bug: Due to a confirmed Microsoft bug, conversion operators in template classes
// must be declared inline in the class. That's why the following function is here
// (even though I'd rather put it outside the class).
operator CMatrix3<TR> () const
{
// From Graphics Gems II, Sec VII.6.
// Store products we need more than once.
TR t_cc = tC * tC;
TR t_cx = tC * v3S.tX;
TR t_cy = tC * v3S.tY;
TR t_cz = tC * v3S.tZ;
TR t_xx = v3S.tX * v3S.tX;
TR t_xy = v3S.tX * v3S.tY;
TR t_xz = v3S.tX * v3S.tZ;
TR t_yy = v3S.tY * v3S.tY;
TR t_yz = v3S.tY * v3S.tZ;
TR t_zz = v3S.tZ * v3S.tZ;
return CMatrix3<TR>
(
2 * (t_cc + t_xx - 0.5), 2 * (t_xy + t_cz ), 2 * (t_xz - t_cy ),
2 * (t_xy - t_cz ), 2 * (t_cc + t_yy - 0.5), 2 * (t_yz + t_cx ),
2 * (t_xz + t_cy ), 2 * (t_yz - t_cx ), 2 * (t_cc + t_zz - 0.5)
);
}
//******************************************************************************************
//
// Operators.
//
// Return the inverse of the rotate.
CRotate3<TR> operator ~() const
{
// For unit quaternions, the inverse is equal to the conjugate (negated vector part).
return CRotate3<TR>(tC, -v3S);
}
//
// Concatenate with another rotation transform.
//
CRotate3<TR> operator *(const CRotate3<TR>& r3) const;
CRotate3<TR>& operator *=(const CRotate3<TR>& r3)
{
return *this = *this * r3;
}
//
// Interpolate rotation by a parameter. Need not be between 0 and 1.
// Note: For interpolations in a loop, this is slow:
//
// CRotate3<> rot_total;
// CDir3<> d3_orig, d3_new;
// for (float f = 0; f < 1; f += 0.1)
// d3_new = d3_orig * (rot_total * f);
//
// and this is fast:
//
// CRotate3<> rot_total;
// CDir3<> d3_orig;
// CDir3<> d3_new = d3_orig;
// CRotate3<> d3_delta = d3_orig * 0.1;
//
// for (int i = 0; i < 10; i++)
// d3_new *= d3_delta;
//
CRotate3<TR> operator *(TR r_scale);
CRotate3<TR>& operator *=(TR r_scale)
{
return *this = *this * r_scale;
}
//
// Operate on a vector.
//
friend CVector3<TR> operator *(const CVector3<TR>& v3, const CRotate3<TR>& r3);
protected:
//******************************************************************************************
//
// Member functions.
//
//******************************************************************************************
//
TR tLenSqr() const
//
// Returns:
// The square of the length of the quaternion, as if a 4D vector.
//
//**********************************
{
return tC * tC + v3S * v3S;
}
//******************************************************************************************
//
TR tLen() const
//
// Returns:
// Length of the quaternion, as if a 4D vector.
//
//**********************************
{
return (TR) sqrt((double) tLenSqr());
}
//******************************************************************************************
//
bool bIsNormalised() const
//
// Returns:
// Whether the square of the length of the quaternion is fuzzily equal to 1.
//
//**********************************
{
return Fuzzy(tLenSqr(), (TR)fMAX_QUAT_DENORMALISATION) == 1;
}
//******************************************************************************************
//
void Normalise
(
bool b_always = false // Always renormalise without checking.
)
//
// Sets the 4-dimensional quaternion length to 1.
// If b_always is false, check first. This will generally be more efficient,
// as the checking is much faster than the renormalising.
//
//**********************************
{
#if VER_DEBUG
SetMax(iMaxCheckCount, iCount);
#endif
TR t_lensqr = tLenSqr();
// If requested, test whether already approximately normalised.
if (!b_always && bIsNormalised())
return;
#if VER_DEBUG
SetMax(iMaxNormalisationCount, iCount);
fTotalNormalisationCount += iCount;
fTotalNormalisations ++;
iAvgNormalisationCount = (int) (fTotalNormalisationCount / fTotalNormalisations);
#endif
#if VER_QUATERNION_COUNT
iCount = 0;
#endif
//
// We want to use a *particular* sqrt approximation, one
// appropriate to values very close to 1:
//
// sqrt(x) ~= (x+1)/2.
// 1/sqrt(x) ~= 2/(x+1).
//
Assert(Fuzzy(t_lensqr) != 0);
TR t_invlen = (TR)2 / (t_lensqr + (TR)1);
Assert(Fuzzy(t_invlen * t_invlen) == (TR)1 / t_lensqr);
tC *= t_invlen;
v3S *= t_invlen;
Assert(bIsNormalised());
}
//******************************************************************************************
//
void InitCounter()
{
#if VER_QUATERNION_COUNT
iCount = 0;
#endif
}
//
//**********************************
//******************************************************************************************
//
void BumpCounter()
{
#if VER_QUATERNION_COUNT
iCount++;
#endif
}
//
//**********************************
public:
//******************************************************************************************
//
void GetQuaternion
(
CQuaternion<TR> *pq
)
//
//**********************************
{
pq->tE1 = v3S.tX;
pq->tE2 = v3S.tY;
pq->tE3 = v3S.tZ;
pq->tE0 = tC;
}
};
//**********************************************************************************************
//
// Global operators for CRotate3<>.
//
template<class TR> inline CVector3<TR>& operator *=(CVector3<TR>& v3, const CRotate3<TR>& r3)
{
return v3 = v3 * r3;
}
//
// Define transformation of CDir3 as well, to return CDir3.
//
template<class TR> CDir3<TR> operator *(const CDir3<TR>& d3, const CRotate3<TR>& r3)
{
// Transform dir like a vector, then copy to a direction, bypassing renormalisation.
return CDir3<TR>((CVector3<TR>&)d3 * r3, true);
}
//**********************************************************************************************
//
template<class TR = TReal> class CPlacement3
//
// Prefix: p3
//
// A rigid transform, capable of describing an object's placement in a coordinate system.
// Contains a rotation and a translation vector.
//
//**************************************
{
public:
CVector3<TR> v3Pos; // The translation to add.
CRotate3<TR> r3Rot; // The non-translating transform.
public:
//******************************************************************************************
//
// Constructors.
//
CPlacement3()
: v3Pos(0, 0, 0)
{
}
//
// Provide constructors for all combinations of rotation and translation.
//
CPlacement3(const CRotate3<TR>& r3, const CVector3<TR>& v3)
: r3Rot(r3), v3Pos(v3)
{
}
CPlacement3(const CRotate3<TR>& r3)
: r3Rot(r3), v3Pos(0, 0, 0)
{
}
CPlacement3(const CVector3<TR>& v3)
: v3Pos(v3)
{
}
//******************************************************************************************
//
// Member functions.
//
//******************************************************************************************
//
void SetOrigin
(
const CVector3<TR>& v3_origin // Point acting as origin of rotation.
)
//
// Adjusts the transform so that the rotation is centred on v3_origin.
//
//**********************************
{
//
// To do the operation, first translate the object from v3_at back to the origin,
// do the rotation, then put the object back at v3_at.
//
// This is the same as just setting the translation to -v3_at * mx3 + v3_at.
//
v3Pos = -v3_origin * r3Rot + v3_origin;
}
//******************************************************************************************
//
void AdjustOrigin
(
const CVector3<TR>& v3_origin // Point acting as origin of transformation.
)
//
// Adjusts the placement so that it is centred on v3_origin. This is similar
// to SetOrigin() above, except that the placement's current translation is kept, and added
// to the new translation.
//
//**********************************
{
v3Pos += -v3_origin * r3Rot + v3_origin;
}
//******************************************************************************************
//
// Conversion operators.
//
operator CTransform3<TR> () const
{
return CTransform3<TR> ((CMatrix3<TR>)r3Rot, v3Pos);
}
//******************************************************************************************
//
// Operators.
//
// Return the inverse of the transform.
CPlacement3<TR> operator ~() const
{
// The inverse of a composite operation S*TR is ~TR * ~S, which is CPlacement3(~S, -TR * ~S).
CRotate3<TR> r3_inverse = ~r3Rot;
return CPlacement3<TR>(r3_inverse, -v3Pos * r3_inverse);
}
//
// Concatenate with another CPlacement3.
//
CPlacement3<TR> operator *(const CPlacement3<TR>& p3) const
{
return CPlacement3<TR>(r3Rot * p3.r3Rot, v3Pos * p3);
}
CPlacement3<TR>& operator *=(const CPlacement3<TR>& p3)
{
// Concatenate base transform and translation separately.
r3Rot *= p3.r3Rot;
v3Pos *= p3;
return *this;
}
//
// Concatenate with simple rotation of same type.
//
CPlacement3<TR> operator *(const CRotate3<TR>& r3) const
{
return CPlacement3<TR>(r3Rot * r3, v3Pos * r3);
}
CPlacement3<TR>& operator *=(const CRotate3<TR>& r3)
{
// Concatenate base transform and translation separately.
r3Rot *= r3;
v3Pos *= r3;
return *this;
}
//
// Concatenate with a translation.
//
CPlacement3<TR> operator *(const CTranslate3<TR>& tl3) const
{
return CPlacement3<TR>(r3Rot, v3Pos + tl3.v3Pos);
}
CPlacement3<TR>& operator *=(const CTranslate3<TR>& tl3)
{
v3Pos += tl3.v3Pos;
return *this;
}
};
//**********************************************************************************************
//
// Global operators for CRotate3<>.
//
template<class TR> inline CPlacement3<TR> operator *(const CRotate3<TR>& r3, const CPlacement3<TR>& p3)
{
return CPlacement3<TR>(r3 * p3.r3Rot, p3.v3Pos);
}
template<class TR> inline CPlacement3<TR> operator *(const CTranslate3<TR>& tl3, const CPlacement3<TR>& p3)
{
return CPlacement3<TR>(p3.r3Rot, tl3.v3Pos * p3);
}
//
// Combine a rotation and translation into a placement.
//
template<class TR> inline CPlacement3<TR> operator *(const CRotate3<TR>& r3, const CTranslate3<TR>& tl3)
{
return CPlacement3<TR>(r3, tl3.v3Pos);
}
template<class TR> inline CPlacement3<TR> operator *(const CTranslate3<TR>& tl3, const CRotate3<TR>& r3)
{
return CPlacement3<TR>(r3, tl3.v3Pos * r3);
}
//
// Operate on a vector.
//
template<class TR> inline CVector3<TR> operator *(const CVector3<TR>& v3, const CPlacement3<TR>& p3)
{
// Perform the transform, then add the translation.
return v3 * p3.r3Rot + p3.v3Pos;
}
template<class TR> inline CVector3<TR>& operator *=(CVector3<TR>& v3, const CPlacement3<TR>& p3)
{
v3 *= p3.r3Rot;
v3 += p3.v3Pos;
return v3;
}
//
// Operate on a direction by skipping the translation step.
//
template<class TR> inline CDir3<TR> operator *(const CDir3<TR>& d3, const CPlacement3<TR>& p3)
{
return d3 * p3.r3Rot;
}
template<class TR> inline CDir3<TR>& operator *=(CDir3<TR>& d3, const CPlacement3<TR>& p3)
{
return d3 *= p3.r3Rot;
}
//**********************************************************************************************
//
// Global functions for CRotate3<>.
//
//******************************************************************************************
//
template<class TR> inline CPlacement3<TR> TransformAt(const CRotate3<TR>& r3, const CVector3<TR>& v3_at)
//
// Returns:
// A CPlacement3 which performs the rotation r3 as if the point v3_at
// were the origin.
//
// Notes:
// This function does not have a type prefix because it is meant to be generic.
// There is another version for CMatrix3 and CTransform3.
//
//**********************************
{
//
// To do the operation, first translate the object from v3_at back to the origin,
// do the rotation, then put the object back at v3_at.
//
// This is the same as just setting the translation to -v3_at * r3 + v3_at.
//
return CPlacement3<TR>(r3, (-v3_at) * r3 + v3_at);
}
//******************************************************************************************
//
// We must include all the implementation code because this is a template class.
//
#include "Rotate.cpp"
#endif