/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/


///This file was written by Erwin Coumans
///Separating axis rest based on work from Pierre Terdiman, see
///And contact clipping based on work from Simon Hobbs


#include "btPolyhedralContactClipping.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"

#include <float.h> //for FLT_MAX

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

// Clips a face to the back of a plane
void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
{
	
	int ve;
	btScalar ds, de;
	int numVerts = pVtxIn.size();
	if (numVerts < 2)
		return;

	btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
	btVector3 endVertex = pVtxIn[0];
	
	ds = planeNormalWS.dot(firstVertex)+planeEqWS;

	for (ve = 0; ve < numVerts; ve++)
	{
		endVertex=pVtxIn[ve];

		de = planeNormalWS.dot(endVertex)+planeEqWS;

		if (ds<0)
		{
			if (de<0)
			{
				// Start < 0, end < 0, so output endVertex
				ppVtxOut.push_back(endVertex);
			}
			else
			{
				// Start < 0, end >= 0, so output intersection
				ppVtxOut.push_back( 	firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
			}
		}
		else
		{
			if (de<0)
			{
				// Start >= 0, end < 0 so output intersection and end
				ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
				ppVtxOut.push_back(endVertex);
			}
		}
		firstVertex = endVertex;
		ds = de;
	}
}


static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
{
	btScalar Min0,Max0;
	btScalar Min1,Max1;
	btVector3 witnesPtMinA,witnesPtMaxA;
	btVector3 witnesPtMinB,witnesPtMaxB;

	hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
	hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);

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

	btScalar d0 = Max0 - Min1;
	btAssert(d0>=0.0f);
	btScalar d1 = Max1 - Min0;
	btAssert(d1>=0.0f);
	if (d0<d1)
	{
		depth = d0;
		witnessPointA = witnesPtMaxA;
		witnessPointB = witnesPtMinB;

	} else
	{
		depth = d1;
		witnessPointA = witnesPtMinA;
		witnessPointB = witnesPtMaxB;
	}
	
	return true;
}



static int gActualSATPairTests=0;

inline bool IsAlmostZero(const btVector3& v)
{
	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
	return true;
}

#ifdef TEST_INTERNAL_OBJECTS

inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar 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];
}

