arcattackandClaude Opus 4.8
7b7d465e5e
Clean, self-contained extraction of the BattleTech-specific work from the
reverse-engineering workspace -- engine + game + content + build, with nothing
from Red Planet or the raw archive dumps. Builds green (Win32) and runs the
single-player drive->animate->target->fire->damage->destroy loop out of the box.
Layout:
engine/ MUNGA + MUNGA_L4 shared 2007 engine, carrying our BT render/loader
work (bgfload/L4D3D/L4VIDEO: BSL bit-slice decode, LOD/ground/shadow
models) + image codec; the minimal rp/ headers the audio HAL needs
game/ reconstructed BT logic + surviving-original BT source + fwd shims
+ WinMain launcher
content/ full runtime tree (BTL4.RES, VIDEO/, GAUGE/, AUDIO/, eggs, BTDPL.INI)
docs/ format specs + reconstruction ledgers
reference/ raw Ghidra pseudocode (recon source-of-truth) + decomp exporter
tools/ MP console emulator + map/resource scanners
One top-level CMake builds munga_engine lib + bt410_l4 game lib + btl4.exe.
All paths relativized (186 fwd shims + ~437 CMake abs paths -> repo-relative);
DXSDK is the one external, overridable via -DDXSDK. Verified: builds to a
byte-identical 2.27MB exe and runs combat (TARGET DESTROYED, 0 crashes) against
the bundled content.
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
1339 lines
28 KiB
C++
1339 lines
28 KiB
C++
#include "munga.h"
|
|
#pragma hdrstop
|
|
|
|
#include "rotation.h"
|
|
#include "linmtrx.h"
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
Logical
|
|
Close_Enough(
|
|
const Hinge &a1,
|
|
const Hinge &a2,
|
|
Scalar e
|
|
)
|
|
{
|
|
return a1.axisNumber == a2.axisNumber
|
|
&& Close_Enough(a1.rotationAmount, a2.rotationAmount, e);
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
Hinge&
|
|
Hinge::Lerp(
|
|
const Hinge& v1,
|
|
const Hinge& v2,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&v1);
|
|
Check(&v2);
|
|
Verify(v1.axisNumber == v2.axisNumber);
|
|
|
|
axisNumber = v1.axisNumber;
|
|
rotationAmount.Lerp(v1.rotationAmount, v2.rotationAmount, t);
|
|
return *this;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
std::ostream& operator<<(std::ostream& stream, const Hinge& angles)
|
|
{
|
|
Check(&angles);
|
|
stream << angles.rotationAmount << " about ";
|
|
switch (angles.axisNumber)
|
|
{
|
|
case X_Axis:
|
|
stream << "X Axis";
|
|
break;
|
|
case Y_Axis:
|
|
stream << "Y Axis";
|
|
break;
|
|
case Z_Axis:
|
|
stream << "Z Axis";
|
|
break;
|
|
}
|
|
return stream;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
Logical
|
|
Hinge::TestInstance() const
|
|
{
|
|
return (unsigned)axisNumber <= Z_Axis;
|
|
}
|
|
|
|
#if defined(USE_SIGNATURE)
|
|
int
|
|
Is_Signature_Bad(const volatile Hinge *)
|
|
{
|
|
return False;
|
|
}
|
|
#endif
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ EulerAngles ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
|
|
const EulerAngles
|
|
EulerAngles::Identity(0.0f,0.0f,0.0f);
|
|
|
|
#if defined(USE_SIGNATURE)
|
|
int
|
|
Is_Signature_Bad(const volatile EulerAngles *)
|
|
{
|
|
return False;
|
|
}
|
|
#endif
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::operator=(const EulerAngles &angles)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&angles);
|
|
|
|
pitch = angles.pitch;
|
|
yaw = angles.yaw;
|
|
roll = angles.roll;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::operator=(const YawPitchRoll &angles)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&angles);
|
|
|
|
LinearMatrix m;
|
|
m = angles;
|
|
*this = m;
|
|
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::operator=(const Hinge &hinge)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&hinge);
|
|
|
|
pitch = 0.0f;
|
|
yaw = 0.0f;
|
|
roll = 0.0f;
|
|
|
|
switch (hinge.axisNumber)
|
|
{
|
|
case X_Axis:
|
|
pitch = hinge.rotationAmount;
|
|
break;
|
|
case Y_Axis:
|
|
yaw = hinge.rotationAmount;
|
|
break;
|
|
case Z_Axis:
|
|
roll = hinge.rotationAmount;
|
|
break;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::operator=(const Quaternion &quaternion)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&quaternion);
|
|
|
|
LinearMatrix m;
|
|
m = quaternion;
|
|
return *this = m;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::operator=(const LinearMatrix &matrix)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&matrix);
|
|
|
|
SinCosPair
|
|
p,y,r;
|
|
|
|
y.sine = -matrix(0,2);
|
|
if (Close_Enough(y.sine,1.0f,0.0001f)) {
|
|
y.cosine = 0.0f;
|
|
r.sine = 0.0f;
|
|
r.cosine = 1.0f;
|
|
p.sine = matrix(1,0);
|
|
p.cosine = matrix(2,0);
|
|
}
|
|
else if (Close_Enough(y.sine,-1.0f,0.0001f)) {
|
|
y.cosine = 0.0f;
|
|
r.sine = 0.0f;
|
|
r.cosine = 1.0f;
|
|
p.sine = -matrix(1,0);
|
|
p.cosine = -matrix(2,0);
|
|
}
|
|
else {
|
|
y.cosine = Sqrt(1.0f - y.sine*y.sine);
|
|
p.sine = matrix(1,2) / y.cosine;
|
|
p.cosine = matrix(2,2) / y.cosine;
|
|
r.sine = matrix(0,1) / y.cosine;
|
|
r.cosine = matrix(0,0) / y.cosine;
|
|
|
|
if (
|
|
!Close_Enough(p.sine*y.sine*r.cosine - p.cosine*r.sine,matrix(1,0))
|
|
) {
|
|
y.sine = -y.sine;
|
|
p.sine = -p.sine;
|
|
p.cosine = -p.cosine;
|
|
r.sine = -r.sine;
|
|
r.cosine = -r.cosine;
|
|
}
|
|
}
|
|
|
|
pitch = p;
|
|
yaw = y;
|
|
roll = r;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
Small_Enough(
|
|
const EulerAngles& angles,
|
|
Scalar e
|
|
)
|
|
{
|
|
Check(&angles);
|
|
|
|
return
|
|
Small_Enough(angles.pitch,e)
|
|
&& Small_Enough(angles.yaw,e)
|
|
&& Small_Enough(angles.roll,e);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
Close_Enough(
|
|
const EulerAngles& a1,
|
|
const EulerAngles& a2,
|
|
Scalar e
|
|
)
|
|
{
|
|
Check(&a1);
|
|
Check(&a2);
|
|
|
|
return
|
|
Close_Enough(a1.pitch,a2.pitch,e)
|
|
&& Close_Enough(a1.yaw,a2.yaw,e)
|
|
&& Close_Enough(a1.roll,a2.roll,e);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::Multiply(
|
|
const EulerAngles &q1,
|
|
const EulerAngles &q2
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q1);
|
|
Check(&q2);
|
|
|
|
pitch = q1.pitch + q2.pitch;
|
|
yaw = q1.yaw + q2.yaw;
|
|
roll = q1.roll + q2.roll;
|
|
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(q1);
|
|
Dump(q2);
|
|
Dump(*this);
|
|
Fail("EulerAngle multiplication is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::Multiply(
|
|
const EulerAngles &q,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q);
|
|
|
|
pitch = q.pitch * t;
|
|
yaw = q.yaw * t;
|
|
roll = q.roll * t;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::MultiplyScaled(
|
|
const EulerAngles &q1,
|
|
const EulerAngles &q2,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q1);
|
|
Check(&q2);
|
|
Verify(t>=0.0f);
|
|
|
|
pitch = q1.pitch + q2.pitch * t;
|
|
yaw = q1.yaw + q2.yaw * t;
|
|
roll = q1.roll + q2.roll * t;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::Lerp(
|
|
const EulerAngles &a1,
|
|
const EulerAngles &a2,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&a1);
|
|
Check(&a2);
|
|
|
|
pitch = ::Lerp(a1.pitch,a2.pitch,t);
|
|
yaw = ::Lerp(a1.yaw,a2.yaw,t);
|
|
roll = ::Lerp(a1.roll,a2.roll,t);
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
EulerAngles&
|
|
EulerAngles::Normalize()
|
|
{
|
|
Check_Pointer(this);
|
|
|
|
pitch.Normalize();
|
|
yaw.Normalize();
|
|
roll.Normalize();
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
std::ostream& operator<<(std::ostream& stream, const EulerAngles& angles)
|
|
{
|
|
return stream << '<' << angles.pitch << ',' << angles.yaw << ',' << angles.roll << '>';
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
EulerAngles::TestInstance() const
|
|
{
|
|
return True;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ YawPitchRoll ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
|
|
const YawPitchRoll
|
|
YawPitchRoll::Identity(0.0f,0.0f,0.0f);
|
|
|
|
#if defined(USE_SIGNATURE)
|
|
int
|
|
Is_Signature_Bad(const volatile YawPitchRoll *)
|
|
{
|
|
return False;
|
|
}
|
|
#endif
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::operator=(const YawPitchRoll &angles)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&angles);
|
|
|
|
pitch = angles.pitch;
|
|
yaw = angles.yaw;
|
|
roll = angles.roll;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::operator=(const EulerAngles &angles)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&angles);
|
|
|
|
LinearMatrix m;
|
|
m = angles;
|
|
*this = m;
|
|
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::operator=(const Hinge &hinge)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&hinge);
|
|
|
|
pitch = 0.0f;
|
|
yaw = 0.0f;
|
|
roll = 0.0f;
|
|
|
|
switch (hinge.axisNumber)
|
|
{
|
|
case X_Axis:
|
|
pitch = hinge.rotationAmount;
|
|
break;
|
|
case Y_Axis:
|
|
yaw = hinge.rotationAmount;
|
|
break;
|
|
case Z_Axis:
|
|
roll = hinge.rotationAmount;
|
|
break;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::operator=(const Quaternion &quaternion)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&quaternion);
|
|
|
|
LinearMatrix m;
|
|
m = quaternion;
|
|
return *this = m;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::operator=(const LinearMatrix &matrix)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&matrix);
|
|
|
|
SinCosPair
|
|
x,y,z;
|
|
|
|
x.sine = -matrix(2,1);
|
|
if (Close_Enough(x.sine,1.0f,0.0001f)) {
|
|
x.cosine = 0.0f;
|
|
z.sine = 0.0f;
|
|
z.cosine = 1.0f;
|
|
y.sine = matrix(1,0);
|
|
y.cosine = matrix(0,0);
|
|
}
|
|
else if (Close_Enough(x.sine,-1.0f,0.0001f)) {
|
|
x.cosine = 0.0f;
|
|
z.sine = 0.0f;
|
|
z.cosine = 1.0f;
|
|
y.sine = matrix(0,2);
|
|
y.cosine = matrix(0,0);
|
|
}
|
|
else {
|
|
x.cosine = Sqrt(1.0f - x.sine*x.sine);
|
|
y.sine = matrix(2,0) / x.cosine;
|
|
y.cosine = matrix(2,2) / x.cosine;
|
|
z.sine = matrix(0,1) / x.cosine;
|
|
z.cosine = matrix(1,1) / x.cosine;
|
|
|
|
if (
|
|
!Close_Enough(y.cosine*z.cosine + x.sine*y.sine*z.sine,matrix(0,0))
|
|
)
|
|
{
|
|
x.sine = -x.sine;
|
|
y.sine = -y.sine;
|
|
y.cosine = -y.cosine;
|
|
z.sine = -z.sine;
|
|
z.cosine = -z.cosine;
|
|
}
|
|
}
|
|
|
|
pitch = x;
|
|
yaw = y;
|
|
roll = z;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
Small_Enough(
|
|
const YawPitchRoll& angles,
|
|
Scalar e
|
|
)
|
|
{
|
|
Check(&angles);
|
|
|
|
return
|
|
Small_Enough(angles.pitch,e)
|
|
&& Small_Enough(angles.yaw,e)
|
|
&& Small_Enough(angles.roll,e);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
Close_Enough(
|
|
const YawPitchRoll& a1,
|
|
const YawPitchRoll& a2,
|
|
Scalar e
|
|
)
|
|
{
|
|
Check(&a1);
|
|
Check(&a2);
|
|
|
|
return
|
|
Close_Enough(a1.pitch,a2.pitch,e)
|
|
&& Close_Enough(a1.yaw,a2.yaw,e)
|
|
&& Close_Enough(a1.roll,a2.roll,e);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
YawPitchRoll&
|
|
YawPitchRoll::Normalize()
|
|
{
|
|
Check_Pointer(this);
|
|
|
|
pitch.Normalize();
|
|
yaw.Normalize();
|
|
roll.Normalize();
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
std::ostream& operator<<(std::ostream& stream, const YawPitchRoll& angles)
|
|
{
|
|
return stream << '<' << angles.yaw << ',' << angles.pitch << ',' << angles.roll << '>';
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical
|
|
YawPitchRoll::TestInstance() const
|
|
{
|
|
return True;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Quaternion ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
|
|
const Quaternion
|
|
Quaternion::Identity(0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
#if defined(USE_SIGNATURE)
|
|
int
|
|
Is_Signature_Bad(const volatile Quaternion *)
|
|
{
|
|
return False;
|
|
}
|
|
#endif
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion::Quaternion(
|
|
Scalar x,
|
|
Scalar y,
|
|
Scalar z,
|
|
Scalar w
|
|
)
|
|
{
|
|
this->x = x;
|
|
this->y = y;
|
|
this->z = z;
|
|
this->w = w;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::operator=(const Quaternion &q)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q);
|
|
|
|
x = q.x;
|
|
y = q.y;
|
|
z = q.z;
|
|
w = q.w;
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::operator=(const Hinge &hinge)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&hinge);
|
|
|
|
Radian temp;
|
|
temp = hinge.rotationAmount * 0.5f;
|
|
SinCosPair half_angle;
|
|
half_angle = temp;
|
|
|
|
w = half_angle.cosine;
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
|
|
switch (hinge.axisNumber)
|
|
{
|
|
case X_Axis:
|
|
x = half_angle.sine;
|
|
break;
|
|
case Y_Axis:
|
|
y = half_angle.sine;
|
|
break;
|
|
case Z_Axis:
|
|
z = half_angle.sine;
|
|
break;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::operator=(const EulerAngles &angles)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&angles);
|
|
|
|
LinearMatrix m;
|
|
m = angles;
|
|
Check(&m);
|
|
*this = m;
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(angles);
|
|
Dump(*this);
|
|
Fail("Quaternion construction from angles is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::operator=(const YawPitchRoll &angles)
|
|
{
|
|
LinearMatrix lin_matrix;
|
|
lin_matrix = angles;
|
|
*this = lin_matrix;
|
|
return *this;
|
|
}
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::operator=(const LinearMatrix &matrix)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&matrix);
|
|
|
|
//
|
|
//------------------------------------------------------------------------
|
|
// Compute the w component. If it is close enough to zero, then we have a
|
|
// 180 degree pivot, so figure out the correct axis to rotate around
|
|
//------------------------------------------------------------------------
|
|
//
|
|
w = (1.0f + matrix(0,0) + matrix(1,1) + matrix(2,2)) * 0.25f;
|
|
if (Small_Enough(w,1e-2))
|
|
{
|
|
Verify(w >= -SMALL);
|
|
if (w<0.0f)
|
|
{
|
|
w = 0.0f;
|
|
}
|
|
|
|
//
|
|
//----------------------------------------------------------------
|
|
// Figure out the length of each component of the axis of rotation
|
|
//----------------------------------------------------------------
|
|
//
|
|
Scalar temp = (1.0f + matrix(0,0)) * 0.5f - w;
|
|
Min_Clamp(temp, 0.0f);
|
|
x = Sqrt(temp);
|
|
temp = (1.0f + matrix(1,1)) * 0.5f - w;
|
|
Min_Clamp(temp, 0.0f);
|
|
y = Sqrt(temp);
|
|
temp = (1.0f + matrix(2,2)) * 0.5f - w;
|
|
Min_Clamp(temp, 0.0f);
|
|
z = Sqrt(temp);
|
|
w = Sqrt(w);
|
|
|
|
//
|
|
//-------------------------------------------
|
|
// Now figure out the signs of the components
|
|
//-------------------------------------------
|
|
//
|
|
if (matrix(0,1) < matrix(1,0))
|
|
{
|
|
z = -z;
|
|
}
|
|
if (matrix(2,0) < matrix(0,2))
|
|
{
|
|
y = -y;
|
|
}
|
|
if (matrix(1,2) < matrix(2,1))
|
|
{
|
|
x = -x;
|
|
}
|
|
}
|
|
|
|
//
|
|
//----------------------------------------------------------
|
|
// Otherwise, determine x, y, and z directly from the matrix
|
|
//----------------------------------------------------------
|
|
//
|
|
else
|
|
{
|
|
Verify(w>0.0f);
|
|
w = Sqrt(w);
|
|
x = (matrix(1,2) - matrix(2,1)) * 0.25f / w;
|
|
y = (matrix(2,0) - matrix(0,2)) * 0.25f / w;
|
|
z = (matrix(0,1) - matrix(1,0)) * 0.25f / w;
|
|
}
|
|
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(matrix);
|
|
Dump(*this);
|
|
Fail("Quaternion construction from matrix is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Scalar
|
|
Quaternion::GetAngle()
|
|
{
|
|
Check(this);
|
|
|
|
Scalar sine_of_half = Sqrt(x*x + y*y + z*z);
|
|
if (Small_Enough(sine_of_half))
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
SinCosPair half_angle(sine_of_half, w);
|
|
Radian angle;
|
|
angle = half_angle;
|
|
|
|
return angle * 2.0f;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
void
|
|
Quaternion::GetAxis(UnitVector *axis)
|
|
{
|
|
Check(this);
|
|
Check_Pointer(axis);
|
|
|
|
Scalar len = Sqrt(x*x + y*y + z*z);
|
|
if (Small_Enough(len))
|
|
{
|
|
axis->x = 1.0f;
|
|
axis->y = 0.0f;
|
|
axis->z = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
axis->x = x / len;
|
|
axis->y = y / len;
|
|
axis->z = z / len;
|
|
}
|
|
|
|
Check(axis);
|
|
return;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Multiply(const Quaternion &q1, const Quaternion &q2)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q1);
|
|
Check(&q2);
|
|
Verify(this != &q1 && this != &q2);
|
|
|
|
x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
|
|
y = q1.w*q2.y + q2.w*q1.y + q1.z*q2.x - q1.x*q2.z;
|
|
z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
|
|
w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(q1);
|
|
Dump(q2);
|
|
Dump(*this);
|
|
Fail("Quaternion multiplication is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Multiply(
|
|
const Quaternion &q,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q);
|
|
|
|
//
|
|
//---------------------------------------------------------
|
|
// Figure out the half the angle of rotation and scale that
|
|
//---------------------------------------------------------
|
|
//
|
|
Scalar sine_of_half = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z);
|
|
if (Small_Enough(sine_of_half))
|
|
{
|
|
*this = Identity;
|
|
return *this;
|
|
}
|
|
|
|
SinCosPair half_angle(sine_of_half, q.w);
|
|
Radian angle;
|
|
angle = half_angle;
|
|
angle *= t;
|
|
half_angle = angle;
|
|
|
|
//
|
|
//-----------------------------------------------------------------
|
|
// Build the scaled quaternion out of the components of the old one
|
|
//-----------------------------------------------------------------
|
|
//
|
|
w = half_angle.cosine;
|
|
sine_of_half = half_angle.sine / sine_of_half;
|
|
x = q.x * sine_of_half;
|
|
y = q.y * sine_of_half;
|
|
z = q.z * sine_of_half;
|
|
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(q);
|
|
Dump(t);
|
|
Dump(*this);
|
|
Fail("Quaternion multiplication is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::MultiplyScaled(
|
|
const Quaternion &q1,
|
|
const Quaternion &q2,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Verify(this != &q1);
|
|
Check(&q1);
|
|
Check(&q2);
|
|
Verify(t>=0.0f);
|
|
|
|
Quaternion scaled_quat;
|
|
scaled_quat.Multiply(q2, t);
|
|
Multiply(q1, scaled_quat);
|
|
|
|
#if DEBUG_LEVEL>0
|
|
if (!TestInstance())
|
|
{
|
|
Dump(q1);
|
|
Dump(q2);
|
|
Dump(t);
|
|
Dump(*this);
|
|
Fail("Quaternion multiplication is unstable!\n");
|
|
}
|
|
#endif
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Add(
|
|
const Quaternion &source,
|
|
const Vector3D &delta
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&source);
|
|
Check(&delta);
|
|
Verify(&source != this);
|
|
|
|
//
|
|
//---------------------------------------------------------------
|
|
// See if there is any rotation to apply to the source quaternion
|
|
//---------------------------------------------------------------
|
|
//
|
|
Scalar rotation = delta.Length();
|
|
if (Small_Enough(rotation))
|
|
{
|
|
return *this = source;
|
|
}
|
|
|
|
//
|
|
//---------------------------------------------------------------------
|
|
// Build a quaternion from the delta vector, treating the length as the
|
|
// amount of rotation and the direction of the vector as the axis of
|
|
// rotation
|
|
//---------------------------------------------------------------------
|
|
//
|
|
SinCosPair half_angle;
|
|
half_angle = 0.5f * Radian::Normalize(rotation);
|
|
rotation = half_angle.sine / rotation;
|
|
Quaternion q(
|
|
delta.x * rotation,
|
|
delta.y * rotation,
|
|
delta.z * rotation,
|
|
half_angle.cosine
|
|
);
|
|
Check(&q);
|
|
return Multiply(source, q);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::AddScaled(
|
|
const Quaternion &source,
|
|
const Vector3D &delta,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&source);
|
|
Check(&delta);
|
|
Verify(&source != this);
|
|
|
|
//
|
|
//---------------------------------------------------------------
|
|
// See if there is any rotation to apply to the source quaternion
|
|
//---------------------------------------------------------------
|
|
//
|
|
Scalar rotation = delta.Length();
|
|
if (Small_Enough(rotation))
|
|
{
|
|
return *this = source;
|
|
}
|
|
|
|
//
|
|
//---------------------------------------------------------------------
|
|
// Build a quaternion from the delta vector, treating the length as the
|
|
// amount of rotation and the direction of the vector as the axis of
|
|
// rotation
|
|
//---------------------------------------------------------------------
|
|
//
|
|
SinCosPair half_angle;
|
|
rotation *= t;
|
|
half_angle = 0.5f*rotation;
|
|
rotation = half_angle.sine / rotation;
|
|
Quaternion q(
|
|
delta.x * rotation,
|
|
delta.y * rotation,
|
|
delta.z * rotation,
|
|
half_angle.cosine
|
|
);
|
|
Check(&q);
|
|
return Multiply(source, q);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Normalize()
|
|
{
|
|
Scalar t = x*x + y*y + z*z;
|
|
if (t <= 1.0f)
|
|
{
|
|
t = Sqrt(1.0f - t);
|
|
if (w<0.0f)
|
|
{
|
|
x = -x;
|
|
y = -y;
|
|
z = -z;
|
|
}
|
|
w = t;
|
|
}
|
|
else
|
|
{
|
|
t = Sqrt(t);
|
|
x /= t;
|
|
y /= t;
|
|
z /= t;
|
|
w = 0.0f;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Subtract(
|
|
const Quaternion &end,
|
|
const Quaternion &start
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&start);
|
|
Check(&end);
|
|
|
|
Quaternion inverse(start);
|
|
inverse.w = -inverse.w;
|
|
return Multiply(inverse, end);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Subtract(
|
|
const UnitVector &end,
|
|
const UnitVector &start
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&start);
|
|
Check(&end);
|
|
|
|
Vector3D
|
|
axis;
|
|
SinCosPair
|
|
delta;
|
|
delta.cosine = start*end;
|
|
|
|
//
|
|
//----------------------------------------------------------------------
|
|
// See if the vectors point in the same direction. If so, return a null
|
|
// rotation
|
|
//----------------------------------------------------------------------
|
|
//
|
|
if (Close_Enough(delta.cosine, 1.0f))
|
|
{
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
w = 1.0f;
|
|
}
|
|
|
|
//
|
|
//-------------------------------------------------------------------------
|
|
// See if the vectors directly oppose each other. If so, pick the smallest
|
|
// axis coordinate and generate a vector along it. Project this onto the
|
|
// base vector and subtract it out, leaving a perpendicular projection.
|
|
// Extend that out to unit length, then set the angle to PI
|
|
//-------------------------------------------------------------------------
|
|
//
|
|
else if (Close_Enough(delta.cosine, -1.0f))
|
|
{
|
|
//
|
|
//---------------------------
|
|
// Pick out the smallest axis
|
|
//---------------------------
|
|
//
|
|
int
|
|
smallest=0;
|
|
Scalar
|
|
value=2.0f;
|
|
for (int i=X_Axis; i<=Z_Axis; ++i)
|
|
{
|
|
if (Abs(start[i]) < value)
|
|
{
|
|
smallest = i;
|
|
value = Abs(start[i]);
|
|
}
|
|
}
|
|
|
|
//
|
|
//----------------------------------------
|
|
// Set up a vector along the selected axis
|
|
//----------------------------------------
|
|
//
|
|
axis.x = 0.0f;
|
|
axis.y = 0.0f;
|
|
axis.z = 0.0f;
|
|
axis[smallest] = 1.0f;
|
|
|
|
//
|
|
//-------------------------------------------------------------------
|
|
// If the value on that axis wasn't zero, subtract out the projection
|
|
//-------------------------------------------------------------------
|
|
//
|
|
if (!Small_Enough(value))
|
|
{
|
|
Vector3D t;
|
|
t.Multiply(start, start*axis);
|
|
axis.Subtract(axis, t);
|
|
axis.Normalize(axis);
|
|
}
|
|
|
|
//
|
|
//----------------------
|
|
// Convert to quaternion
|
|
//----------------------
|
|
//
|
|
x = axis.x;
|
|
y = axis.y;
|
|
z = axis.z;
|
|
w = 0.0f;
|
|
}
|
|
|
|
//
|
|
//--------------------------------------------------
|
|
// Otherwise, generate the cross product and unitize
|
|
//--------------------------------------------------
|
|
//
|
|
else
|
|
{
|
|
axis.Cross(start, end);
|
|
delta.sine = axis.Length();
|
|
axis /= delta.sine;
|
|
|
|
//
|
|
//---------------------------------------------------------------
|
|
// Now compute sine and cosine of half the angle and generate the
|
|
// quaternion
|
|
//---------------------------------------------------------------
|
|
//
|
|
delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
|
|
x = axis.x * delta.sine;
|
|
y = axis.y * delta.sine;
|
|
z = axis.z * delta.sine;
|
|
w = Sqrt((1.0f + delta.cosine)*0.5f);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Subtract(
|
|
const Vector3D &end,
|
|
const Vector3D &start
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&start);
|
|
Check(&end);
|
|
|
|
UnitVector
|
|
s,e;
|
|
|
|
s = start;
|
|
e = end;
|
|
return Subtract(e, s);
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Quaternion&
|
|
Quaternion::Lerp(
|
|
const Quaternion &q1,
|
|
const Quaternion &q2,
|
|
Scalar t
|
|
)
|
|
{
|
|
Check_Pointer(this);
|
|
Check(&q1);
|
|
Check(&q2);
|
|
|
|
if (q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w >= 0.0f)
|
|
{
|
|
x = ::Lerp(q1.x, q2.x, t);
|
|
y = ::Lerp(q1.y, q2.y, t);
|
|
z = ::Lerp(q1.z, q2.z, t);
|
|
w = ::Lerp(q1.w, q2.w, t);
|
|
}
|
|
else
|
|
{
|
|
x = ::Lerp(q1.x, -q2.x, t);
|
|
y = ::Lerp(q1.y, -q2.y, t);
|
|
z = ::Lerp(q1.z, -q2.z, t);
|
|
w = ::Lerp(q1.w, -q2.w, t);
|
|
}
|
|
|
|
Scalar len = x*x + y*y + z*z + w*w;
|
|
if (Small_Enough(len))
|
|
{
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
w = 1.0f;
|
|
}
|
|
else
|
|
{
|
|
len = 1.0f / Sqrt(len);
|
|
x *= len;
|
|
y *= len;
|
|
z *= len;
|
|
w *= len;
|
|
}
|
|
Check(this);
|
|
return *this;
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
std::ostream& operator<<(std::ostream& stream, const Quaternion& q)
|
|
{
|
|
return stream << '<' << q.x << ',' << q.y << ',' << q.z << ',' << q.w
|
|
<< '>';
|
|
}
|
|
|
|
//
|
|
//#############################################################################
|
|
//#############################################################################
|
|
//
|
|
Logical Quaternion::TestInstance() const
|
|
{
|
|
#if 0
|
|
if (!Close_Enough(x*x + y*y + z*z + w*w,1.0f,2e-6))
|
|
{
|
|
Scalar t = 1.0f - x*x - y*y - z*z - w*w;
|
|
|
|
if (Small_Enough(t, 2e-6))
|
|
{
|
|
Dump(*this);
|
|
Dump(1.0f - x*x - y*y - z*z - w*w);
|
|
return False;
|
|
}
|
|
}
|
|
#endif
|
|
return True;
|
|
}
|
|
|
|
#if defined(TEST_CLASS)
|
|
# include "rotation.tcp"
|
|
#endif
|