vwemirror/firestorm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
2cfb14ab983b3e4e870f415133f2016f19a2fa0b
firestorm/Gameleap/code/mw4/Libraries/stuff/AffineMatrix.cpp
T
Cyd 2b8ca921cb Initial full mirror of c:\VWE (source + assets + toolchain + outputs) via Git LFS 
Complete disaster-recovery snapshot: engine/game source, game data assets,
VC6 toolchain + DX SDKs, build outputs, deployed game, and _UNUSED archive.
Large binaries in Git LFS; text preserved byte-for-byte (core.autocrlf=false,
no eol attributes). See RECOVERY.md for the one-clone rebuild procedure.
2026-06-24 21:28:16 -05:00

1038 lines
26 KiB
C++
Raw Blame History

//===========================================================================//
// File: affnmtrx.cc //
// Project: MUNGA Brick: Math Library //
// Contents: Implementation details for the Affine matrices //
//---------------------------------------------------------------------------//
// Date Who Modification //
// -------- --- ---------------------------------------------------------- //
// 11/20/94 JMA Initial coding. //
//---------------------------------------------------------------------------//
// Copyright (C) 1994-1995, Virtual World Entertainment, Inc. //
// All Rights reserved worldwide //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL //
//===========================================================================//
#include "StuffHeaders.hpp"
const AffineMatrix4D
AffineMatrix4D::Identity(true);
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildIdentity()
{
Check_Pointer(this);
entries[0] = 1.0f;
entries[1] = 0.0f;
entries[2] = 0.0f;
entries[3] = 0.0f;
entries[4] = 0.0f;
entries[5] = 1.0f;
entries[6] = 0.0f;
entries[7] = 0.0f;
entries[8] = 0.0f;
entries[9] = 0.0f;
entries[10] = 1.0f;
entries[11] = 0.0f;
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const Matrix4D &m)
{
Check_Pointer(this);
Check_Object(&m);
Verify(Small_Enough(m(0,3)));
Verify(Small_Enough(m(1,3)));
Verify(Small_Enough(m(2,3)));
Verify(Close_Enough(m(3,3),1.0f));
memcpy(entries, m.entries, sizeof(entries));
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const Origin3D& p)
{
Check_Pointer(this);
Check_Object(&p);
BuildRotation(p.angularPosition);
BuildTranslation(p.linearPosition);
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const EulerAngles &angles)
{
Check_Pointer(this);
Check_Object(&angles);
BuildRotation(angles);
(*this)(3,0) = 0.0f;
(*this)(3,1) = 0.0f;
(*this)(3,2) = 0.0f;
Check_Object(this);
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const YawPitchRoll &angles)
{
Check_Pointer(this);
Check_Object(&angles);
BuildRotation(angles);
(*this)(3,0) = 0.0f;
(*this)(3,1) = 0.0f;
(*this)(3,2) = 0.0f;
Check_Object(this);
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const UnitQuaternion &q)
{
Check_Pointer(this);
Check_Object(&q);
BuildRotation(q);
(*this)(3,0) = 0.0f;
(*this)(3,1) = 0.0f;
(*this)(3,2) = 0.0f;
Check_Object(this);
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::operator=(const Point3D &p)
{
Check_Pointer(this);
Check_Object(&p);
(*this)(0,0) = 1.0f;
(*this)(0,1) = 0.0f;
(*this)(0,2) = 0.0f;
(*this)(1,0) = 0.0f;
(*this)(1,1) = 1.0f;
(*this)(1,2) = 0.0f;
(*this)(2,0) = 0.0f;
(*this)(2,1) = 0.0f;
(*this)(2,2) = 1.0f;
BuildTranslation(p);
Check_Object(this);
return *this;
}
//
//#############################################################################
//#############################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildRotation(const EulerAngles &angles)
{
Check_Pointer(this);
Check_Object(&angles);
Verify(
Vector3D::Forward.z == 1.0f && Vector3D::Right.x == -1.0f && Vector3D::Up.y == 1.0f
|| Vector3D::Forward.z == -1.0f && Vector3D::Right.x == 1.0f && Vector3D::Up.y == 1.0f
);
SinCosPair
x,
y,
z;
x = angles.pitch;
y = angles.yaw;
z = angles.roll;
(*this)(0,0) = y.cosine*z.cosine;
(*this)(0,1) = y.cosine*z.sine;
(*this)(0,2) = -y.sine;
(*this)(1,0) = x.sine*y.sine*z.cosine - x.cosine*z.sine;
(*this)(1,1) = x.sine*y.sine*z.sine + x.cosine*z.cosine;
(*this)(1,2) = x.sine*y.cosine;
(*this)(2,0) = x.cosine*y.sine*z.cosine + x.sine*z.sine;
(*this)(2,1) = x.cosine*y.sine*z.sine - x.sine*z.cosine;
(*this)(2,2) = x.cosine*y.cosine;
Check_Object(this);
return *this;
}
//
//#############################################################################
//#############################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildRotation(const YawPitchRoll &angles)
{
Check_Pointer(this);
Check_Object(&angles);
Verify(
Vector3D::Forward.z == 1.0f && Vector3D::Right.x == -1.0f && Vector3D::Up.y == 1.0f
|| Vector3D::Forward.z == -1.0f && Vector3D::Right.x == 1.0f && Vector3D::Up.y == 1.0f
);
SinCosPair
x,
y,
z;
x = angles.pitch;
y = angles.yaw;
z = angles.roll;
(*this)(0,0) = y.cosine*z.cosine + x.sine*y.sine*z.sine;
(*this)(0,1) = x.cosine*z.sine;
(*this)(0,2) = x.sine*y.cosine*z.sine - y.sine*z.cosine;
(*this)(1,0) = x.sine*y.sine*z.cosine - y.cosine*z.sine;
(*this)(1,1) = x.cosine*z.cosine;
(*this)(1,2) = y.sine*z.sine + x.sine*y.cosine*z.cosine;
(*this)(2,0) = x.cosine*y.sine;
(*this)(2,1) = -x.sine;
(*this)(2,2) = x.cosine*y.cosine;
Check_Object(this);
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildRotation(const UnitQuaternion &q)
{
Check_Pointer(this);
Check_Object(&q);
Scalar
a = q.x*q.y,
b = q.y*q.z,
c = q.z*q.x,
d = q.w*q.x,
e = q.w*q.y,
f = q.w*q.z,
g = q.w*q.w,
h = q.x*q.x,
i = q.y*q.y,
j = q.z*q.z;
(*this)(0,0) = g + h - i - j;
(*this)(1,0) = 2.0f*(a - f);
(*this)(2,0) = 2.0f*(c + e);
(*this)(0,1) = 2.0f*(f + a);
(*this)(1,1) = g - h + i - j;
(*this)(2,1) = 2.0f*(b - d);
(*this)(0,2) = 2.0f*(c - e);
(*this)(1,2) = 2.0f*(b + d);
(*this)(2,2) = g - h - i + j;
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildRotation(const Vector3D &v)
{
Check_Pointer(this);
Check_Object(&v);
#if 0 // HACK
//
//---------------------------------------------------------------------
// If we are dealing with a zero length vector, just set up an identity
// matrix for rotation
//---------------------------------------------------------------------
//
Scalar rotation = v.GetLength();
if (Small_Enough(rotation))
{
(*this)(0,0) = 1.0f;
(*this)(0,1) = 0.0f;
(*this)(0,2) = 0.0f;
(*this)(1,0) = 0.0f;
(*this)(1,1) = 1.0f;
(*this)(1,2) = 0.0f;
(*this)(2,0) = 0.0f;
(*this)(2,1) = 0.0f;
(*this)(2,2) = 1.0f;
return *this;
}
//
//-----------------------------------------------
// Figure out the axis of rotation and unitize it
//-----------------------------------------------
//
Vector3D axis;
axis.Multiply(v, 1.0f/rotation);
Abort_Program("Not implemented");
return *this;
#else
UnitQuaternion temp_quat;
temp_quat = v;
operator=(temp_quat);
return *this;
#endif
}
//
//###########################################################################
//###########################################################################
//
bool
Stuff::Close_Enough(
const AffineMatrix4D &m1,
const AffineMatrix4D &m2,
Scalar e
)
{
Check_Object(&m2);
Check_Object(&m1);
for (size_t i=0; i<ELEMENTS(m1.entries); ++i)
{
if (!Close_Enough(m1.entries[i], m2.entries[i], e))
{
return false;
}
}
return true;
}
#if 0 // moved it to the hpp-file to make it inline
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Multiply(
const AffineMatrix4D& Source1,
const AffineMatrix4D& Source2
)
{
Check_Pointer(this);
Check_Object(&Source1);
Check_Object(&Source2);
Verify(this != &Source1);
Verify(this != &Source2);
#if USE_ASSEMBLER_CODE
Scalar *f = entries;
_asm {
mov edx, Source1.entries
push esi
mov esi, Source2.entries
mov eax, f
fld dword ptr [edx] // s1[0][0]
fmul dword ptr [esi] // s2[0][0] M0,1
fld dword ptr [edx+010h] // s1[0][1]
fmul dword ptr [esi+4] // s2[1][0] M0,2
fld dword ptr [edx+020h] // s1[0][2]
fmul dword ptr [esi+8] // s2[2][0] M0,3
fxch st(2)
faddp st(1),st // A0,1
fld dword ptr [edx+4] // s1[1][0]
fmul dword ptr [esi] // s2[0][0] M1,1
fxch st(2)
faddp st(1),st // A0,2
fld dword ptr [edx+14h] // s1[1][1]
fmul dword ptr [esi+4] // s2[1][0] M1,2
fxch st(1)
fstp dword ptr [eax] // [0][0] S0
fld dword ptr [edx+24h] // s1[1][2]
fmul dword ptr [esi+8] // s2[2][0] M1,3
fxch st(2)
faddp st(1),st // A1,1
fld dword ptr [edx+8] // s1[2][0]
fmul dword ptr [esi] // s2[0][0] M2,1
fxch st(2)
faddp st(1),st // A1,2
fld dword ptr [edx+018h] // s1[2][1]
fmul dword ptr [esi+4] // s2[1][0] M2,2
fxch st(1)
fstp dword ptr [eax+4] // [1][0] S1
fld dword ptr [edx+28h] // s1[2][2]
fmul dword ptr [esi+8] // s2[2][0] M2,3
fxch st(2)
faddp st(1),st // A2,1
fld dword ptr [edx+0ch] // s1[3][0]
fmul dword ptr [esi] // s2[0][0] M3,1
fxch st(2)
faddp st(1),st // A2,2
fld dword ptr [edx+1ch] // s1[3][1]
fmul dword ptr [esi+4] // s2[1][0] M3,2
fxch st(1)
fstp dword ptr [eax+8] // [2][0] S2
fld dword ptr [edx+2ch] // s1[3][2]
fmul dword ptr [esi+8] // s2[2][0] M3,3
fxch st(2)
faddp st(1),st // A3,1
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
fld dword ptr [edx] // s1[0][0]
fmul dword ptr [esi+010h] // s2[0][1] M0,1
fxch st(2)
faddp st(1),st // A3,2
fld dword ptr [edx+010h] // s1[0][1]
fmul dword ptr [esi+014h] // s2[1][1] M0,2
fxch st(1)
fadd dword ptr [esi+0Ch] // s2[3][0] A3,3
fld dword ptr [edx+020h] // s1[0][2]
fmul dword ptr [esi+018h] // s2[2][1] M0,3
fxch st(1)
fstp dword ptr [eax+0Ch] // [3][0] S3
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
fxch st(2)
faddp st(1),st // A0,1
fld dword ptr [edx+4] // s1[1][0]
fmul dword ptr [esi+010h] // s2[0][1] M1,1
fxch st(2)
faddp st(1),st // A0,2
fld dword ptr [edx+014h] // s1[1][1]
fmul dword ptr [esi+014h] // s2[1][1] M1,2
fxch st(1)
fstp dword ptr [eax+010h] // [0][1] S0
fld dword ptr [edx+024h] // s1[1][2]
fmul dword ptr [esi+018h] // s2[2][1] M1,3
fxch st(2)
faddp st(1),st // A1,1
fld dword ptr [edx+8] // s1[2][0]
fmul dword ptr [esi+010h] // s2[0][1] M2,1
fxch st(2)
faddp st(1),st // A1,2
fld dword ptr [edx+018h] // s1[2][1]
fmul dword ptr [esi+014h] // s2[1][1] M2,2
fxch st(1)
fstp dword ptr [eax+014h] // [1][1] S1
fld dword ptr [edx+028h] // s1[2][2]
fmul dword ptr [esi+018h] // s2[2][1] M2,3
fxch st(2)
faddp st(1),st // A2,1
fld dword ptr [edx+0ch] // s1[3][0]
fmul dword ptr [esi+010h] // s2[0][1] M3,1
fxch st(2)
faddp st(1),st // A2,2
fld dword ptr [edx+01ch] // s1[3][1]
fmul dword ptr [esi+014h] // s2[1][1] M3,2
fxch st(1)
fstp dword ptr [eax+018h] // [2][1] S2
fld dword ptr [edx+02ch] // s1[3][2]
fmul dword ptr [esi+018h] // s2[2][1] M3,3
fxch st(2)
faddp st(1),st // A3,1
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
fld dword ptr [edx] // s1[0][0]
fmul dword ptr [esi+020h] // s2[0][2] M0,1
fxch st(2)
faddp st(1),st // A3,2
fld dword ptr [edx+010h] // s1[0][1]
fmul dword ptr [esi+024h] // s2[1][2] M0,2
fxch st(1)
fadd dword ptr [esi+01Ch] // s2[3][1] A3,3
fld dword ptr [edx+020h] // s1[0][2]
fmul dword ptr [esi+028h] // s2[2][2] M0,3
fxch st(1)
fstp dword ptr [eax+01Ch] // [3][1] S3
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
fxch st(2)
faddp st(1),st // A0,1
fld dword ptr [edx+4] // s1[1][0]
fmul dword ptr [esi+020h] // s2[0][2] M1,1
fxch st(2)
faddp st(1),st // A0,2
fld dword ptr [edx+014h] // s1[1][1]
fmul dword ptr [esi+024h] // s2[1][2] M1,2
fxch st(1)
fstp dword ptr [eax+020h] // [0][2] S0
fld dword ptr [edx+024h] // s1[1][2]
fmul dword ptr [esi+028h] // s2[2][2] M1,3
fxch st(2)
faddp st(1),st // A1,1
fld dword ptr [edx+8] // s1[2][0]
fmul dword ptr [esi+020h] // s2[0][2] M2,1
fxch st(2)
faddp st(1),st // A1,2
fld dword ptr [edx+018h] // s1[2][1]
fmul dword ptr [esi+024h] // s2[1][2] M2,2
fxch st(1)
fstp dword ptr [eax+024h] // [1][2] S1
fld dword ptr [edx+028h] // s1[2][2]
fmul dword ptr [esi+028h] // s2[2][2] M2,3
fxch st(2)
faddp st(1),st // A2,1
fld dword ptr [edx+0ch] // s1[3][0]
fmul dword ptr [esi+020h] // s2[0][2] M3,1
fxch st(2)
faddp st(1),st // A2,2
fld dword ptr [edx+01ch] // s1[3][1]
fmul dword ptr [esi+024h] // s2[1][2] M3,2
fxch st(1)
fstp dword ptr [eax+028h] // [2][2] S2
fld dword ptr [edx+02ch] // s1[3][2]
fmul dword ptr [esi+028h] // s2[2][2] M3,3
fxch st(2)
faddp st(1),st // A3,1
faddp st(1),st // A3,2
fadd dword ptr [esi+02Ch] // s2[3][2] A3,3
fstp dword ptr [eax+02Ch] // [3][2] S3
pop esi
}
#else
(*this)(0,0) =
Source1(0,0)*Source2(0,0)
+ Source1(0,1)*Source2(1,0)
+ Source1(0,2)*Source2(2,0);
(*this)(1,0) =
Source1(1,0)*Source2(0,0)
+ Source1(1,1)*Source2(1,0)
+ Source1(1,2)*Source2(2,0);
(*this)(2,0) =
Source1(2,0)*Source2(0,0)
+ Source1(2,1)*Source2(1,0)
+ Source1(2,2)*Source2(2,0);
(*this)(3,0) =
Source1(3,0)*Source2(0,0)
+ Source1(3,1)*Source2(1,0)
+ Source1(3,2)*Source2(2,0)
+ Source2(3,0);
(*this)(0,1) =
Source1(0,0)*Source2(0,1)
+ Source1(0,1)*Source2(1,1)
+ Source1(0,2)*Source2(2,1);
(*this)(1,1) =
Source1(1,0)*Source2(0,1)
+ Source1(1,1)*Source2(1,1)
+ Source1(1,2)*Source2(2,1);
(*this)(2,1) =
Source1(2,0)*Source2(0,1)
+ Source1(2,1)*Source2(1,1)
+ Source1(2,2)*Source2(2,1);
(*this)(3,1) =
Source1(3,0)*Source2(0,1)
+ Source1(3,1)*Source2(1,1)
+ Source1(3,2)*Source2(2,1)
+ Source2(3,1);
(*this)(0,2) =
Source1(0,0)*Source2(0,2)
+ Source1(0,1)*Source2(1,2)
+ Source1(0,2)*Source2(2,2);
(*this)(1,2) =
Source1(1,0)*Source2(0,2)
+ Source1(1,1)*Source2(1,2)
+ Source1(1,2)*Source2(2,2);
(*this)(2,2) =
Source1(2,0)*Source2(0,2)
+ Source1(2,1)*Source2(1,2)
+ Source1(2,2)*Source2(2,2);
(*this)(3,2) =
Source1(3,0)*Source2(0,2)
+ Source1(3,1)*Source2(1,2)
+ Source1(3,2)*Source2(2,2)
+ Source2(3,2);
#endif
return *this;
}
#endif
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Invert(const AffineMatrix4D& Source)
{
Check_Pointer(this);
Check_Object(&Source);
Verify(this != &Source);
(*this)(0,0) = Source(1,1)*Source(2,2) - Source(1,2)*Source(2,1);
(*this)(1,0) = Source(1,2)*Source(2,0) - Source(1,0)*Source(2,2);
(*this)(2,0) = Source(1,0)*Source(2,1) - Source(1,1)*Source(2,0);
Scalar det =
(*this)(0,0)*Source(0,0)
+ (*this)(1,0)*Source(0,1)
+ (*this)(2,0)*Source(0,2);
Verify(!Small_Enough(det));
(*this)(3,0) =
-Source(3,0)*(*this)(0,0)
- Source(3,1)*(*this)(1,0)
- Source(3,2)*(*this)(2,0);
(*this)(0,1) = Source(0,2)*Source(2,1) - Source(0,1)*Source(2,2);
(*this)(1,1) = Source(0,0)*Source(2,2) - Source(0,2)*Source(2,0);
(*this)(2,1) = Source(0,1)*Source(2,0) - Source(0,0)*Source(2,1);
(*this)(3,1) =
-Source(3,0)*(*this)(0,1)
- Source(3,1)*(*this)(1,1)
- Source(3,2)*(*this)(2,1);
(*this)(0,2) = Source(0,1)*Source(1,2) - Source(0,2)*Source(1,1);
(*this)(1,2) = Source(1,0)*Source(0,2) - Source(0,0)*Source(1,2);
(*this)(2,2) = Source(0,0)*Source(1,1) - Source(0,1)*Source(1,0);
(*this)(3,2) =
-Source(3,0)*(*this)(0,2)
- Source(3,1)*(*this)(1,2)
- Source(3,2)*(*this)(2,2);
det = 1.0f/det;
for (int i=0; i<12; ++i)
{
entries[i] *= det;
}
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Multiply(const AffineMatrix4D &m,const Vector3D &v)
{
Check_Pointer(this);
Check_Object(&m);
Check_Object(&v);
(*this)(0,0) = m(0,0)*v.x;
(*this)(1,0) = m(1,0)*v.x;
(*this)(2,0) = m(2,0)*v.x;
(*this)(3,0) = m(3,0)*v.x;
(*this)(0,1) = m(0,1)*v.y;
(*this)(1,1) = m(1,1)*v.y;
(*this)(2,1) = m(2,1)*v.y;
(*this)(3,1) = m(3,1)*v.y;
(*this)(0,2) = m(0,2)*v.z;
(*this)(1,2) = m(1,2)*v.z;
(*this)(2,2) = m(2,2)*v.z;
(*this)(3,2) = m(3,2)*v.z;
return *this;
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Multiply(const AffineMatrix4D& m,const UnitQuaternion &q)
{
Check_Pointer(this);
Check_Object(&m);
Check_Object(&q);
LinearMatrix4D t(LinearMatrix4D::Identity);
t.BuildRotation(q);
return Multiply(m,t);
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Multiply(const AffineMatrix4D &m,const Point3D& p)
{
Check_Pointer(this);
Check_Object(&m);
Check_Object(&p);
(*this)(3,0) = m(3,0) + p.x;
(*this)(3,1) = m(3,1) + p.y;
(*this)(3,2) = m(3,2) + p.z;
return *this;
}
//
//###########################################################################
//###########################################################################
//
Scalar
AffineMatrix4D::Determinant() const
{
Check_Object(this);
return
(*this)(0,0)*((*this)(1,1)*(*this)(2,2) - (*this)(1,2)*(*this)(2,1))
+ (*this)(0,1)*((*this)(1,2)*(*this)(2,0) - (*this)(1,0)*(*this)(2,2))
+ (*this)(0,2)*((*this)(1,0)*(*this)(2,1) - (*this)(1,1)*(*this)(2,0));
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::Solve()
{
Check_Object(this);
int column;
Scalar temp;
//
//------------------------------------------------------------------
// Make sure that we get a decent value into the first diagonal spot
//------------------------------------------------------------------
//
if (!(*this)(0,0))
{
for (column=0; column<3; ++column)
if ((*this)(0,column))
break;
Verify(column != 3);
//
//--------------
// Swap the columns
//--------------
//
temp = (*this)(0,0);
(*this)(0,0) = (*this)(0,column);
(*this)(0,column) = temp;
temp = (*this)(1,0);
(*this)(1,0) = (*this)(1,column);
(*this)(1,column) = temp;
temp = (*this)(2,0);
(*this)(2,0) = (*this)(2,column);
(*this)(2,column) = temp;
temp = (*this)(3,0);
(*this)(3,0) = (*this)(3,column);
(*this)(3,column) = temp;
}
//
//------------------------------------
// Make sure the diagonal entry is 1.0
//------------------------------------
//
temp = (*this)(0,0);
(*this)(0,0) = 1.0f;
(*this)(1,0) /= temp;
(*this)(2,0) /= temp;
(*this)(3,0) /= temp;
//
//------------------------
// Make the first row zero
//------------------------
//
temp = (*this)(0,1);
(*this)(0,1) = 0.0f;
(*this)(1,1) -= temp * (*this)(1,0);
(*this)(2,1) -= temp * (*this)(2,0);
(*this)(3,1) -= temp * (*this)(3,0);
temp = (*this)(0,2);
(*this)(0,2) = 0.0f;
(*this)(1,2) -= temp * (*this)(1,0);
(*this)(2,2) -= temp * (*this)(2,0);
(*this)(3,2) -= temp * (*this)(3,0);
//
//-------------------------------------------------------------------
// Make sure that we get a decent value into the second diagonal spot
//-------------------------------------------------------------------
//
if (!(*this)(1,1))
{
Verify(!(*this)(2,2));
//
//---------------------
// Swap the (*this) columns
//---------------------
//
temp = (*this)(1,1);
(*this)(1,1) = (*this)(1,2);
(*this)(1,2) = temp;
temp = (*this)(2,1);
(*this)(2,1) = (*this)(2,2);
(*this)(2,2) = temp;
temp = (*this)(3,1);
(*this)(3,1) = (*this)(3,2);
(*this)(3,2) = temp;
}
//
//-----------------------------------
// Make the second diaginal entry 1.0
//-----------------------------------
//
temp = (*this)(1,1);
(*this)(1,1) = 1.0f;
(*this)(2,1) /= temp;
(*this)(3,1) /= temp;
//
//------------------------------------
// Make the second row zeros otherwise
//------------------------------------
//
temp = (*this)(1,0);
(*this)(1,0) = 0.0f;
(*this)(2,0) -= temp * (*this)(2,1);
(*this)(3,0) -= temp * (*this)(3,1);
temp = (*this)(1,2);
(*this)(1,2) = 0.0f;
(*this)(2,2) -= temp * (*this)(2,1);
(*this)(3,2) -= temp * (*this)(3,1);
//
//---------------------------
// Make the last diagonal 1.0
//---------------------------
//
Verify((*this)(2,2));
temp = (*this)(2,2);
(*this)(2,2) = 1.0f;
(*this)(3,2) /= temp;
//
//------------------------------------
// Make the third row zeros otherwise
//------------------------------------
//
temp = (*this)(2,0);
(*this)(2,0) = 0.0f;
(*this)(3,0) -= temp * (*this)(3,2);
temp = (*this)(2,1);
(*this)(2,1) = 0.0f;
(*this)(3,1) -= temp * (*this)(3,2);
//
//-------------------------
// Return the reduced array
//-------------------------
//
return *this;
}
//
//############################################################################
//############################################################################
//
#if !defined(Spew)
void
Spew(
const char* group,
const AffineMatrix4D &matrix
)
{
Check_Object(&matrix);
SPEW((
group,
"\n\t| %9f, %9f, %9f, 0 |",
matrix(0,0),
matrix(0,1),
matrix(0,2)
));
SPEW((
group,
"\t| %9f, %9f, %9f, 0 |",
matrix(1,0),
matrix(1,1),
matrix(1,2)
));
SPEW((
group,
"\t| %9f, %9f, %9f, 0 |",
matrix(2,0),
matrix(2,1),
matrix(2,2)
));
SPEW((
group,
"\t| %9f, %9f, %9f, 1 |+",
matrix(3,0),
matrix(3,1),
matrix(3,2)
));
}
#endif
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildLookAt(const YawPitchRange& rotation)
{
YawPitchRoll ypr(rotation.yaw,rotation.pitch,0);
BuildRotation(ypr);
UnitVector3D backward;
GetLocalBackwardInWorld(&backward);
Point3D backward_point(backward);
backward_point *= rotation.range;
backward_point += (Point3D)*this;
BuildTranslation(backward_point);
return (*this);
}
//
//###########################################################################
//###########################################################################
//
AffineMatrix4D&
AffineMatrix4D::BuildLookFrom(const YawPitchRange& rotation)
{
YawPitchRoll ypr(-rotation.yaw,-rotation.pitch,0);
BuildRotation(ypr);
UnitVector3D forward;
GetLocalForwardInWorld(&forward);
Point3D forward_point(forward);
forward_point *= rotation.range;
forward_point += (Point3D)*this;
BuildTranslation(forward_point);
return (*this);
}
Reference in New Issue View Git Blame Copy Permalink