void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
	const btMatrix3x3& rot = tr.getBasis();
	const btVector3& r0 = rot[0];
	const btVector3& r1 = rot[1];
	const btVector3& r2 = rot[2];

	const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
	const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
	const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();

	out.setValue(x, y, z);
}

 bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
{
	const btScalar dp = delta_c.dot(axis);

	btVector3 localAxis0;
	InverseTransformPoint3x3(localAxis0, axis,trans0);
	btVector3 localAxis1;
	InverseTransformPoint3x3(localAxis1, axis,trans1);

	btScalar p0[3];
	BoxSupport(convex0.m_extents, localAxis0, p0);
	btScalar p1[3];
	BoxSupport(convex1.m_extents, localAxis1, p1);

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

	const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
	const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;

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

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

 
 
 SIMD_FORCE_INLINE void btSegmentsClosestPoints(
	btVector3& ptsVector,
	btVector3& offsetA,
	btVector3& offsetB,
	btScalar& tA, btScalar& tB,
	const btVector3& translation,
	const btVector3& dirA, btScalar hlenA,
	const btVector3& dirB, btScalar hlenB )
{
	// compute the parameters of the closest points on each line segment

	btScalar dirA_dot_dirB = btDot(dirA,dirB);
	btScalar dirA_dot_trans = btDot(dirA,translation);
	btScalar dirB_dot_trans = btDot(dirB,translation);

	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;

	if ( denom == 0.0f ) {
		tA = 0.0f;
	} else {
		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
		if ( tA < -hlenA )
			tA = -hlenA;
		else if ( tA > hlenA )
			tA = hlenA;
	}

	tB = tA * dirA_dot_dirB - dirB_dot_trans;

	if ( tB < -hlenB ) {
		tB = -hlenB;
		tA = tB * dirA_dot_dirB + dirA_dot_trans;

		if ( tA < -hlenA )
			tA = -hlenA;
		else if ( tA > hlenA )
			tA = hlenA;
	} else if ( tB > hlenB ) {
		tB = hlenB;
		tA = tB * dirA_dot_dirB + dirA_dot_trans;

		if ( tA < -hlenA )
			tA = -hlenA;
		else if ( tA > hlenA )
			tA = hlenA;
	}

	// compute the closest points relative to segment centers.

	offsetA = dirA * tA;
	offsetB = dirB * tB;

	ptsVector = translation - offsetA + offsetB;
}



bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
	gActualSATPairTests++;

//#ifdef TEST_INTERNAL_OBJECTS
	const btVector3 c0 = transA * hullA.m_localCenter;
	const btVector3 c1 = transB * hullB.m_localCenter;
	const btVector3 DeltaC2 = c0 - c1;
//#endif

	btScalar dmin = FLT_MAX;
	int curPlaneTests=0;

	int numFacesA = hullA.m_faces.size();
	// Test normals from hullA
	for(int i=0;i<numFacesA;i++)
	{
		const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
		btVector3 faceANormalWS = transA.getBasis() * Normal;
		if (DeltaC2.dot(faceANormalWS)<0)
			faceANormalWS*=-1.f;

		curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
		gExpectedNbTests++;
		if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
			continue;
		gActualNbTests++;
#endif

		btScalar d;
		btVector3 wA,wB;
		if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
			return false;

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

	int numFacesB = hullB.m_faces.size();
	// Test normals from hullB
	for(int i=0;i<numFacesB;i++)
	{
		const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
		btVector3 WorldNormal = transB.getBasis() * Normal;
		if (DeltaC2.dot(WorldNormal)<0)
			WorldNormal *=-1.f;

		curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
		gExpectedNbTests++;
		if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
			continue;
		gActualNbTests++;
#endif

		btScalar d;
		btVector3 wA,wB;
		if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
			return false;

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

	btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
	int edgeA=-1;
	int edgeB=-1;
	btVector3 worldEdgeA;
	btVector3 worldEdgeB;
	btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
	

	int curEdgeEdge = 0;
	// Test edges
	for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
	{
		const btVector3 edge0 = hullA.m_uniqueEdges[e0];
		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
		for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
		{
			const btVector3 edge1 = hullB.m_uniqueEdges[e1];
			const btVector3 WorldEdge1 = transB.getBasis() * edge1;

			btVector3 Cross = WorldEdge0.cross(WorldEdge1);
			curEdgeEdge++;
			if(!IsAlmostZero(Cross))
			{
				Cross = Cross.normalize();
				if (DeltaC2.dot(Cross)<0)
					Cross *= -1.f;


#ifdef TEST_INTERNAL_OBJECTS
				gExpectedNbTests++;
				if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
					continue;
				gActualNbTests++;
#endif

				btScalar dist;
				btVector3 wA,wB;
				if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
					return false;

				if(dist<dmin)
				{
					dmin = dist;
					sep = Cross;
					edgeA=e0;
					edgeB=e1;
					worldEdgeA = WorldEdge0;
					worldEdgeB = WorldEdge1;
					witnessPointA=wA;
					witnessPointB=wB;
				}
			}
		}

	}

	if (edgeA>=0&&edgeB>=0)
	{
//		printf("edge-edge\n");
		//add an edge-edge contact

		btVector3 ptsVector;
		btVector3 offsetA;
		btVector3 offsetB;
		btScalar tA;
		btScalar tB;

		btVector3 translation = witnessPointB-witnessPointA;

		btVector3 dirA = worldEdgeA;
		btVector3 dirB = worldEdgeB;
		
		btScalar hlenB = 1e30f;
		btScalar hlenA = 1e30f;

		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
			translation,
			dirA, hlenA,
			dirB,hlenB);

		btScalar nlSqrt = ptsVector.length2();
		if (nlSqrt>SIMD_EPSILON)
		{
			btScalar nl = btSqrt(nlSqrt);
			ptsVector *= 1.f/nl;
			if (ptsVector.dot(DeltaC2)<0.f)
			{
				ptsVector*=-1.f;
			}
			btVector3 ptOnB = witnessPointB + offsetB;
			btScalar distance = nl;
			resultOut.addContactPoint(ptsVector, ptOnB,-distance);
		}

	}


	if((DeltaC2.dot(sep))<0.0f)
		sep = -sep;

	return true;
}

void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
	btVertexArray worldVertsB2;
	btVertexArray* pVtxIn = &worldVertsB1;
	btVertexArray* pVtxOut = &worldVertsB2;
	pVtxOut->reserve(pVtxIn->size());

	int closestFaceA=-1;
	{
		btScalar dmin = FLT_MAX;
		for(int face=0;face<hullA.m_faces.size();face++)
		{
			const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
			const btVector3 faceANormalWS = transA.getBasis() * Normal;
		
			btScalar d = faceANormalWS.dot(separatingNormal);
			if (d < dmin)
			{
				dmin = d;
				closestFaceA = face;
			}
		}
	}
	if (closestFaceA<0)
		return;

	const btFace& polyA = hullA.m_faces[closestFaceA];

		// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
	int numVerticesA = polyA.m_indices.size();
	for(int e0=0;e0<numVerticesA;e0++)
	{
		const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
		const btVector3 edge0 = a - b;
		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
		btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);

		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
		btVector3 worldA1 = transA*a;
		btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
		
//int otherFace=0;
#ifdef BLA1
		int otherFace = polyA.m_connectedFaces[e0];
		btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
		btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];

		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else 
		btVector3 planeNormalWS = planeNormalWS1;
		btScalar planeEqWS=planeEqWS1;
		
