Cyd
2b8ca921cb
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.
423 lines
13 KiB
C++
423 lines
13 KiB
C++
//===========================================================================//
|
|
// File: linmtrx.cc //
|
|
// Project: MUNGA Brick: Math Library //
|
|
// Contents: Implementation details for the linear 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 LinearMatrix4D
|
|
LinearMatrix4D::Identity(true);
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
void
|
|
LinearMatrix4D::AlignLocalAxisToWorldVector(
|
|
const Vector3D &world_target,
|
|
int pointing_axis,
|
|
int rotating_axis,
|
|
int minor_axis
|
|
)
|
|
{
|
|
Check_Object(this);
|
|
Check_Object(&world_target);
|
|
Verify(static_cast<unsigned>(pointing_axis) <= Z_Axis);
|
|
Verify(static_cast<unsigned>(rotating_axis) <= Z_Axis);
|
|
Verify(rotating_axis != pointing_axis);
|
|
|
|
//
|
|
//------------------------------------------------------------------
|
|
// These are the variables that the alignment algorithm must fill in
|
|
//------------------------------------------------------------------
|
|
//
|
|
UnitVector3D
|
|
rotation_vector,
|
|
pointing_vector,
|
|
minor_vector;
|
|
|
|
//
|
|
//------------------------------------------------------------------
|
|
// Extract the current target axis direction, then cross it with the
|
|
// plane target to find the minor axis direction (unsigned)
|
|
//------------------------------------------------------------------
|
|
//
|
|
if (Small_Enough(world_target.GetLengthSquared()))
|
|
return;
|
|
rotation_vector.x = (*this)(rotating_axis, X_Axis);
|
|
rotation_vector.y = (*this)(rotating_axis, Y_Axis);
|
|
rotation_vector.z = (*this)(rotating_axis, Z_Axis);
|
|
Check_Object(&rotation_vector);
|
|
Vector3D temp;
|
|
temp.Cross(rotation_vector, world_target);
|
|
|
|
//
|
|
//----------------------------------------------------------------------
|
|
// First check to see if we are rotating around a frozen axis. If so,
|
|
// if the axes specified are in the right-handed configuration, simply
|
|
// generate the new pointing axis values, otherwise negate the minor
|
|
// axis and generate the pointing vector appropriately
|
|
//----------------------------------------------------------------------
|
|
//
|
|
if (minor_axis == -1)
|
|
{
|
|
minor_axis = 3 - pointing_axis - rotating_axis;
|
|
if (Small_Enough(temp.GetLengthSquared()))
|
|
{
|
|
if (world_target*rotation_vector > 0.0f)
|
|
{
|
|
pointing_vector.x = (*this)(rotating_axis, X_Axis);
|
|
pointing_vector.y = (*this)(rotating_axis, Y_Axis);
|
|
pointing_vector.z = (*this)(rotating_axis, Z_Axis);
|
|
rotation_vector.x = -(*this)(pointing_axis, X_Axis);
|
|
rotation_vector.y = -(*this)(pointing_axis, Y_Axis);
|
|
rotation_vector.z = -(*this)(pointing_axis, Z_Axis);
|
|
}
|
|
else
|
|
{
|
|
pointing_vector.x = -(*this)(rotating_axis, X_Axis);
|
|
pointing_vector.y = -(*this)(rotating_axis, Y_Axis);
|
|
pointing_vector.z = -(*this)(rotating_axis, Z_Axis);
|
|
rotation_vector.x = (*this)(pointing_axis, X_Axis);
|
|
rotation_vector.y = (*this)(pointing_axis, Y_Axis);
|
|
rotation_vector.z = (*this)(pointing_axis, Z_Axis);
|
|
}
|
|
minor_vector.x = (*this)(minor_axis, X_Axis);
|
|
minor_vector.y = (*this)(minor_axis, Y_Axis);
|
|
minor_vector.z = (*this)(minor_axis, Z_Axis);
|
|
}
|
|
|
|
else
|
|
{
|
|
minor_vector.Normalize(temp);
|
|
if ((rotating_axis+1)%3 == pointing_axis)
|
|
pointing_vector.Vector3D::Cross(minor_vector, rotation_vector);
|
|
else
|
|
{
|
|
minor_vector.Vector3D::Negate(minor_vector);
|
|
pointing_vector.Vector3D::Cross(rotation_vector, minor_vector);
|
|
}
|
|
}
|
|
Check_Object(&pointing_vector);
|
|
}
|
|
|
|
//
|
|
//------------------------------------------------------------------------
|
|
// The next case to check is non-frozen rotation. In this case, maximum
|
|
// effort is taken to preserve the rotating matrix, but it will be rotated
|
|
// around the minor axis so that the pointing axis is exactly aligned with
|
|
// the target vector
|
|
//------------------------------------------------------------------------
|
|
//
|
|
else
|
|
{
|
|
|
|
//
|
|
//--------------------------------------------------------------------
|
|
// If the resultant vector is zero, it means the rotating axis is
|
|
// parallel to the target vector, and thus a correct orthogonal set of
|
|
// axis vectors can already be found in the matrix
|
|
//--------------------------------------------------------------------
|
|
//
|
|
Verify(minor_axis == 3 - pointing_axis - rotating_axis);
|
|
if (Small_Enough(temp.GetLengthSquared()))
|
|
{
|
|
if (world_target*rotation_vector > 0.0f)
|
|
{
|
|
pointing_vector.x = (*this)(rotating_axis, X_Axis);
|
|
pointing_vector.y = (*this)(rotating_axis, Y_Axis);
|
|
pointing_vector.z = (*this)(rotating_axis, Z_Axis);
|
|
rotation_vector.x = -(*this)(pointing_axis, X_Axis);
|
|
rotation_vector.y = -(*this)(pointing_axis, Y_Axis);
|
|
rotation_vector.z = -(*this)(pointing_axis, Z_Axis);
|
|
}
|
|
else
|
|
{
|
|
pointing_vector.x = -(*this)(rotating_axis, X_Axis);
|
|
pointing_vector.y = -(*this)(rotating_axis, Y_Axis);
|
|
pointing_vector.z = -(*this)(rotating_axis, Z_Axis);
|
|
rotation_vector.x = (*this)(pointing_axis, X_Axis);
|
|
rotation_vector.y = (*this)(pointing_axis, Y_Axis);
|
|
rotation_vector.z = (*this)(pointing_axis, Z_Axis);
|
|
}
|
|
minor_vector.x = (*this)(minor_axis, X_Axis);
|
|
minor_vector.y = (*this)(minor_axis, Y_Axis);
|
|
minor_vector.z = (*this)(minor_axis, Z_Axis);
|
|
}
|
|
|
|
//
|
|
//---------------------------------------------------------------------
|
|
// We have a non-trivial minor vector, so use it to generate the real
|
|
// minor axis, then calculate the new rotation axis. If the axes
|
|
// specified are in the right-handed configuration, simply generate the
|
|
// new pointing axis values, otherwise negate the minor axis and
|
|
// generate the pointing vector appropriately
|
|
//---------------------------------------------------------------------
|
|
//
|
|
else
|
|
{
|
|
pointing_vector.Normalize(world_target);
|
|
minor_vector.Normalize(temp);
|
|
if ((rotating_axis+1)%3 == pointing_axis)
|
|
rotation_vector.Vector3D::Cross(pointing_vector, minor_vector);
|
|
else
|
|
{
|
|
minor_vector.Vector3D::Negate(minor_vector);
|
|
rotation_vector.Vector3D::Cross(minor_vector, pointing_vector);
|
|
}
|
|
Check_Object(&rotation_vector);
|
|
}
|
|
}
|
|
|
|
//
|
|
//------------------------------------------------
|
|
// Now stuff the unit vectors back into the matrix
|
|
//------------------------------------------------
|
|
//
|
|
Check_Object(&pointing_vector);
|
|
(*this)(pointing_axis, X_Axis) = pointing_vector.x;
|
|
(*this)(pointing_axis, Y_Axis) = pointing_vector.y;
|
|
(*this)(pointing_axis, Z_Axis) = pointing_vector.z;
|
|
|
|
Check_Object(&rotation_vector);
|
|
(*this)(rotating_axis, X_Axis) = rotation_vector.x;
|
|
(*this)(rotating_axis, Y_Axis) = rotation_vector.y;
|
|
(*this)(rotating_axis, Z_Axis) = rotation_vector.z;
|
|
|
|
Check_Object(&minor_vector);
|
|
(*this)(minor_axis, X_Axis) = minor_vector.x;
|
|
(*this)(minor_axis, Y_Axis) = minor_vector.y;
|
|
(*this)(minor_axis, Z_Axis) = minor_vector.z;
|
|
|
|
Check_Object(this);
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
void
|
|
LinearMatrix4D::ComputeAxes(ReadOnlyArrayOf<Point3D> &points)
|
|
{
|
|
Check_Object(&points);
|
|
|
|
//
|
|
//-----------------------------
|
|
// Compute the covariant matrix
|
|
//-----------------------------
|
|
//
|
|
unsigned count = points.GetLength();
|
|
Verify(count > 1);
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
double z = 0.0;
|
|
double xx = 0.0;
|
|
double xy = 0.0;
|
|
double xz = 0.0;
|
|
double yy = 0.0;
|
|
double yz = 0.0;
|
|
double zz = 0.0;
|
|
unsigned i;
|
|
for (i=0; i<count; i++)
|
|
{
|
|
x += points[i].x;
|
|
y += points[i].y;
|
|
z += points[i].z;
|
|
xx += points[i].x*points[i].x;
|
|
xy += points[i].x*points[i].y;
|
|
xz += points[i].x*points[i].z;
|
|
yy += points[i].y*points[i].y;
|
|
yz += points[i].y*points[i].z;
|
|
zz += points[i].z*points[i].z;
|
|
}
|
|
double t = 1.0 / count;
|
|
xx -= t*x*x;
|
|
xy -= t*x*y;
|
|
xz -= t*x*z;
|
|
yy -= t*y*y;
|
|
yz -= t*y*z;
|
|
zz -= t*z*z;
|
|
|
|
//
|
|
//-----------------------------------------------------------
|
|
// Set the center of the matrix to the centroid of the points
|
|
//-----------------------------------------------------------
|
|
//
|
|
(*this)(3,0) = static_cast<Scalar>(x * t);
|
|
(*this)(3,1) = static_cast<Scalar>(y * t);
|
|
(*this)(3,2) = static_cast<Scalar>(z * t);
|
|
|
|
//
|
|
//--------------------------------
|
|
// Figure out the two biggest axes
|
|
//--------------------------------
|
|
//
|
|
double coefs[3][3];
|
|
coefs[0][0] = xx;
|
|
coefs[0][1] = xy;
|
|
coefs[0][2] = xz;
|
|
coefs[1][0] = xy;
|
|
coefs[1][1] = yy;
|
|
coefs[1][2] = yz;
|
|
coefs[2][0] = xz;
|
|
coefs[2][1] = yz;
|
|
coefs[2][2] = zz;
|
|
int major_axis;
|
|
int minor_axis;
|
|
if (xx > yy)
|
|
{
|
|
if (xx > zz)
|
|
{
|
|
major_axis = X_Axis;
|
|
minor_axis = (yy > zz) ? Y_Axis : Z_Axis;
|
|
}
|
|
else
|
|
{
|
|
major_axis = Z_Axis;
|
|
minor_axis = X_Axis;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (yy > zz)
|
|
{
|
|
major_axis = Y_Axis;
|
|
minor_axis = (xx > zz) ? X_Axis : Z_Axis;
|
|
}
|
|
else
|
|
{
|
|
major_axis = Z_Axis;
|
|
minor_axis = Y_Axis;
|
|
}
|
|
}
|
|
|
|
//
|
|
//-------------------------
|
|
// Compute the first vector
|
|
//-------------------------
|
|
//
|
|
int other_axis = 3 - major_axis - minor_axis;
|
|
Vector3D temp;
|
|
temp[major_axis] = 1.0f;
|
|
temp[minor_axis] = static_cast<Scalar>(coefs[major_axis][minor_axis] / coefs[major_axis][major_axis]);
|
|
temp[other_axis] = static_cast<Scalar>(coefs[major_axis][other_axis] / coefs[major_axis][major_axis]);
|
|
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
// Compute the second vector by applying the Gram-Schmidt process to the first
|
|
//----------------------------------------------------------------------------
|
|
//
|
|
Vector3D temp2;
|
|
temp2[major_axis] = static_cast<Scalar>(coefs[minor_axis][major_axis] / coefs[minor_axis][minor_axis]);
|
|
temp2[minor_axis] = 1.0f;
|
|
temp2[other_axis] = static_cast<Scalar>(coefs[minor_axis][other_axis] / coefs[minor_axis][minor_axis]);
|
|
temp2.AddScaled(temp2, temp, -(temp*temp2)/(temp*temp));
|
|
|
|
//
|
|
//---------------------------------------------------------------------------
|
|
// Now, normalize the two vector and we should end up with orthogonal vectors
|
|
//---------------------------------------------------------------------------
|
|
//
|
|
UnitVector3D major_direction(temp);
|
|
UnitVector3D minor_direction(temp2);
|
|
Verify(Small_Enough(major_direction*minor_direction));
|
|
temp.Cross(major_direction, minor_direction);
|
|
(*this)(0,0) = major_direction.x;
|
|
(*this)(0,1) = major_direction.y;
|
|
(*this)(0,2) = major_direction.z;
|
|
(*this)(1,0) = minor_direction.x;
|
|
(*this)(1,1) = minor_direction.y;
|
|
(*this)(1,2) = minor_direction.z;
|
|
(*this)(2,0) = temp.x;
|
|
(*this)(2,1) = temp.y;
|
|
(*this)(2,2) = temp.z;
|
|
Check_Object(this);
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
LinearMatrix4D&
|
|
LinearMatrix4D::Invert(const LinearMatrix4D& m)
|
|
{
|
|
Check_Pointer(this);
|
|
Check_Object(&m);
|
|
Verify(this != &m);
|
|
|
|
//
|
|
//-----------------------------------------
|
|
// First, transpose the 3x3 rotation matrix
|
|
//-----------------------------------------
|
|
//
|
|
(*this)(0,0) = m(0,0);
|
|
(*this)(0,1) = m(1,0);
|
|
(*this)(0,2) = m(2,0);
|
|
(*this)(1,0) = m(0,1);
|
|
(*this)(1,1) = m(1,1);
|
|
(*this)(1,2) = m(2,1);
|
|
(*this)(2,0) = m(0,2);
|
|
(*this)(2,1) = m(1,2);
|
|
(*this)(2,2) = m(2,2);
|
|
|
|
//
|
|
//----------------------------
|
|
// Now run the offsets through
|
|
//----------------------------
|
|
//
|
|
(*this)(3,0) = -m(3,0)*m(0,0) - m(3,1)*m(0,1) - m(3,2)*m(0,2);
|
|
(*this)(3,1) = -m(3,0)*m(1,0) - m(3,1)*m(1,1) - m(3,2)*m(1,2);
|
|
(*this)(3,2) = -m(3,0)*m(2,0) - m(3,1)*m(2,1) - m(3,2)*m(2,2);
|
|
return *this;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
LinearMatrix4D&
|
|
LinearMatrix4D::Normalize()
|
|
{
|
|
Check_Pointer(this);
|
|
|
|
#if defined(LEFT_HANDED_COORDINATES)
|
|
#error Right handed coordinate dependancy!
|
|
#endif
|
|
(*this)(0,2) = (*this)(1,0)*(*this)(2,1) - (*this)(1,1)*(*this)(2,0);
|
|
(*this)(1,2) = (*this)(2,0)*(*this)(0,1) - (*this)(2,1)*(*this)(0,0);
|
|
(*this)(2,2) = (*this)(0,0)*(*this)(1,1) - (*this)(0,1)*(*this)(1,0);
|
|
return *this;
|
|
}
|
|
|
|
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
|
//
|
|
void
|
|
LinearMatrix4D::TestInstance() const
|
|
{
|
|
UnitVector3D v1;
|
|
v1.x = (*this)(0,0);
|
|
v1.y = (*this)(0,1);
|
|
v1.z = (*this)(0,2);
|
|
Check_Object(&v1);
|
|
|
|
UnitVector3D v2;
|
|
v2.x = (*this)(1,0);
|
|
v2.y = (*this)(1,1);
|
|
v2.z = (*this)(1,2);
|
|
Check_Object(&v2);
|
|
|
|
UnitVector3D v3;
|
|
v3.Vector3D::Cross(v1,v2);
|
|
#if defined(LEFT_HANDED_COORDINATES)
|
|
#error Right handed coordinate depenancy!
|
|
#endif
|
|
|
|
|
|
Verify(Close_Enough(v3.x, (*this)(2,0)));
|
|
Verify(Close_Enough(v3.y, (*this)(2,1)));
|
|
Verify(Close_Enough(v3.z, (*this)(2,2)));
|
|
|
|
|
|
}
|