affinematrix.cpp

机甲指挥官2源代码

//===========================================================================//
// File:	affnmtrx.cc                                                      //
// Contents: Implementation details for the Affine matrices                  //
//---------------------------------------------------------------------------//
// Copyright (C) Microsoft Corporation. All rights reserved.                 //
//===========================================================================//

#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