#endif
		//clip face

		clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
		btSwap(pVtxIn,pVtxOut);
		pVtxOut->resize(0);
	}



//#define ONLY_REPORT_DEEPEST_POINT

	btVector3 point;
	

	// only keep points that are behind the witness face
	{
		btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
		btScalar localPlaneEq = polyA.m_plane[3];
		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
		for (int i=0;i<pVtxIn->size();i++)
		{
			btVector3 vtx = pVtxIn->at(i);
			btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
			if (depth <=minDist)
			{
//				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
				depth = minDist;
			}

			if (depth <=maxDist)
			{
				btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT
				curMaxDist = depth;
#else
#if 0
				if (depth<-3)
				{
					printf("error in btPolyhedralContactClipping depth = %f\n", depth);
					printf("likely wrong separatingNormal passed in\n");
				} 
#endif				
				resultOut.addContactPoint(separatingNormal,point,depth);
#endif
			}
		}
	}
#ifdef ONLY_REPORT_DEEPEST_POINT
	if (curMaxDist<maxDist)
	{
		resultOut.addContactPoint(separatingNormal,point,curMaxDist);
	}
#endif //ONLY_REPORT_DEEPEST_POINT

}





void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{

	btVector3 separatingNormal = separatingNormal1.normalized();
//	const btVector3 c0 = transA * hullA.m_localCenter;
//	const btVector3 c1 = transB * hullB.m_localCenter;
	//const btVector3 DeltaC2 = c0 - c1;



	int closestFaceB=-1;
	btScalar dmax = -FLT_MAX;
	{
		for(int face=0;face<hullB.m_faces.size();face++)
		{
			const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
			const btVector3 WorldNormal = transB.getBasis() * Normal;
			btScalar d = WorldNormal.dot(separatingNormal);
			if (d > dmax)
			{
				dmax = d;
				closestFaceB = face;
			}
		}
	}
				btVertexArray worldVertsB1;
				{
					const btFace& polyB = hullB.m_faces[closestFaceB];
					const int numVertices = polyB.m_indices.size();
					for(int e0=0;e0<numVertices;e0++)
					{
						const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
						worldVertsB1.push_back(transB*b);
					}
				}

	
	if (closestFaceB>=0)
		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);

}