#include "stdafx.h"

bool gUseInternalObject = true;
int gExpectedNbTests = 0;
int gActualNbTests = 0;

#ifdef TEST_INTERNAL_OBJECTS

__forceinline void BoxSupport(const float extents[3], const float sv[3], float p[3])
{
	// This version is ~11.000 cycles (4%) faster overall in one of the tests.
	IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
	IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
	IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
//	p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
//	p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
//	p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
}

__forceinline bool TestInternalObjects(const btVector3& delta_c, const btVector3& axis, const MyConvex& convex0, const MyConvex& convex1, float dmin)
{
	const float dp = delta_c.dot(axis);

	btVector3 localAxis0;
	InverseTransformPoint3x3(localAxis0, axis, convex0.mTransform);
	btVector3 localAxis1;
	InverseTransformPoint3x3(localAxis1, axis, convex1.mTransform);

	float p0[3];
	BoxSupport(convex0.mExtents, localAxis0, p0);
	float p1[3];
	BoxSupport(convex1.mExtents, localAxis1, p1);

	const float Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
	const float Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();

	const float MinRadius = Radius0>convex0.mRadius ? Radius0 : convex0.mRadius;
	const float MaxRadius = Radius1>convex1.mRadius ? Radius1 : convex1.mRadius;

	const float MinMaxRadius = MaxRadius + MinRadius;
	const float d0 = MinMaxRadius + dp;
	const float d1 = MinMaxRadius - dp;

	const float depth = d0<d1 ? d0:d1;
	if(depth>dmin)
		return false;
	return true;
}
#endif









static bool TestSepAxis(const btVector3& sep_axis, const MyConvex& hull0, const MyConvex& hull1, float& depth)
{
	float Min0,Max0;
	hull0.Project(sep_axis, Min0, Max0);

	float Min1,Max1;
	hull1.Project(sep_axis, Min1, Max1);

	if(Max0<Min1 || Max1<Min0)
		return false;

	const float d0 = Max0 - Min1;
	assert(d0>=0.0f);
	const float d1 = Max1 - Min0;
	assert(d1>=0.0f);
	depth = d0<d1 ? d0:d1;
	return true;
}

bool ReferenceAlgo(ConvexResults& results, const MyConvex& hull0, const MyConvex& hull1)
{
#ifdef TEST_INTERNAL_OBJECTS
	const btVector3 c0 = hull0.mTransform * hull0.mLocalCenter;
	const btVector3 c1 = hull1.mTransform * hull1.mLocalCenter;
	const btVector3 DeltaC2 = c0 - c1;
#endif

	float dmin = FLT_MAX;
	btVector3 sep;

	// Test normals from hull0
	for(int i=0;i<hull0.mNbPolys;i++)
	{
		const btVector3 Normal(hull0.mPolys[i].mPlane[0], hull0.mPolys[i].mPlane[1], hull0.mPolys[i].mPlane[2]);

		const btVector3 WorldNormal = hull0.mTransform.getBasis() * Normal;

#ifdef TEST_INTERNAL_OBJECTS
		gExpectedNbTests++;
		if(gUseInternalObject && !TestInternalObjects(DeltaC2, WorldNormal, hull0, hull1, dmin))
			continue;
		gActualNbTests++;
#endif

		float d;
		if(!TestSepAxis(WorldNormal, hull0, hull1, d))
			return false;

		if(d<dmin)
		{
			dmin = d;
			sep = WorldNormal;
		}
	}

	// Test normals from hull1
	for(int i=0;i<hull1.mNbPolys;i++)
	{
		const btVector3 Normal(hull1.mPolys[i].mPlane[0], hull1.mPolys[i].mPlane[1], hull1.mPolys[i].mPlane[2]);

		const btVector3 WorldNormal = hull1.mTransform.getBasis() * Normal;

#ifdef TEST_INTERNAL_OBJECTS
		gExpectedNbTests++;
		if(gUseInternalObject && !TestInternalObjects(DeltaC2, WorldNormal, hull0, hull1, dmin))
			continue;
		gActualNbTests++;
#endif

		float d;
		if(!TestSepAxis(WorldNormal, hull0, hull1, d))
			return false;

		if(d<dmin)
		{
			dmin = d;
			sep = WorldNormal;
		}
	}

	// Test edges
	for(int j=0;j<hull0.mNbPolys;j++)
	{
		const MyPoly& poly0 = hull0.mPolys[j];

		for(int i=0;i<hull1.mNbPolys;i++)
		{
			const MyPoly& poly1 = hull1.mPolys[i];

			for(int e0=0;e0<poly0.mNbVerts;e0++)
			{
				const btVector3& a = hull0.mVerts[poly0.mIndices[e0]];
				const btVector3& b = hull0.mVerts[poly0.mIndices[(e0+1)%poly0.mNbVerts]];

				const btVector3 edge0 = a - b;
				const btVector3 WorldEdge0 = hull0.mTransform.getBasis() * edge0;

				for(int e1=0;e1<poly1.mNbVerts;e1++)
				{
					const btVector3& c = hull1.mVerts[poly1.mIndices[e1]];
					const btVector3& d = hull1.mVerts[poly1.mIndices[(e1+1)%poly1.mNbVerts]];

					const btVector3 edge1 = c - d;
					const btVector3 WorldEdge1 = hull1.mTransform.getBasis() * edge1;

					btVector3 Cross = WorldEdge0.cross(WorldEdge1);
					if(!IsAlmostZero(Cross))
					{
						Cross = Cross.normalize();

#ifdef TEST_INTERNAL_OBJECTS
						gExpectedNbTests++;
						if(gUseInternalObject && !TestInternalObjects(DeltaC2, Cross, hull0, hull1, dmin))
							continue;
						gActualNbTests++;
#endif

						float d;
						if(!TestSepAxis(Cross, hull0, hull1, d))
							return false;

						if(d<dmin)
						{
							dmin = d;
							sep = Cross;
						}
					}
				}
			}
		}
	}

	const btVector3 DeltaC = hull1.mTransform.getOrigin() - hull0.mTransform.getOrigin();
	if((DeltaC.dot(sep))>0.0f)
		sep = -sep;

	results.mDepth	= -dmin;
	results.mNormal	= sep;
	results.mPoint.setValue(0, 0, 0);
	return true;
}
