matrix.cpp

机甲指挥官2源代码

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

#include "StuffHeaders.hpp"

const Matrix4D
	Matrix4D::Identity(true);

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::BuildIdentity()
{
	(*this)(0,0) = 1.0f;
	(*this)(1,0) = 0.0f;
	(*this)(2,0) = 0.0f;
	(*this)(3,0) = 0.0f;

	(*this)(0,1) = 0.0f;
	(*this)(1,1) = 1.0f;
	(*this)(2,1) = 0.0f;
	(*this)(3,1) = 0.0f;

	(*this)(0,2) = 0.0f;
	(*this)(1,2) = 0.0f;
	(*this)(2,2) = 1.0f;
	(*this)(3,2) = 0.0f;

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::operator=(const AffineMatrix4D &m)
{
	Check_Pointer(this);
	Check_Object(&m);

	Mem_Copy(entries, m.entries, sizeof(m.entries), sizeof(entries));
	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::operator=(const Origin3D& p)
{
	Check_Pointer(this);
	Check_Object(&p);

	BuildRotation(p.angularPosition);
	BuildTranslation(p.linearPosition);

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
Matrix4D&
	Matrix4D::BuildRotation(const EulerAngles &angles)
{
	Check_Pointer(this);
	Check_Object(&angles);

	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;
}

//
//#############################################################################
//#############################################################################
//
Matrix4D&
	Matrix4D::operator=(const EulerAngles &angles)
{
	Check_Pointer(this);
	Check_Object(&angles);

	(*this)(3,0) = 0.0f;
	(*this)(3,1) = 0.0f;
	(*this)(3,2) = 0.0f;

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return BuildRotation(angles);
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::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;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::operator=(const Point3D& p)
{
	Check_Pointer(this);
	Check_Object(&p);

	(*this)(0,0) = 1.0f;
	(*this)(1,0) = 0.0f;
	(*this)(2,0) = 0.0f;
	(*this)(3,0) = p.x;

	(*this)(0,1) = 0.0f;
	(*this)(1,1) = 1.0f;
	(*this)(2,1) = 0.0f;
	(*this)(3,1) = p.y;

	(*this)(0,2) = 0.0f;
	(*this)(1,2) = 0.0f;
	(*this)(2,2) = 1.0f;
	(*this)(3,2) = p.z;

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::BuildTranslation(const Point3D& p)
{
	Check_Pointer(this);
	Check_Object(&p);

	(*this)(3,0) = p.x;
	(*this)(3,1) = p.y;
	(*this)(3,2) = p.z;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
bool
	Stuff::Close_Enough(
		const Matrix4D &m1,
		const Matrix4D &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;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::Multiply(
		const Matrix4D &Source1,
		const Matrix4D &Source2
	)
{
	(*this)(0,0) =
		Source1(0,0)*Source2(0,0)
		 + Source1(0,1)*Source2(1,0)
		 + Source1(0,2)*Source2(2,0)
		 + Source1(0,3)*Source2(3,0);
	(*this)(1,0) =
		Source1(1,0)*Source2(0,0)
		 + Source1(1,1)*Source2(1,0)
		 + Source1(1,2)*Source2(2,0)
		 + Source1(1,3)*Source2(3,0);
	(*this)(2,0) =
		Source1(2,0)*Source2(0,0)
		 + Source1(2,1)*Source2(1,0)
		 + Source1(2,2)*Source2(2,0)
		 + Source1(2,3)*Source2(3,0);
	(*this)(3,0) =
		Source1(3,0)*Source2(0,0)
		 + Source1(3,1)*Source2(1,0)
		 + Source1(3,2)*Source2(2,0)
		 + Source1(3,3)*Source2(3,0);

	(*this)(0,1) =
		Source1(0,0)*Source2(0,1)
		 + Source1(0,1)*Source2(1,1)
		 + Source1(0,2)*Source2(2,1)
		 + Source1(0,3)*Source2(3,1);
	(*this)(1,1) =
		Source1(1,0)*Source2(0,1)
		 + Source1(1,1)*Source2(1,1)
		 + Source1(1,2)*Source2(2,1)
		 + Source1(1,3)*Source2(3,1);
	(*this)(2,1) =
		Source1(2,0)*Source2(0,1)
		 + Source1(2,1)*Source2(1,1)
		 + Source1(2,2)*Source2(2,1)
		 + Source1(2,3)*Source2(3,1);
	(*this)(3,1) =
		Source1(3,0)*Source2(0,1)
		 + Source1(3,1)*Source2(1,1)
		 + Source1(3,2)*Source2(2,1)
		 + Source1(3,3)*Source2(3,1);

	(*this)(0,2) =
		Source1(0,0)*Source2(0,2)
		 + Source1(0,1)*Source2(1,2)
		 + Source1(0,2)*Source2(2,2)
		 + Source1(0,3)*Source2(3,2);
	(*this)(1,2) =
		Source1(1,0)*Source2(0,2)
		 + Source1(1,1)*Source2(1,2)
		 + Source1(1,2)*Source2(2,2)
		 + Source1(1,3)*Source2(3,2);
	(*this)(2,2) =
		Source1(2,0)*Source2(0,2)
		 + Source1(2,1)*Source2(1,2)
		 + Source1(2,2)*Source2(2,2)
		 + Source1(2,3)*Source2(3,2);
	(*this)(3,2) =
		Source1(3,0)*Source2(0,2)
		 + Source1(3,1)*Source2(1,2)
		 + Source1(3,2)*Source2(2,2)
		 + Source1(3,3)*Source2(3,2);

	(*this)(0,3) =
		Source1(0,0)*Source2(0,3)
		 + Source1(0,1)*Source2(1,3)
		 + Source1(0,2)*Source2(2,3)
		 + Source1(0,3)*Source2(3,3);
	(*this)(1,3) =
		Source1(1,0)*Source2(0,3)
		 + Source1(1,1)*Source2(1,3)
		 + Source1(1,2)*Source2(2,3)
		 + Source1(1,3)*Source2(3,3);
	(*this)(2,3) =
		Source1(2,0)*Source2(0,3)
		 + Source1(2,1)*Source2(1,3)
		 + Source1(2,2)*Source2(2,3)
		 + Source1(2,3)*Source2(3,3);
	(*this)(3,3) =
		Source1(3,0)*Source2(0,3)
		 + Source1(3,1)*Source2(1,3)
		 + Source1(3,2)*Source2(2,3)
		 + Source1(3,3)*Source2(3,3);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4D&
	Matrix4D::Multiply(
		const Matrix4D &Source1,
		const AffineMatrix4D &Source2
	)
{
	(*this)(0,0) =
		Source1(0,0)*Source2(0,0)
		 + Source1(0,1)*Source2(1,0)
		 + Source1(0,2)*Source2(2,0)
		 + Source1(0,3)*Source2(3,0);
	(*this)(1,0) =
		Source1(1,0)*Source2(0,0)
		 + Source1(1,1)*Source2(1,0)
		 + Source1(1,2)*Source2(2,0)
		 + Source1(1,3)*Source2(3,0);
	(*this)(2,0) =
		Source1(2,0)*Source2(0,0)
		 + Source1(2,1)*Source2(1,0)
		 + Source1(2,2)*Source2(2,0)
		 + Source1(2,3)*Source2(3,0);
	(*this)(3,0) =
		Source1(3,0)*Source2(0,0)
		 + Source1(3,1)*Source2(1,0)
		 + Source1(3,2)*Source2(2,0)
		 + Source1(3,3)*Source2(3,0);

	(*this)(0,1) =
		Source1(0,0)*Source2(0,1)
		 + Source1(0,1)*Source2(1,1)
		 + Source1(0,2)*Source2(2,1)
		 + Source1(0,3)*Source2(3,1);
	(*this)(1,1) =
		Source1(1,0)*Source2(0,1)
		 + Source1(1,1)*Source2(1,1)
		 + Source1(1,2)*Source2(2,1)
		 + Source1(1,3)*Source2(3,1);
	(*this)(2,1) =
		Source1(2,0)*Source2(0,1)