vwemirror/TeslaRel410
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e8467172834392de265433c7fdf20c41ec4e32fa
TeslaRel410/restoration/source410/MUNGA/ROTATION.CPP
T
CydandClaude Fable 5 3c42766f48 BT410 5.3.62: correction -- the fault address was never in our code 
Every conclusion that named EulerAngles::operator=(const LinearMatrix&) as the
fault site was wrong, including the disassembly built on top of it.  Three
independent proofs:

1. ROTATION.CPP is ours, so the function was instrumented directly -- print
   this, &matrix and a local's address on the first twelve calls plus any wild
   pointer.  The run faulted with ZERO [euler] lines: never called.

2. After the probe was added, 0x66D9 disassembles to a two-byte conditional
   jump (jnl) that touches no memory.  The dump reports ErrCode 0004, a data
   READ.

3. EIP 0x66D9 and cr2 0x7000FA64 are identical to the byte across every build
   this session, including builds where the code at that offset changed
   completely.  A fault in our CODE segment moves when the code moves.  This
   one does not -- it lives in the DPMI host, which loads low and never gets
   relinked.

The map lookup was a coincidence: our _TEXT is section-relative from 0, so its
offsets overlap the host's low addresses, and the map will name a function for
any small number.  Before resolving a fault address against the map, confirm it
belongs to our segment -- cheapest check is whether it moves on relink.

What is actually true, by same-binary A/B:

    vRIO DOWN, serial1 still a namedpipe -> mission LAUNCHES
    vRIO UP,   same conf, same binary    -> FAULT

The trigger is a LIVE RIO stream, not an unplugged cable.  The emulator reports
continuous serial1 RX overruns, the fault lands wherever the app happened to be
(terrain one run, the mech's inside view another), and the shipped binary
survives the identical conditions.  A fault at a fixed host address, at
arbitrary points in the app, requiring an interrupt-driven stream, is an
interrupt re-entrancy signature rather than a wild pointer.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
2026-07-29 08:22:15 -05:00

1397 lines
31 KiB
C++
Raw Blame History

# if !defined(MUNGA_HPP)
# include <munga.hpp>
# endif
#pragma hdrstop
# if !defined(ROTATION_HPP)
# include <rotation.hpp>
# endif
# if !defined(LINMTRX_HPP)
# include <linmtrx.hpp>
# endif
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
ostream& operator<<(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);
//
// FAULT PROBE (env BT_EULER_LOG).
//
// The pod's page fault lands at +0x19 of THIS function -- the first read
// through the matrix -- with cr2 = 0x7000FA64, which is 1.79 GB away from
// the frame the main loop reports. Disassembly says that read is
// `fld dword [ebx+0x20]` with ebx the matrix, and every one of the three
// call sites passes `lea eax,[ebp-0x30]` -- a STACK local. Those two
// facts cannot both be true, so one of the inferences is wrong and no
// amount of further reading will say which.
//
// This settles it from inside the function, before any dereference:
// * matrix or this at a wild address -> a caller really does pass one,
// despite the disassembly.
// * a local at ~0x7000FA74 -> we are executing on a DIFFERENT STACK
// (an interrupt or DPMI callback), and the main-loop frame address
// was never the relevant one.
// * everything sane and the fault still lands here -> the reported EIP
// or cr2 is not trustworthy in this emulated DPMI host, and the hunt
// moves to the emulator side.
//
// getenv is cached: this is a hot path once a mission runs.
//
{
static int
euler_log = -1,
euler_count = 0;
char
here;
if (euler_log < 0)
{
euler_log = (getenv("BT_EULER_LOG") != NULL) ? 1 : 0;
}
if (euler_log)
{
euler_count++;
if (
euler_count <= 12 ||
(unsigned long)(void *)&matrix >= 0x10000000L ||
(unsigned long)(void *)this >= 0x10000000L
)
{
DEBUG_STREAM << "[euler] " << euler_count
<< " this=0x" << hex << (int)this
<< " matrix=0x" << (int)&matrix
<< " stack=0x" << (int)&here
<< dec << endl << flush;
}
}
}
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;
}
//
//#############################################################################
//#############################################################################
//
ostream& operator<<(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;
}
//
//#############################################################################
//#############################################################################
//
ostream& operator<<(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;
}
//
//#############################################################################
//#############################################################################
//
ostream& operator<<(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
Reference in New Issue View Git Blame Copy Permalink