//===========================================================================//
// Copyright (C) Microsoft Corporation. All rights reserved.                 //
//===========================================================================//

#include "StuffHeaders.hpp"

OBB
	OBB::Identity(LinearMatrix4D::Identity, Vector3D::Identity, 0.0f);

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	OBB::TestInstance() const
{
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
#if !defined(Spew)
	void
		Spew(
			const char* group,
			const OBB &box
		)
	{
		Check_Object(&box);
		SPEW((group, ""));
		SPEW((group, "  Transform = +"));
		Spew(group, box.localToParent);
		SPEW((group, ""));
		SPEW((
			group,
			"  Extents = <%4f,%4f,%4f>, Radius = %4f+",
			box.axisExtents.x,
			box.axisExtents.y,
			box.axisExtents.z,
			box.sphereRadius
		));
	}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB&
	OBB::Multiply(
		const OBB &obb,
		const LinearMatrix4D &matrix
	)
{
	Check_Pointer(this);
	Check_Object(&obb);
	Check_Object(&matrix);

	localToParent.Multiply(obb.localToParent, matrix);
	axisExtents = obb.axisExtents;
	sphereRadius = obb.sphereRadius;
	return *this;
}

#if 0	// moved it into hpp-file
//
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB&
	OBB::MultiplySphereOnly(
		const OBB &obb,
		const LinearMatrix4D &matrix
	)
{
	Check_Pointer(this);
	Check_Object(&obb);
	Check_Object(&matrix);

#if USE_ASSEMBLER_CODE

	Scalar *f = localToParent.entries;

	_asm {
		mov         edx, matrix
		push        esi
		mov         esi, obb.localToParent
		
		mov         eax, f

		fld         dword ptr [edx+4]		//	m[1][0]
		fmul        dword ptr [esi+01ch]	//	obb.localToParent(3,1)
		
		fld         dword ptr [edx+8]		//	m[2][0]
		fmul        dword ptr [esi+02Ch]	//	obb.localToParent(3,2)

		fxch		st(1)
		fadd        dword ptr [edx+0Ch]		//	m[3][0]
		
		fld         dword ptr [edx]			//	m[0][0]
		fmul        dword ptr [esi+0Ch]		//	obb.localToParent(3,0)

		fxch		st(2)
		faddp       st(1),st

		fld         dword ptr [edx+14h]		//	m[1][1]
		fmul        dword ptr [esi+01ch]	//	obb.localToParent(3,1)

		fxch		st(2)
		faddp       st(1),st

		fld         dword ptr [edx+18h]		//	m[2][1]
		fmul        dword ptr [esi+02ch]	//	obb.localToParent(3,2)
		
		fxch		st(1)
		fstp        dword ptr [eax+0ch]		//	localToParent(3,0)

		fadd        dword ptr [edx+1Ch]		//	m[3][1]

		fld         dword ptr [edx+10h]		//	m[0][1]
		fmul        dword ptr [esi+0ch]		//	obb.localToParent(3,0)

		fxch		st(2)
		faddp       st(1),st

		fld         dword ptr [edx+24h]		//	m[1][2]
		fmul        dword ptr [esi+01ch]	//	obb.localToParent(3,1)

		fxch		st(2)
		faddp       st(1),st

		fld         dword ptr [edx+28h]		//	m[2][2]
		fmul        dword ptr [esi+02ch]	//	obb.localToParent(3,2)
		
		fxch		st(1)
		fstp        dword ptr [eax+01ch]	//	localToParent(3,1)

		fadd        dword ptr [edx+2Ch]		//	m[3][2]
		
		fld         dword ptr [edx+20h]		//	m[0][2]
		fmul        dword ptr [esi+0ch]		//	obb.localToParent(3,0)

		fxch		st(2)
		faddp       st(1),st

		pop         esi

		faddp       st(1),st

		fstp        dword ptr [eax+02ch]	//	localToParent(3,2)
	}
#else
	localToParent(3,0) =
		obb.localToParent(3,0)*matrix(0,0)
		 + obb.localToParent(3,1)*matrix(1,0)
		 + obb.localToParent(3,2)*matrix(2,0)
		 + matrix(3,0);
	localToParent(3,1) =
		obb.localToParent(3,0)*matrix(0,1)
		 + obb.localToParent(3,1)*matrix(1,1)
		 + obb.localToParent(3,2)*matrix(2,1)
		 + matrix(3,1);
	localToParent(3,2) =
		obb.localToParent(3,0)*matrix(0,2)
		 + obb.localToParent(3,1)*matrix(1,2)
		 + obb.localToParent(3,2)*matrix(2,2)
		 + matrix(3,2);
#endif

	sphereRadius = obb.sphereRadius;
	return *this;
}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB::SeparatingAxis
	OBB::FindSeparatingAxis(const OBB& box) const
{
	Check_Object(this);
	Check_Object(&box);
	Verify(axisExtents.x >= 0.0f);
	Verify(axisExtents.y >= 0.0f);
	Verify(axisExtents.z >= 0.0f);
	Verify(box.axisExtents.x >= 0.0f);
	Verify(box.axisExtents.y >= 0.0f);
	Verify(box.axisExtents.z >= 0.0f);

	//
	//------------------------------------------
	// Get the distance between the centerpoints
	//------------------------------------------
	//
	Check_Object(&localToParent);
	Check_Object(&box.localToParent);
	Point3D
		distance(
			localToParent(3,0) - box.localToParent(3,0),
			localToParent(3,1) - box.localToParent(3,1),
			localToParent(3,2) - box.localToParent(3,2)
		);

	//
	//-----------------------------------------------------------------------
	// First check L = Ax.  The next 6 tests are basically face tests to try
	// and find a separating plane.  Both boxes are currently in parent space
	//-----------------------------------------------------------------------
	//
	Scalar ab[3][3];
	ab[0][0] =
		Fabs(
			localToParent(0,0) * box.localToParent(0,0)
			 + localToParent(0,1) * box.localToParent(0,1)
			 + localToParent(0,2) * box.localToParent(0,2)
		);
	ab[0][1] =
		Fabs(
			localToParent(0,0) * box.localToParent(1,0)
			 + localToParent(0,1) * box.localToParent(1,1)
			 + localToParent(0,2) * box.localToParent(1,2)
		);
	ab[0][2] =
		Fabs(
			localToParent(0,0) * box.localToParent(2,0)
			 + localToParent(0,1) * box.localToParent(2,1)
			 + localToParent(0,2) * box.localToParent(2,2)
		);

	Scalar radius =
		axisExtents.x
		 + box.axisExtents.x * ab[0][0]
		 + box.axisExtents.y * ab[0][1]
		 + box.axisExtents.z * ab[0][2];
	Scalar projection =
		localToParent(0,0) * distance.x
		 + localToParent(0,1) * distance.y
		 + localToParent(0,2) * distance.z;
	if (projection > radius || projection < -radius)
	{
		return A0;
	}

	//
	//-------------
	// Check L = Ay
	//-------------
	//
	ab[1][0] =
		Fabs(
			localToParent(1,0) * box.localToParent(0,0)
			 + localToParent(1,1) * box.localToParent(0,1)
			 + localToParent(1,2) * box.localToParent(0,2)
		);
	ab[1][1] =
		Fabs(
			localToParent(1,0) * box.localToParent(1,0)
			 + localToParent(1,1) * box.localToParent(1,1)
			 + localToParent(1,2) * box.localToParent(1,2)
		);
	ab[1][2] =
		Fabs(
			localToParent(1,0) * box.localToParent(2,0)
			 + localToParent(1,1) * box.localToParent(2,1)
			 + localToParent(1,2) * box.localToParent(2,2)
		);

	radius =
		axisExtents.y
		 + box.axisExtents.x * ab[1][0]
		 + box.axisExtents.y * ab[1][1]
		 + box.axisExtents.z * ab[1][2];
	projection =
		localToParent(1,0) * distance.x
		 + localToParent(1,1) * distance.y
		 + localToParent(1,2) * distance.z;
	if (projection > radius || projection < -radius)
	{
		return A1;
	}

	//
	//-------------
	// Check L = Az
	//-------------
	//
	ab[2][0] =
		Fabs(
			localToParent(2,0) * box.localToParent(0,0)
			 + localToParent(2,1) * box.localToParent(0,1)
			 + localToParent(2,2) * box.localToParent(0,2)
		);
	ab[2][1] =
		Fabs(
			localToParent(2,0) * box.localToParent(1,0)
			 + localToParent(2,1) * box.localToParent(1,1)
			 + localToParent(2,2) * box.localToParent(1,2)
		);
	ab[2][2] =
		Fabs(
			localToParent(2,0) * box.localToParent(2,0)
			 + localToParent(2,1) * box.localToParent(2,1)
			 + localToParent(2,2) * box.localToParent(2,2)
		);

	radius =
		axisExtents.z
		 + box.axisExtents.x * ab[2][0]
		 + box.axisExtents.y * ab[2][1]
		 + box.axisExtents.z * ab[2][2];
	projection =
		localToParent(2,0) * distance.x
		 + localToParent(2,1) * distance.y
		 + localToParent(2,2) * distance.z;
	if (projection > radius || projection < -radius)
	{
		return A2;
	}