#include "StuffHeaders.hpp"

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

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
int OBB::ComputeBounds(ReadOnlyArrayOf<Point3D> &points)
{
	Check_Object(&points);

	//
	//----------------------------------------------------------
	// Find the coordinate system the points would like to be in
	//----------------------------------------------------------
	//
	localToParent.ComputeAxes(points);
	LinearMatrix4D world_to_local;
	world_to_local.Invert(localToParent);
	int type = 1;

	//
	//----------------------------------------------------------------
	// Find the extents of both the local and world bounds and use the
	// smaller set
	//----------------------------------------------------------------
	//
	DynamicArrayOf<Point3D> local_points(points.GetLength());
	for (int i=0; i<points.GetLength(); ++i)
		local_points[i].Multiply(points[i], world_to_local);
	ExtentBox local_box;
	ExtentBox box;
	local_box.ComputeBounds(local_points);
	box.ComputeBounds(points);

	//
	//----------------------------------------------------
	// If we are using the world box reset localToParent
	//----------------------------------------------------
	//
	Point3D center_in_world;
	if (box.GetVolume() < local_box.GetVolume())
	{
		// Get center-in-world directly from world box
		box.GetCenterpoint(&center_in_world);
		localToParent = LinearMatrix4D::Identity;
		axisExtents.x = 0.5f * (box.maxX - box.minX) + SMALL;
		axisExtents.y = 0.5f * (box.maxY - box.minY) + SMALL;
		axisExtents.z = 0.5f * (box.maxZ - box.minZ) + SMALL;
		type = 2;
	}

	//
	//--------------------------------------------------------------
	// Else we are using the local box recompute the center-in-world
	//--------------------------------------------------------------
	//
	else {
		Point3D center;
		local_box.GetCenterpoint(&center);
		center_in_world.Multiply(center, localToParent);
		axisExtents.x = 0.5f * (local_box.maxX - local_box.minX) + SMALL;
		axisExtents.y = 0.5f * (local_box.maxY - local_box.minY) + SMALL;
		axisExtents.z = 0.5f * (local_box.maxZ - local_box.minZ) + SMALL;
	}

	localToParent(3,0) = center_in_world.x;
	localToParent(3,1) = center_in_world.y;
	localToParent(3,2) = center_in_world.z;

	sphereRadius = axisExtents.GetLength();
	return type;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
#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);

	//
	//-----------------------------------------------------------------------
	// The following routine is based on various simplifications of the
	// following formula: If L represents a line in space, two OBBs (A and B)
	// are separated by on that axis if (B-A)*L > sum of the projected radii.
	// The projected radius of each OBB is equal to the sum of the three
	// cardinal axes (in parent space) projected unto the line and multiplied
	// by the OBBs extent size in that direction
	//-----------------------------------------------------------------------
	//
	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.  Both boxes are assumed to
	// convert their local axes into a common parent space
	//----------------------------------------------------------------------
	//
	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)
		);

	//
	//--------------------------------------------------------------------------------------------------
	// The rows of the matrix encode the local axes in world space.  The first
	// separation axis we will be testing our x axis in world space, so
	// project the other box's primary axes onto it, and store the results
	// away for future reference
	//
	// The math for L = A0: (A0 is the X axis row of our matrix)
	//  Given: A0,A1, and B2 are orthogonal
	//         B0,B1, and B2 are orthogonal
	//         Ax, Ay, and Az >= 0
	//         Bx, By, and Bz >= 0
	//
	//  projection = (B-A)*A0
	//  radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
	//         = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
	//--------------------------------------------------------------------------------------------------
	//
	Scalar axis_projections[3][3];
	axis_projections[X_Axis][X_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[X_Axis][Y_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[X_Axis][Z_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
		);

	//
	//-----------------------------------------------------------------------
	// The maximum distance the centerpoints can be away and still touch is
	// equal to the sum of our x extent plus the other's extents, as modified
	// by the projection values.  If the boxes can't touch, return A0 as the
	// separating plane
	//-----------------------------------------------------------------------
	//
	Scalar radius =
		axisExtents.x
		 + box.axisExtents.x * axis_projections[X_Axis][0]
		 + box.axisExtents.y * axis_projections[X_Axis][1]
		 + box.axisExtents.z * axis_projections[X_Axis][2];
	Scalar projection =
		localToParent(X_Axis,0) * distance.x
		 + localToParent(X_Axis,1) * distance.y
		 + localToParent(X_Axis,2) * distance.z;
	if (projection > radius || projection < -radius)
		return A0;

	//
	//--------------------------------------------------------------------------------------------------
	// Do the same check against our Y axis
	//
	// The math for L = A1:
	//
	//  projection = (B-A)*A1
	//  radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
	//         = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Y_Axis][X_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Y_Axis][Y_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Y_Axis][Z_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.y
		 + box.axisExtents.x * axis_projections[1][0]
		 + box.axisExtents.y * axis_projections[1][1]
		 + box.axisExtents.z * axis_projections[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 our z axis
	//
	// The math for L = A2:
	//
	//  projection = (B-A)*A2
	//  radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
	//         = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Z_Axis][X_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Z_Axis][Y_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Z_Axis][Z_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.z
		 + box.axisExtents.x * axis_projections[2][0]
		 + box.axisExtents.y * axis_projections[2][1]
		 + box.axisExtents.z * axis_projections[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;

	//
	//--------------------------------------------------------------------------------------------------
	// See if the other box's X axis is a separator
	//
	// The math for L = B0:
	//
	//  projection = (B-A)*B0
	//  radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
	//         = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.x 
		 + axisExtents.x * axis_projections[0][X_Axis]
		 + axisExtents.y * axis_projections[1][X_Axis]
		 + axisExtents.z * axis_projections[2][X_Axis];
	projection =
		box.localToParent(X_Axis,0) * distance.x
		 + box.localToParent(X_Axis,1) * distance.y
		 + box.localToParent(X_Axis,2) * distance.z;
	if (projection > radius || projection < -radius)
		return B0;

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Y axis
	//
	// The math for L = B1:
	//
	//  projection = (B-A)*B1
	//  radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
	//         = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.y
		 + axisExtents.x * axis_projections[0][Y_Axis]
		 + axisExtents.y * axis_projections[1][Y_Axis]
		 + axisExtents.z * axis_projections[2][Y_Axis];
	projection =
		box.localToParent(Y_Axis,0) * distance.x
		 + box.localToParent(Y_Axis,1) * distance.y
		 + box.localToParent(Y_Axis,2) * distance.z;
	if (projection > radius || projection < -radius)
		return B1;

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Z axis
	//
	// The math for L = B2:
	//
	//  projection = (B-A)*B2
	//  radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
	//         = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.z
		 + axisExtents.x * axis_projections[0][Z_Axis]
		 + axisExtents.y * axis_projections[1][Z_Axis]
		 + axisExtents.z * axis_projections[2][Z_Axis];
	projection =
		box.localToParent(Z_Axis,0) * distance.x
		 + box.localToParent(Z_Axis,1) * distance.y
		 + box.localToParent(Z_Axis,2) * distance.z;
	if (projection > radius || projection < -radius)
		return B2;

	//
	//--------------------------------------------------------------------------
	// We have finished with the separation tests based upon the box faces.  We
	// now need to test with the edges, so make a matrix that converts the other
	// box into our space, then recompute the distance vector (its just the
	// other boxes's new origin)
	//--------------------------------------------------------------------------
	//
	LinearMatrix4D parent_to_local;
	parent_to_local.Invert(localToParent);
	LinearMatrix4D b;
	b.Multiply(box.localToParent, parent_to_local);
	distance = b;

	//
	//------------------------------------------------------------------------------------------------------------
	// The next 9 tests will create separation axes by crossing the 3 direction
	// vectors of our box with the 3 direction vectors of the other box
	//
	// The math for L = A0xB0 or <1,0,0>xB0:
	//
	//  projection = (B-A)*(A0xB0) = (B-A)*<0,-B(0,2),B(0,1)>
	//  radius = |Ax*((A0xB0)*A0)| + |Ay*((A0xB0)*A1)| + |Az*((A0xB0)*A2)|
	//           + |Bx*((A0xB0)*B0)| + |By*((A0xB0)*B1)| + |Bz*((A0xB0)*B2)|
	//         = Ay*|(A0xB0)*A1| + Az*|(A0xB0)*A2| + By*|(A0xB0)*B1| + Bz*|(A0xB0)*B2|
	//         = Ay*|(<1,0,0>xB0)*<0,1,0>| + Az*|(<1,0,0>xB0)*<0,0,1>| 
	//           + By*|(<1,0,0>xB0)*B1| + Bz*|(<1,0,0>xB0)*B2|
	//         = Ay*|<0,-B(0,2),B(0,1)>*<0,1,0>| + Az*|<0,-B(0,2),B(0,1)>*<0,0,1>|
	//           + By*|<0,-B(0,2),B(0,1)>*B1| + Bz*|<0,-B(0,2),B(0,1)>*B2|
	//         = Ay*|-B(0,2)| + Az*|B(0,1)| + By*|<0,-B(0,2),B(0,1)>*B1| + Bz*|<0,-B(0,2),B(0,1)>*B2|
	//         = Ay*|B(0,2)| + Az*|B(0,1)|
	//           + By*|<0,-B(0,2),B(0,1)>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<0,-B(0,2),B(0,1)>*<B(2,0),B(2,1),B(2,2)>|
	//         = Ay*|B(0,2)| + Az*|B(0,1)|
	//           + By*|<-B(0,2),B(0,1)>*<B(1,1),B(1,2)>| + Bz*|<-B(0,2),B(0,1)>*<B(2,1),B(2,2)>|
	//         = Ay*|B(0,2)| + Az*|B(0,1)|
	//           + By*|B(0,1)*B(1,2) - B(0,2)*<B(1,1)| + Bz*|B(0,1)*B(2,2) - B(0,2)*B(2,1)|
	//         = Ay*|B(0,2)| + Az*|B(0,1)| + By*dets(2,0) + Bz*dets(1,0)
	//------------------------------------------------------------------------------------------------------------
	//
	Scalar dets[3][3];
	dets[1][0] = Fabs(b(0,1)*b(2,2) - b(2,1)*b(0,2));
	dets[2][0] = Fabs(b(0,1)*b(1,2) - b(1,1)*b(0,2));

	Scalar absolutes[3][3];
	absolutes[0][1] = Fabs(b(0,1));
	absolutes[0][2] = Fabs(b(0,2));

	radius =
		axisExtents.y*absolutes[0][2]
		 + axisExtents.z*absolutes[0][1]
		 + box.axisExtents.y*dets[2][0]
		 + box.axisExtents.z*dets[1][0];
	projection = distance.z*b(0,1) - distance.y*b(0,2);
	if (projection > radius || projection < -radius)
		return A0xB0;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A0xB1 or <1,0,0>xB1:
	//
	//  dets   absolutes
	//  ...    .**
	//  *..    ...
	//  *..    ...
	//
	//  projection = (B-A)*(A0xB1) = (B-A)*<0,-B(1,2),B(1,1)>
	//  radius = Ax*|((A0xB1)*A0)| + Ay*|((A0xB1)*A1)| + Az*|((A0xB1)*A2)|
	//           + Bx*|((A0xB1)*B0)| + By*|((A0xB1)*B1)| + Bz*|((A0xB1)*B2)|
	//         = Ay*|(A0xB1)*A1| + Az*|(A0xB1)*A2| + Bx*|(A0xB1)*B0| + Bz*|(A0xB1)*B2|
	//         = Ay*|(<1,0,0>xB1)*A1| + Az*|(<1,0,0>xB1)*A2| + Bx*|(<1,0,0>xB1)*B0| + Bz*|(<1,0,0>xB1)*B2|
	//         = Ay*|<0,-B(1,2),B(1,1)>*<0,1,0>| + Az*|<0,-B(1,2),B(1,1)>*<0,0,1>|
	//           + Bx*|<0,-B(1,2),B(1,1)>*B0| + Bz*|<0,-B(1,2),B(1,1)>*B2|
	//         = Ay*|-B(1,2)| + Az*|B(1,1)| + Bx*|<0,-B(1,2),B(1,1)>*B0| + Bz*|<0,-B(1,2),B(1,1)>*B2|
	//         = Ay*|B(1,2)| + Az*|B(1,1)|
	//           + Bx*|<0,-B(1,2),B(1,1)>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<0,-B(1,2),B(1,1)>*<B(2,0),B(2,1),B(2,2)>|
	//         = Ay*|B(1,2)| + Az*|B(1,1)|
	//           + Bx*|<-B(1,2),B(1,1)>*<B(0,1),B(0,2)>| + Bz*|<-B(1,2),B(1,1)>*<B(2,1),B(2,2)>|
	//         = Ay*|B(1,2)| + Az*|B(1,1)|
	//           + Bx*|B(1,1)*B(0,2) - B(1,2)*B(0,1)| + Bz*|B(1,1)*B(2,2) - B(1,2)*B(2,1)|
	//         = Ay*|B(1,2)| + Az*|B(1,1)| + Bx*dets(2,0) + Bz*dets(0,0)
	//------------------------------------------------------------------------------------------------------------
	//
	dets[0][0] = Fabs(b(1,1)*b(2,2) - b(2,1)*b(1,2));

	absolutes[1][1] = Fabs(b(1,1));
	absolutes[1][2] = Fabs(b(1,2));

	radius =
		axisExtents.y*absolutes[1][2]
		 + axisExtents.z*absolutes[1][1]
		 + box.axisExtents.x*dets[2][0]
		 + box.axisExtents.z*dets[0][0];
	projection = distance.z*b(1,1) - distance.y*b(1,2);
	if (projection > radius || projection < -radius)
		return A0xB1;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A0xB2 or <1,0,0>xB2:
	//
	//  dets   absolutes
	//  *..    .**
	//  *..    .**
	//  *..    ...
	//
	//  projection = (B-A)*(A0xB2) = (B-A)*<0,-B(2,2),B(2,1)>
	//  radius = Ax*|(A0xB2)*A0| + Ay*|(A0xB2)*A1| + Az*|(A0xB2)*A2|
	//           + Bx*|(A0xB2)*B0| + By*|(A0xB2)*B1| + Bz*|(A0xB2)*B2|
	//         = Ay*|(A0xB2)*A1| + Az*|(A0xB2)*A2| + Bx*|(A0xB2)*B0| + By*|(A0xB2)*B1|
	//         = Ay*|<0,-B(2,2),B(2,1)>*A1| + Az*|<0,-B(2,2),B(2,1)>*A2|
	//           + Bx*|<0,-B(2,2),B(2,1)>*B0| + By*|<0,-B(2,2),B(2,1)>*B1|
	//         = Ay*|<0,-B(2,2),B(2,1)>*<0,1,0>| + Az*|<0,-B(2,2),B(2,1)>*<0,0,1>|
	//           + Bx*|<0,-B(2,2),B(2,1)>*<B(0,0),B(0,1),B(0,2)>| + By*|<0,-B(2,2),B(2,1)>*<B(1,0),B(1,1),B(1,2)>|
	//         = Ay*|-B(2,2)| + Az*|B(2,1)|
	//           + Bx*|<-B(2,2),B(2,1)>*<B(0,1),B(0,2)>| + By*|<-B(2,2),B(2,1)>*<B(1,1),B(1,2)>|
	//         = Ay*|B(2,2)| + Az*|B(2,1)|
	//           + Bx*|B(2,1)*B(0,2) - B(2,2)*B(0,1)| + By*|B(2,1)*B(1,2) - B(2,2)*B(1,1)|
	//         = Ay*|B(2,2)| + Az*|B(2,1)| + Bx*dets(1,0) + By*dets(0,0)
	//------------------------------------------------------------------------------------------------------------
	//
	absolutes[2][1] = Fabs(b(2,1));
	absolutes[2][2] = Fabs(b(2,2));

	radius =
		axisExtents.y*absolutes[2][2]
		 + axisExtents.z*absolutes[2][1]
		 + box.axisExtents.x*dets[1][0]
		 + box.axisExtents.y*dets[0][0];
	projection = distance.z*b(2,1) - distance.y*b(2,2);
	if (projection > radius || projection < -radius)
		return A0xB2;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A1xB0 or <0,1,0>xB0:
	//
	//  dets   absolutes
	//  *..    .**
	//  *..    .**
	//  *..    .**
	//
	//  projection = (B-A)*(A1xB0) = (B-A)*<B(0,2),0,-B(0,0)>
	//  radius = Ax*|(A1xB0)*A0| + Ay*|(A1xB0)*A1| + Az*|(A1xB0)*A2|
	//           + Bx*|(A1xB0)*B0| + By*|(A1xB0)*B1| + Bz*|(A1xB0)*B2|
	//         = Ax*|(A1xB0)*A0| + Az*|(A1xB0)*A2| + By*|(A1xB0)*B1| + Bz*|(A1xB0)*B2|
	//         = Ax*|<B(0,2),0,-B(0,0)>*A0| + Az*|<B(0,2),0,-B(0,0)>*A2|
	//           + By*|<B(0,2),0,-B(0,0)>*B1| + Bz*|<B(0,2),0,-B(0,0)>*B2|
	//         = Ax*|<B(0,2),0,-B(0,0)>*<1,0,0>| + Az*|<B(0,2),0,-B(0,0)>*<0,0,1>|
	//           + By*|<B(0,2),0,-B(0,0)>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<B(0,2),0,-B(0,0)>*<B(2,0),B(2,1),B(2,2)>|
	//         = Ax*|B(0,2)| + Az*|-B(0,0)| 
	//           + By*|<B(0,2),-B(0,0)>*<B(1,0),B(1,2)>| + Bz*|<B(0,2),-B(0,0)>*<B(2,0),B(2,2)>|
	//         = Ax*|B(0,2)| + Az*|B(0,0)| 
	//           + By*|B(0,2)*B(1,0) - B(0,0)*B(1,2)| + Bz*|B(0,2)*B(2,0) - B(0,0)*B(2,2)|
	//         = Ax*|B(0,2)| + Az*|B(0,0)| + By*dets(2,1) + Bz*dets(1,1)
	//------------------------------------------------------------------------------------------------------------
	//
	dets[2][1] = Fabs(b(0,2)*b(1,0) - b(0,0)*b(1,2));
	dets[1][1] = Fabs(b(0,0)*b(2,2) - b(2,0)*b(0,2));

	absolutes[0][0] = Fabs(b(0,0));

	radius =
		axisExtents.x*absolutes[0][2]
		 + axisExtents.z*absolutes[0][0]
		 + box.axisExtents.y*dets[2][1]
		 + box.axisExtents.z*dets[1][1];
	projection = distance.x*b(0,2) - distance.z*b(0,0);
	if (projection > radius || projection < -radius)
		return A1xB0;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A1xB1 or <0,1,0>xB1:
	//
	//  dets   absolutes
	//  *..    ***
	//  **.    .**
	//  **.    .**
	//
	//  projection = (B-A)*(A1xB1) = (B-A)*<B(1,2),0,-B(1,0)>
	//  radius = Ax*|(A1xB1)*A0| + Ay*|(A1xB1)*A1| + Az*|(A1xB1)*A2|
	//           + Bx*|(A1xB1)*B0| + By*|(A1xB1)*B1| + Bz*|(A1xB1)*B2|
	//         = Ax*|(A1xB1)*A0| + Az*|(A1xB1)*A2| + Bx*|(A1xB1)*B0| + Bz*|(A1xB1)*B2|
	//         = Ax*|<B(1,2),0,-B(1,0)>*A0| + Az*|<B(1,2),0,-B(1,0)>*A2|
	//           + Bx*|<B(1,2),0,-B(1,0)>*B0| + Bz*|<B(1,2),0,-B(1,0)>*B2|
	//         = Ax*|<B(1,2),0,-B(1,0)>*<1,0,0>| + Az*|<B(1,2),0,-B(1,0)>*<0,0,1>|
	//           + Bx*|<B(1,2),0,-B(1,0)>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<B(1,2),0,-B(1,0)>*B(2,0),B(2,1),B(2,2)|
	//         = Ax*|B(1,2)| + Az*|-B(1,0)|
	//           + Bx*|<B(1,2),-B(1,0)>*<B(0,0),B(0,2)>| + Bz*|<B(1,2),-B(1,0)>*B(2,0),B(2,2)>|
	//         = Ax*|B(1,2)| + Az*|B(1,0)|
	//           + Bx*|B(1,2)*<B(0,0) - B(1,0)*B(0,2)| + Bz*|B(1,2)*B(2,0) - B(1,0)*B(2,2)|
	//         = Ax*|B(1,2)| + Az*|B(1,0)| + Bx*dets(2,1) + Bz*dets(0,1)
	//------------------------------------------------------------------------------------------------------------
	//
	dets[0][1] = Fabs(b(1,2)*b(2,0) - b(1,0)*b(2,2));

	absolutes[1][0] = Fabs(b(1,0));

	radius =
		axisExtents.x*absolutes[1][2]
		 + axisExtents.z*absolutes[1][0]
		 + box.axisExtents.x*dets[2][1]
		 + box.axisExtents.z*dets[0][1];
	projection = distance.x*b(1,2) - distance.z*b(1,0);
	if (projection > radius || projection < -radius)
		return A1xB1;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A1xB2 or <0,1,0>xB2:
	//
	//  dets   absolutes
	//  **.    ***
	//  **.    ***
	//  **.    .**
	//
	//  projection = (B-A)*(A1xB2) = (B-A)*<B(2,2),0,-B(2,0)>
	//  radius = Ax*|(A1xB2)*A0| + Ay*|(A1xB2)*A1| + Az*|(A1xB2)*A2|
	//           + Bx*|(A1xB2)*B0| + By*|(A1xB2)*B1| + Bz*|(A1xB2)*B2|
	//         = Ax*|(A1xB2)*A0| + Az*|(A1xB2)*A2| + Bx*|(A1xB2)*B0| + By*|(A1xB2)*B1|
	//         = Ax*|<B(2,2),0,-B(2,0)>*A0| + Az*|<B(2,2),0,-B(2,0)>*A2|
	//           + Bx*|<B(2,2),0,-B(2,0)>*B0| + By*|<B(2,2),0,-B(2,0)>*B1|
	//         = Ax*|<B(2,2),0,-B(2,0)>*<1,0,0>| + Az*|<B(2,2),0,-B(2,0)>*<0,0,1>|
	//           + Bx*|<B(2,2),0,-B(2,0)>*<B(0,0),B(0,1),B(0,2)>| + By*|<B(2,2),0,-B(2,0)>*<B(1,0),B(1,1),B(1,2)>|
	//         = Ax*|B(2,2)| + Az*|-B(2,0)|
	//           + Bx*|<B(2,2),-B(2,0)>*<B(0,0),B(0,2)>| + By*|<B(2,2),-B(2,0)>*<B(1,0),B(1,2)>|
	//         = Ax*|B(2,2)| + Az*|B(2,0)|
	//           + Bx*|B(2,2)*B(0,0) -B(2,0)*B(0,2)| + By*|B(2,2)*B(1,0) - B(2,0)*B(1,2)|
	//         = Ax*|B(2,2)| + Az*|B(2,0)| + Bx*dets(1,1) + By*dets(0,1)
	//------------------------------------------------------------------------------------------------------------
	//
	absolutes[2][0] = Fabs(b(2,0));

	radius =
		axisExtents.x*absolutes[2][2]
		 + axisExtents.z*absolutes[2][0]
		 + box.axisExtents.x*dets[1][1]
		 + box.axisExtents.y*dets[0][1];
	projection = distance.x*b(2,2) - distance.z*b(2,0);
	if (projection > radius || projection < -radius)
		return A1xB2;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A2xB0 or <0,0,1>xB0:
	//
	//  dets   absolutes
	//  **.    ***
	//  **.    ***
	//  **.    ***
	//
	//  projection = (B-A)*(A2xB0) = (B-A)*<-B(0,1),B(0,0),0>
	//  radius = Ax*|(A2xB0)*A0| + Ay*|(A2xB0)*A1| + Az*|(A2xB0)*A2|
	//           + Bx*|(A2xB0)*B0| + By*|(A2xB0)*B1| + Bz*|(A2xB0)*B2|
	//         = Ax*|(A2xB0)*A0| + Ay*|(A2xB0)*A1| + By*|(A2xB0)*B1| + Bz*|(A2xB0)*B2|
	//         = Ax*|<-B(0,1),B(0,0),0>*A0| + Ay*|<-B(0,1),B(0,0),0>*A1|
	//           + By*|<-B(0,1),B(0,0),0>*B1| + Bz*|<-B(0,1),B(0,0),0>*B2|
	//         = Ax*|<-B(0,1),B(0,0),0>*<1,0,0>| + Ay*|<-B(0,1),B(0,0),0>*<0,1,0>|
	//           + By*|<-B(0,1),B(0,0),0>*<B(1,0),B(1,1),B(1,2)>| + Bz*|<-B(0,1),B(0,0),0>*<B(2,0),B(2,1),B(2,2)>|
	//         = Ax*|-B(0,1)| + Ay*|B(0,0)|
	//           + By*|<-B(0,1),B(0,0)>*<B(1,0),B(1,1)>| + Bz*|<-B(0,1),B(0,0)>*<B(2,0),B(2,1)>|
	//         = Ax*|B(0,1)| + Ay*|B(0,0)|
	//           + By*|B(0,0)*B(1,1) -B(0,1)*B(1,0)| + Bz*|B(0,0)*B(2,1) - B(0,1)*B(2,0)|
	//         = Ax*|B(0,1)| + Ay*|B(0,0)| + By*dets(2,2) + Bz*dets(1,2)
	//------------------------------------------------------------------------------------------------------------
	//
	dets[2][2] = Fabs(b(1,1)*b(0,0) - b(0,1)*b(1,0));
	dets[1][2] = Fabs(b(2,1)*b(0,0) - b(2,0)*b(0,1));

	radius =
		axisExtents.x*absolutes[0][1]
		 + axisExtents.y*absolutes[0][0]
		 + box.axisExtents.y*dets[2][2]
		 + box.axisExtents.z*dets[1][2];
	projection = distance.y*b(0,0) - distance.x*b(0,1);
	if (projection > radius || projection < -radius)
		return A2xB0;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A2xB1 or <0,0,1>xB1:
	//
	//  dets   absolutes
	//  **.    ***
	//  ***    ***
	//  ***    ***
	//
	//  projection = (B-A)*(A2xB1) = (B-A)*<-B(1,1),B(1,0),0>
	//  radius = Ax*|(A2xB1)*A0| + Ay*|(A2xB1)*A1| + Az*|(A2xB1)*A2|
	//           + Bx*|(A2xB1)*B0| + By*|(A2xB1)*B1| + Bz*|(A2xB1)*B2|
	//         = Ax*|(A2xB1)*A0| + Ay*|(A2xB1)*A1| + Bx*|(A2xB1)*B0| + Bz*|(A2xB1)*B2|
	//         = Ax*|<-B(1,1),B(1,0),0>*A0| + Ay*|<-B(1,1),B(1,0),0>*A1|
	//           + Bx*|<-B(1,1),B(1,0),0>*B0| + Bz*|<-B(1,1),B(1,0),0>*B2|
	//         = Ax*|<-B(1,1),B(1,0),0>*<1,0,0>| + Ay*|<-B(1,1),B(1,0),0>*<0,1,0>|
	//           + Bx*|<-B(1,1),B(1,0),0>*<B(0,0),B(0,1),B(0,2)>| + Bz*|<-B(1,1),B(1,0),0>*<B(2,0),B(2,1),B(2,2)>|
	//         = Ax*|-B(1,1)| + Ay*|B(1,0)|
	//           + Bx*|<-B(1,1),B(1,0)>*<B(0,0),B(0,1)>| + Bz*|<-B(1,1),B(1,0)>*<B(2,0),B(2,1)>|
	//         = Ax*|B(1,1)| + Ay*|B(1,0)|
	//           + Bx*|B(1,0)*B(0,1) - B(1,1)*B(0,0)| + Bz*|B(1,0)*B(2,1) - B(1,1)*B(2,0)|
	//         = Ax*|B(1,1)| + Ay*|B(1,0)| + Bx*dets(2,2) + Bz*dets(0,2)
	//------------------------------------------------------------------------------------------------------------
	//
	dets[0][2] = Fabs(b(1,0)*b(2,1) - b(2,0)*b(1,1));

	radius =
		axisExtents.x*absolutes[1][1]
		 + axisExtents.y*absolutes[1][0]
		 + box.axisExtents.x*dets[2][2]
		 + box.axisExtents.z*dets[0][2];
	projection = distance.y*b(1,0) - distance.x*b(1,1);
	if (projection > radius || projection < -radius)
		return A2xB1;

	//
	//------------------------------------------------------------------------------------------------------------
	// The math for L = A2xB2 or <0,0,1>xB2:
	//
	//  dets   absolutes
	//  ***    ***
	//  ***    ***
	//  ***    ***
	//
	//  projection = (B-A)*(A2xB2) = (B-A)*<-B(2,1),B(2,0),0>
	//  radius = Ax*|(A2xB2)*A0| + Ay*|(A2xB2)*A1| + Az*|(A2xB2)*A2|
	//           + Bx*|(A2xB2)*B0| + By*|(A2xB2)*B1| + Bz*|(A2xB2)*B2|
	//         = Ax*|(A2xB2)*A0| + Ay*|(A2xB2)*A1| + Bx*|(A2xB2)*B0| + By*|(A2xB2)*B1|
	//         = Ax*|<-B(2,1),B(2,0),0>*A0| + Ay*|<-B(2,1),B(2,0),0>*A1|
	//           + Bx*|<-B(2,1),B(2,0),0>*B0| + By*|<-B(2,1),B(2,0),0>*B1|
	//         = Ax*|<-B(2,1),B(2,0),0>*<1,0,0>| + Ay*|<-B(2,1),B(2,0),0>*<0,1,0>|
	//           + Bx*|<-B(2,1),B(2,0),0>*<B(0,0),B(0,1),B(0,2)>| + By*|<-B(2,1),B(2,0),0>*<B(1,0),B(1,1),B(1,2)>|
	//         = Ax*|-B(2,1)| + Ay*|B(2,0)|
	//           + Bx*|<-B(2,1),B(2,0)>*<B(0,0),B(0,1)>| + By*|<-B(2,1),B(2,0)>*<B(1,0),B(1,1)>|
	//         = Ax*|B(2,1)| + Ay*|B(2,0)|
	//           + Bx*|B(2,0)*B(0,1) - B(2,1)*B(0,0)| + By*|B(2,0)*B(1,1) - B(2,1)*B(1,0)|
	//         = Ax*|B(2,1)| + Ay*|B(2,0)| + Bx*dets(1,2) + By*dets(0,2)
	//------------------------------------------------------------------------------------------------------------
	//
	radius =
		axisExtents.x*absolutes[2][1]
		 + axisExtents.y*absolutes[2][0]
		 + box.axisExtents.x*dets[1][2]
		 + box.axisExtents.y*dets[0][2];
	projection = distance.y*b(2,0) - distance.x*b(2,1);
	if (projection > radius || projection < -radius)
		return A2xB2;

	return NoSeparation;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
bool OBB::Contains(const Point3D &point) const
{
	Check_Pointer(this);
	Check_Object(&point);

	//
	//-------------------------------------
	// Translate the point into local space
	//-------------------------------------
	//
	Point3D local;
	local.MultiplyByInverse(point, localToParent);

	//
	//--------------------------
	// Compare it to the extents
	//--------------------------
	//
	return Fabs(local.x)<=axisExtents.x && Fabs(local.y)<=axisExtents.y && Fabs(local.z)<=axisExtents.z;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void OBB::Union(
	const OBB &first,
	const OBB &second
)
{
	Check_Pointer(this);
	Check_Object(&first);
	Check_Object(&second);

	//
	//-------------------------------------------------
	// Learn about the spheres surrounding the two OBBs
	//-------------------------------------------------
	//
	Point3D c1(first.localToParent);
	Point3D c2(second.localToParent);
	Vector3D diff;
	diff.Subtract(c2, c1);
	Scalar len = diff.GetLength();

	//
	//---------------------------------------------------
	// See if the first sphere is contained in the second
	//---------------------------------------------------
	//
	if (len+first.sphereRadius <= second.sphereRadius || !first.sphereRadius)
	{
		if (this != &second)
			*this = second;
		return;
	}

	//
	//---------------------------------------------------
	// See if the second sphere is contained in the first
	//---------------------------------------------------
	//
	if (len+second.sphereRadius <= first.sphereRadius || !second.sphereRadius)
	{
		if (this != &first)
			*this = first;
		return;
	}

	//
	//-------------------------------------------------------
	// The new sphere will lie somewhere between the old ones
	//-------------------------------------------------------
	//
	localToParent = LinearMatrix4D::Identity;
	axisExtents = Vector3D::Identity;
	c1.Lerp(c1, c2, (len + second.sphereRadius - first.sphereRadius) / (2.0f*len));
	localToParent.BuildTranslation(c1);
	sphereRadius = 0.5f * (len + first.sphereRadius + second.sphereRadius);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
OBB::SeparatingAxis OBB::CalculateSeparationVector(
	Vector3D *vector,
	const OBB &box
)
{
	Check_Object(this);
	Check_Pointer(vector);
	Check_Object(&box);

	//
	//-----------------------------------------------------------------------
	// We use the same logic as in detecting separation, but since we know
	// the boxes are already intersecting, we will look to see which axis
	// separates the boxes by the least space, then we will nudge this box by
	// that amount.  We will initially only concern ourselves with the axes
	// based on the face normals
	//-----------------------------------------------------------------------
	//
	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.  Both boxes are assumed to
	// convert their local axes into a common parent space
	//----------------------------------------------------------------------
	//
	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)
		);

	//
	//--------------------------------------------------------------------------------------------------
	// The rows of the matrix encode the local axes in world space.  The first
	// separation axis we will be testing our x axis in world space, so
	// project the other box's primary axes onto it, and store the results
	// away for future reference
	//
	// The math for L = A0: (A0 is the X axis row of our matrix)
	//  Given: A0,A1, and B2 are orthogonal
	//         B0,B1, and B2 are orthogonal
	//         Ax, Ay, and Az >= 0
	//         Bx, By, and Bz >= 0
	//
	//  projection = (B-A)*A0
	//  radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
	//         = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
	//--------------------------------------------------------------------------------------------------
	//
	Scalar axis_projections[3][3];
	axis_projections[X_Axis][X_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[X_Axis][Y_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[X_Axis][Z_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
		);

	//
	//-----------------------------------------------------------------------
	// The maximum distance the centerpoints can be away and still touch is
	// equal to the sum of our x extent plus the other's extents, as modified
	// by the projection values.  If the boxes can't touch, return A0 as the
	// separating plane
	//-----------------------------------------------------------------------
	//
	Scalar radius =
		axisExtents.x
		 + box.axisExtents.x * axis_projections[X_Axis][0]
		 + box.axisExtents.y * axis_projections[X_Axis][1]
		 + box.axisExtents.z * axis_projections[X_Axis][2];
	Scalar projection =
		localToParent(X_Axis,0) * distance.x
		 + localToParent(X_Axis,1) * distance.y
		 + localToParent(X_Axis,2) * distance.z;

	radius -= Fabs(projection);
	Scalar best_radius = radius;
	radius += SMALL;
	Verify(radius > 0.0f);
	if (projection<0.0f)
		radius = -radius;
	vector->x = localToParent(0,0) * radius;
	vector->y = localToParent(0,1) * radius;
	vector->z = localToParent(0,2) * radius;
	SeparatingAxis axis = A0;

	//
	//--------------------------------------------------------------------------------------------------
	// Do the same check against our Y axis
	//
	// The math for L = A1:
	//
	//  projection = (B-A)*A1
	//  radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
	//         = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Y_Axis][X_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Y_Axis][Y_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Y_Axis][Z_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.y
		 + box.axisExtents.x * axis_projections[1][0]
		 + box.axisExtents.y * axis_projections[1][1]
		 + box.axisExtents.z * axis_projections[1][2];
	projection =
		localToParent(1,0) * distance.x
		 + localToParent(1,1) * distance.y
		 + localToParent(1,2) * distance.z;

	radius -= Fabs(projection);
	if (radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		Verify(radius > 0.0f);
		if (projection<0.0f)
			radius = -radius;
		vector->x = localToParent(1,0) * radius;
		vector->y = localToParent(1,1) * radius;
		vector->z = localToParent(1,2) * radius;
		axis = A1;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// Check our z axis
	//
	// The math for L = A2:
	//
	//  projection = (B-A)*A2
	//  radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
	//         = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Z_Axis][X_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Z_Axis][Y_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Z_Axis][Z_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.z
		 + box.axisExtents.x * axis_projections[2][0]
		 + box.axisExtents.y * axis_projections[2][1]
		 + box.axisExtents.z * axis_projections[2][2];
	projection =
		localToParent(2,0) * distance.x
		 + localToParent(2,1) * distance.y
		 + localToParent(2,2) * distance.z;

	radius -= Fabs(projection);
	if (radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		Verify(radius > 0.0f);
		if (projection<0.0f)
			radius = -radius;
		vector->x = localToParent(2,0) * radius;
		vector->y = localToParent(2,1) * radius;
		vector->z = localToParent(2,2) * radius;
		axis = A2;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// See if the other box's X axis is a separator
	//
	// The math for L = B0:
	//
	//  projection = (B-A)*B0
	//  radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
	//         = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.x 
		 + axisExtents.x * axis_projections[0][X_Axis]
		 + axisExtents.y * axis_projections[1][X_Axis]
		 + axisExtents.z * axis_projections[2][X_Axis];
	projection =
		box.localToParent(X_Axis,0) * distance.x
		 + box.localToParent(X_Axis,1) * distance.y
		 + box.localToParent(X_Axis,2) * distance.z;

	radius -= Fabs(projection);
	if (radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		Verify(radius > 0.0f);
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(0,0) * radius;
		vector->y = box.localToParent(0,1) * radius;
		vector->z = box.localToParent(0,2) * radius;
		axis = B0;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Y axis
	//
	// The math for L = B1:
	//
	//  projection = (B-A)*B1
	//  radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
	//         = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.y
		 + axisExtents.x * axis_projections[0][Y_Axis]
		 + axisExtents.y * axis_projections[1][Y_Axis]
		 + axisExtents.z * axis_projections[2][Y_Axis];
	projection =
		box.localToParent(Y_Axis,0) * distance.x
		 + box.localToParent(Y_Axis,1) * distance.y
		 + box.localToParent(Y_Axis,2) * distance.z;

	radius -= Fabs(projection);
	if (radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		Verify(radius > 0.0f);
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(1,0) * radius;
		vector->y = box.localToParent(1,1) * radius;
		vector->z = box.localToParent(1,2) * radius;
		axis = B1;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Z axis
	//
	// The math for L = B2:
	//
	//  projection = (B-A)*B2
	//  radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
	//         = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.z
		 + axisExtents.x * axis_projections[0][Z_Axis]
		 + axisExtents.y * axis_projections[1][Z_Axis]
		 + axisExtents.z * axis_projections[2][Z_Axis];
	projection =
		box.localToParent(Z_Axis,0) * distance.x
		 + box.localToParent(Z_Axis,1) * distance.y
		 + box.localToParent(Z_Axis,2) * distance.z;

	radius -= Fabs(projection);
	if (radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		Verify(radius > 0.0f);
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(2,0) * radius;
		vector->y = box.localToParent(2,1) * radius;
		vector->z = box.localToParent(2,2) * radius;
		axis = B2;
	}
	
	return axis;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void OBB::CalculateSecondarySeparationVector(
	Vector3D *vector,
	const OBB &box,
	const UnitVector3D &primary
)
{
	Check_Object(this);
	Check_Pointer(vector);
	Check_Object(&box);

	//
	//-----------------------------------------------------------------------
	// We use the same logic as in detecting separation, but since we know
	// the boxes are already intersecting, we will look to see which axis
	// separates the boxes by the least space, then we will nudge this box by
	// that amount.  We will initially only concern ourselves with the axes
	// based on the face normals
	//-----------------------------------------------------------------------
	//
	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.  Both boxes are assumed to
	// convert their local axes into a common parent space
	//----------------------------------------------------------------------
	//
	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)
		);

	//
	//--------------------------------------------------------------------------------------------------
	// The rows of the matrix encode the local axes in world space.  The first
	// separation axis we will be testing our x axis in world space, so
	// project the other box's primary axes onto it, and store the results
	// away for future reference
	//
	// The math for L = A0: (A0 is the X axis row of our matrix)
	//  Given: A0,A1, and B2 are orthogonal
	//         B0,B1, and B2 are orthogonal
	//         Ax, Ay, and Az >= 0
	//         Bx, By, and Bz >= 0
	//
	//  projection = (B-A)*A0
	//  radius = |Ax*(A0*A0)| + |Ay*(A0*A1)| + |Az*(A0*A2)| + |Bx*(A0*B0)| + |By*(A0*B1)| + |Bz*(A0*B2)|
	//         = Ax + Bx*|A0*B0| + By*|A0*B1| + Bz*|A0*B2|
	//--------------------------------------------------------------------------------------------------
	//
	Scalar axis_projections[3][3];
	axis_projections[X_Axis][X_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[X_Axis][Y_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[X_Axis][Z_Axis] =
		Fabs(
			localToParent(X_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(X_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(X_Axis,2) * box.localToParent(Z_Axis,2)
		);

	//
	//-----------------------------------------------------------------------
	// The maximum distance the centerpoints can be away and still touch is
	// equal to the sum of our x extent plus the other's extents, as modified
	// by the projection values.  If the boxes can't touch, return A0 as the
	// separating plane
	//-----------------------------------------------------------------------
	//
	Scalar radius =
		axisExtents.x
		 + box.axisExtents.x * axis_projections[X_Axis][0]
		 + box.axisExtents.y * axis_projections[X_Axis][1]
		 + box.axisExtents.z * axis_projections[X_Axis][2];
	Scalar projection =
		localToParent(X_Axis,0) * distance.x
		 + localToParent(X_Axis,1) * distance.y
		 + localToParent(X_Axis,2) * distance.z;
	Scalar primary_dot = 
		Fabs(localToParent(X_Axis,0) * primary.x
		 + localToParent(X_Axis,1) * primary.y
		 + localToParent(X_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	Scalar best_radius = radius;
	radius += SMALL;
	if (projection<0.0f)
		radius = -radius;
	vector->x = localToParent(0,0) * radius;
	vector->y = localToParent(0,1) * radius;
	vector->z = localToParent(0,2) * radius;

	//
	//--------------------------------------------------------------------------------------------------
	// Do the same check against our Y axis
	//
	// The math for L = A1:
	//
	//  projection = (B-A)*A1
	//  radius = |Ax*(A1*A0)| + |Ay*(A1*A1)| + |Az*(A1*A2)| + |Bx*(A1*B0)| + |By*(A1*B1)| + |Bz*(A1*B2)|
	//         = Ay + Bx*|A1*B0| + By*|A1*B1| + Bz*|A1*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Y_Axis][X_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Y_Axis][Y_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Y_Axis][Z_Axis] =
		Fabs(
			localToParent(Y_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Y_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Y_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.y
		 + box.axisExtents.x * axis_projections[1][0]
		 + box.axisExtents.y * axis_projections[1][1]
		 + box.axisExtents.z * axis_projections[1][2];
	projection =
		localToParent(Y_Axis,0) * distance.x
		 + localToParent(Y_Axis,1) * distance.y
		 + localToParent(Y_Axis,2) * distance.z;
	Scalar dot = 
		Fabs(localToParent(Y_Axis,0) * primary.x
		 + localToParent(Y_Axis,1) * primary.y
		 + localToParent(Y_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	if (dot<0.85f && (radius < best_radius || primary_dot<0.85f))
	{
		best_radius = radius;
		radius += SMALL;
		if (projection<0.0f)
			radius = -radius;
		vector->x = localToParent(1,0) * radius;
		vector->y = localToParent(1,1) * radius;
		vector->z = localToParent(1,2) * radius;
	}
	else
		Verify(primary_dot < 0.85f);

	//
	//--------------------------------------------------------------------------------------------------
	// Check our z axis
	//
	// The math for L = A2:
	//
	//  projection = (B-A)*A2
	//  radius = |Ax*(A2*A0)| + |Ay*(A2*A1)| + |Az*(A2*A2)| + |Bx*(A2*B0)| + |By*(A2*B1)| + |Bz*(A2*B2)|
	//         = Az + Bx*|A2*B0| + By*|A2*B1| + Bz*|A2*B2|
	//--------------------------------------------------------------------------------------------------
	//
	axis_projections[Z_Axis][X_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(X_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(X_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(X_Axis,2)
		);
	axis_projections[Z_Axis][Y_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Y_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Y_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Y_Axis,2)
		);
	axis_projections[Z_Axis][Z_Axis] =
		Fabs(
			localToParent(Z_Axis,0) * box.localToParent(Z_Axis,0)
			 + localToParent(Z_Axis,1) * box.localToParent(Z_Axis,1)
			 + localToParent(Z_Axis,2) * box.localToParent(Z_Axis,2)
		);
	radius =
		axisExtents.z
		 + box.axisExtents.x * axis_projections[2][0]
		 + box.axisExtents.y * axis_projections[2][1]
		 + box.axisExtents.z * axis_projections[2][2];
	projection =
		localToParent(2,0) * distance.x
		 + localToParent(2,1) * distance.y
		 + localToParent(2,2) * distance.z;
	dot = 
		Fabs(localToParent(Z_Axis,0) * primary.x
		 + localToParent(Z_Axis,1) * primary.y
		 + localToParent(Z_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	if (dot<0.85f && radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		if (projection<0.0f)
			radius = -radius;
		vector->x = localToParent(2,0) * radius;
		vector->y = localToParent(2,1) * radius;
		vector->z = localToParent(2,2) * radius;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// See if the other box's X axis is a separator
	//
	// The math for L = B0:
	//
	//  projection = (B-A)*B0
	//  radius = |Ax*(B0*A0)| + |Ay*(B0*A1)| + |Az*(B0*A2)| + |Bx*(B0*B0)| + |By*(B0*B1)| + |Bz*(B0*B2)|
	//         = Bx + Ax*|B0*A0| + Ay*|B0*A1| + Az*|B0*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.x 
		 + axisExtents.x * axis_projections[0][X_Axis]
		 + axisExtents.y * axis_projections[1][X_Axis]
		 + axisExtents.z * axis_projections[2][X_Axis];
	projection =
		box.localToParent(X_Axis,0) * distance.x
		 + box.localToParent(X_Axis,1) * distance.y
		 + box.localToParent(X_Axis,2) * distance.z;
	dot = 
		Fabs(box.localToParent(X_Axis,0) * primary.x
		 + box.localToParent(X_Axis,1) * primary.y
		 + box.localToParent(X_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	if (dot<0.85f && radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(0,0) * radius;
		vector->y = box.localToParent(0,1) * radius;
		vector->z = box.localToParent(0,2) * radius;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Y axis
	//
	// The math for L = B1:
	//
	//  projection = (B-A)*B1
	//  radius = |Ax*(B1*A0)| + |Ay*(B1*A1)| + |Az*(B1*A2)| + |Bx*(B1*B0)| + |By*(B1*B1)| + |Bz*(B1*B2)|
	//         = By + Ax*|B1*A0| + Ay*|B1*A1| + Az*|B1*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.y
		 + axisExtents.x * axis_projections[0][Y_Axis]
		 + axisExtents.y * axis_projections[1][Y_Axis]
		 + axisExtents.z * axis_projections[2][Y_Axis];
	projection =
		box.localToParent(Y_Axis,0) * distance.x
		 + box.localToParent(Y_Axis,1) * distance.y
		 + box.localToParent(Y_Axis,2) * distance.z;
	dot = 
		Fabs(box.localToParent(Y_Axis,0) * primary.x
		 + box.localToParent(Y_Axis,1) * primary.y
		 + box.localToParent(Y_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	if (dot<0.85f && radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(1,0) * radius;
		vector->y = box.localToParent(1,1) * radius;
		vector->z = box.localToParent(1,2) * radius;
	}

	//
	//--------------------------------------------------------------------------------------------------
	// Check its Z axis
	//
	// The math for L = B2:
	//
	//  projection = (B-A)*B2
	//  radius = |Ax*(B2*A0)| + |Ay*(B2*A1)| + |Az*(B2*A2)| + |Bx*(B2*B0)| + |By*(B2*B1)| + |Bz*(B2*B2)|
	//         = Bz + Ax*|B2*A0| + Ay*|B2*A1| + Az*|B2*A2|
	//--------------------------------------------------------------------------------------------------
	//
	radius =
		box.axisExtents.z
		 + axisExtents.x * axis_projections[0][Z_Axis]
		 + axisExtents.y * axis_projections[1][Z_Axis]
		 + axisExtents.z * axis_projections[2][Z_Axis];
	projection =
		box.localToParent(Z_Axis,0) * distance.x
		 + box.localToParent(Z_Axis,1) * distance.y
		 + box.localToParent(Z_Axis,2) * distance.z;
	dot = 
		Fabs(box.localToParent(Z_Axis,0) * primary.x
		 + box.localToParent(Z_Axis,1) * primary.y
		 + box.localToParent(Z_Axis,2) * primary.z);

	radius -= Fabs(projection);
	Verify(radius > 0.0f);
	if (dot<0.85f && radius < best_radius)
	{
		best_radius = radius;
		radius += SMALL;
		if (projection<0.0f)
			radius = -radius;
		vector->x = box.localToParent(2,0) * radius;
		vector->y = box.localToParent(2,1) * radius;
		vector->z = box.localToParent(2,2) * radius;
	}
}