rotation.cpp

机甲指挥官2源代码

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

#include "StuffHeaders.hpp"

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ EulerAngles ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const EulerAngles
	EulerAngles::Identity(0.0f,0.0f,0.0f);



bool UseFastLerp = true;
bool UseFastNormalize = true;



static bool __stdcall Check_UseFastLerp() {return UseFastLerp == true;}
static bool __stdcall Check_UseFastNormalize() {return UseFastNormalize == true;}


static void __stdcall Activate_UseFastLerp() {UseFastLerp = !UseFastLerp;}
static void __stdcall Activate_UseFastNormalize() {UseFastNormalize = !UseFastNormalize;}



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

	LinearMatrix4D m;
	m.BuildRotation(angles);
	*this = m;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::operator=(const UnitQuaternion &quaternion)
{
	Check_Pointer(this);
	Check_Object(&quaternion);

	LinearMatrix4D m;
	m.BuildRotation(quaternion);
	return *this = m;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::operator=(const LinearMatrix4D &matrix)
{
	Check_Pointer(this);
	Check_Object(&matrix);

	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
		p,y,r;

	//
	//-------------------------------------------------
	// First deal with the singularity of 90 degree yaw
	//-------------------------------------------------
	//
	y.sine = -matrix(0,2);
	if (Close_Enough(y.sine,1.0f,0.0001f))
	{
		p.sine = matrix(1,0);
		p.cosine = matrix(2,0);
		pitch = p;
		yaw = Pi_Over_2;
		roll = 0.0f;
		return *this;
	}

	//
	//-----------------------------
	// Now deal with -90 degree yaw
	//-----------------------------
	//
	else if (Close_Enough(y.sine,-1.0f,0.0001f))
	{
		p.sine = -matrix(1,0);
		p.cosine = -matrix(2,0);
		pitch = p;
		yaw = -Pi_Over_2;
		roll = 0.0f;
		return *this;
	}

	//
	//-------------------------------------------------------------------------
	// Otherwise, assume that pitch must be constrained between +/- 90 degrees.
	// This is particularly complex in this case, because pitch is the primary
	// axis.  So, we will set the yaw cosine to the appropriate sign that
	// results in the cosine of pitch being positive.
	//-------------------------------------------------------------------------
	//
	else
	{
		y.cosine = Sqrt(1.0f - y.sine*y.sine);

		Scalar one_y_cosine = 1.0f/ y.cosine;

		p.cosine = matrix(2,2) * one_y_cosine;

		if (p.cosine < 0.0f)
		{
			p.cosine = -p.cosine;
			y.cosine = -y.cosine;
			one_y_cosine = -one_y_cosine;
		}
		p.sine = matrix(1,2) * one_y_cosine;
		r.sine = matrix(0,1) * one_y_cosine;
		r.cosine = matrix(0,0) * one_y_cosine;
		#if defined(_ARMOR)
			Scalar temp = p.sine*y.sine*r.cosine - p.cosine*r.sine;
			Verify(Close_Enough(temp, matrix(1,0), 5e-3f));
		#endif
	}

	pitch = p;
	yaw = y;
	roll = r;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
bool
	Stuff::Small_Enough(
		const EulerAngles& angles,
		Scalar e
	)
{
	Check_Object(&angles);

	return
		Small_Enough(angles.pitch,e)
		 && Small_Enough(angles.yaw,e)
		 && Small_Enough(angles.roll,e);
}

//
//#############################################################################
//#############################################################################
//
bool
	Stuff::Close_Enough(
		const EulerAngles& a1,
		const EulerAngles& a2,
		Scalar e
	)
{
	Check_Object(&a1);
	Check_Object(&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::Lerp(
		const EulerAngles &a1,
		const EulerAngles &a2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check_Object(&a1);
	Check_Object(&a2);

	pitch = Stuff::Lerp(a1.pitch,a2.pitch,t);
	yaw = Stuff::Lerp(a1.yaw,a2.yaw,t);
	roll = Stuff::Lerp(a1.roll,a2.roll,t);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::Normalize()
{
	Check_Pointer(this);

	pitch.Normalize();
	yaw.Normalize();
	roll.Normalize();
	return *this;
}

//
//#############################################################################
//#############################################################################
//
#if !defined(Spew)
	void
		Spew(
			const char* group,
			const EulerAngles &angle
		)
	{
		Check_Object(&angle);
		SPEW((group, "<+"));
		Spew(group, angle.pitch);
		SPEW((group, ",+"));
		Spew(group, angle.yaw);
		SPEW((group, ",+"));
		Spew(group, angle.roll);
		SPEW((group, ">+"));
	}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ YawPitchRoll ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const YawPitchRoll
	YawPitchRoll::Identity(0.0f, 0.0f, 0.0f);

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

	LinearMatrix4D m;
	m.BuildRotation(angles);
	*this = m;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::operator=(const UnitQuaternion &quaternion)
{
	Check_Pointer(this);
	Check_Object(&quaternion);

	LinearMatrix4D m;
	m.BuildRotation(quaternion);
	return *this = m;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::operator=(const LinearMatrix4D &matrix)
{
	Check_Pointer(this);
	Check_Object(&matrix);

	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
		p,y,r;

	//
	//---------------------------------------------------
	// First deal with the singularity of 90 degree pitch
	//---------------------------------------------------
	//
	p.sine = -matrix(2,1);
	if (Close_Enough(p.sine,1.0f,0.0001f))
	{
		y.sine = matrix(1,0);
		y.cosine = matrix(0,0);
		yaw = y;
		pitch = Pi_Over_2;
		roll = 0.0f;
		return *this;
	}

	//
	//-------------------------------
	// Now deal with -90 degree pitch
	//-------------------------------
	//
	else if (Close_Enough(p.sine,-1.0f,0.0001f))
	{
		y.sine = matrix(0,2);
		y.cosine = matrix(0,0);
		yaw = y;
		pitch = -Pi_Over_2;
		roll = 0.0f;
		return *this;
	}

	//
	//------------------------------------------------------------------------
	// Otherwise, assume that pitch must be constrained between +/- 90 degrees
	//------------------------------------------------------------------------
	//
	else {
		p.cosine = Sqrt(1.0f - p.sine*p.sine);
		y.sine = matrix(2,0) / p.cosine;
		y.cosine = matrix(2,2) / p.cosine;
		r.sine = matrix(0,1) / p.cosine;
		r.cosine = matrix(1,1) / p.cosine;
		Verify(
			Close_Enough(
				y.cosine*r.cosine + p.sine*y.sine*r.sine,
				matrix(0,0),
				1e-4f
			)
		);
	}

	pitch = p;
	yaw = y;
	roll = r;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
bool
	Stuff::Small_Enough(
		const YawPitchRoll& angles,
		Scalar e
	)
{
	Check_Object(&angles);

	return
		Small_Enough(angles.pitch,e)
		 && Small_Enough(angles.yaw,e)
		 && Small_Enough(angles.roll,e);
}

//
//#############################################################################
//#############################################################################
//
bool
	Stuff::Close_Enough(
		const YawPitchRoll& a1,
		const YawPitchRoll& a2,
		Scalar e
	)
{
	Check_Object(&a1);
	Check_Object(&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::Lerp(
		const YawPitchRoll &a1,
		const YawPitchRoll &a2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check_Object(&a1);
	Check_Object(&a2);

	yaw = Stuff::Lerp(a1.yaw,a2.yaw,t);
	pitch = Stuff::Lerp(a1.pitch,a2.pitch,t);
	roll = Stuff::Lerp(a1.roll,a2.roll,t);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::Normalize()
{
	Check_Pointer(this);

	yaw.Normalize();
	pitch.Normalize();
	roll.Normalize();
	return *this;
}

//
//#############################################################################
//#############################################################################
//
#if !defined(Spew)
	void
		Spew(
			const char* group,
			const YawPitchRoll &angle
		)
	{
		Check_Object(&angle);

		SPEW((group, "<+"));
		Spew(group, angle.yaw);
		SPEW((group, ",+"));
		Spew(group, angle.pitch);
		SPEW((group, ",+"));
		Spew(group, angle.roll);
		SPEW((group, ">+"));
	}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ UnitQuaternion ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const UnitQuaternion
	UnitQuaternion::Identity(0.0f, 0.0f, 0.0f, 1.0f);
DEFINE_TIMER(UnitQuaternion, SlerpTime);
DWORD
	UnitQuaternion::SlerpCount;

//
//#############################################################################
//#############################################################################
//

const int QuaternionLerpTableSize=static_cast<int>(1024);
const int SinTableSize=static_cast<int>(1024);


const float MinCosom = static_cast<float>(-1.0f);
const float MaxCosom = static_cast<float>(1.0f);
const float CosomRangeOverOne = static_cast<float>(1.0f/(MaxCosom-MinCosom));
const float CosBiggestNumber = (float)static_cast<unsigned int>(0xffffffff>>(32-10));


const float MinSin = static_cast<float>(-1.3);
const float MaxSin = static_cast<float>(1.3);
const float SinRangeOverOne = static_cast<float>(1.0f/(MaxSin-MinSin));
const float SinIncrement = static_cast<float>((MaxSin - MinSin) / SinTableSize);
const float SinBiggestNumber = (float)static_cast<unsigned int>(0xffffffff>>(32-10));


float Omega_Table[QuaternionLerpTableSize];
float SinomOverOne_Table[QuaternionLerpTableSize];
float Sin_Table[SinTableSize];

bool  quaternionFastLerpTableBuilt = false;
float tableIncrementStepOverOne;

//
//#############################################################################
//#############################################################################
//

void
	UnitQuaternion::InitializeClass()
{

	Verify(!quaternionFastLerpTableBuilt);
	Verify(QuaternionLerpTableSize > 0);

	Initialize_Timer(SlerpTime, "Slerp Time");
	AddStatistic( "Quat Slerp Count",	"slerps",	gos_DWORD, &SlerpCount, Stat_AutoReset);

	AddDebuggerMenuItem("Libraries\\Animation\\Use Fast Lerp", Check_UseFastLerp, Activate_UseFastLerp, NULL );
	AddDebuggerMenuItem("Libraries\\Animation\\Use Fast Normalize", Check_UseFastNormalize, Activate_UseFastNormalize, NULL );
	

	float increment_step = (MaxCosom - MinCosom) / QuaternionLerpTableSize;
	tableIncrementStepOverOne = 1.0f / increment_step;
	float cosom = MinCosom;

	


	for (int i = 0; i < QuaternionLerpTableSize; ++i)
	{
		Verify(cosom >= MinCosom);
		Verify(cosom <= MaxCosom);

		Omega_Table[i] = Arccos(cosom);
		SinomOverOne_Table[i] = 1.0f/Sin(Omega_Table[i]);

		cosom += increment_step;
	}


	float sin_seed = MinSin;
	
	for (i = 0; i < SinTableSize; ++i)
	{
		Verify(sin_seed >= MinSin);
		Verify(sin_seed <= MaxSin);

		Sin_Table[i] = Sin(sin_seed);

		sin_seed += SinIncrement;
	}

	quaternionFastLerpTableBuilt = true;
}

//
//#############################################################################
//#############################################################################
//

void
	UnitQuaternion::TerminateClass()
{
	quaternionFastLerpTableBuilt = false;
}

//
//#############################################################################
//#############################################################################
//