// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#include "GuPCMContactGen.h"
#include "GuPCMShapeConvex.h"
#include "GuPCMContactGenUtil.h"
#include "foundation/PxVecMath.h"
#include "foundation/PxAlloca.h"
#include "GuVecCapsule.h"
#include "GuVecBox.h"
#include "GuEPA.h"
#include "geomutils/PxContactBuffer.h"
#include "common/PxRenderOutput.h"

#define PCM_USE_INTERNAL_OBJECT 1

using namespace physx;
using namespace Gu;
using namespace aos;

//Precompute the convex data
//     7+------+6			0 = ---
//     /|     /|			1 = +--
//    / |    / |			2 = ++-
//   / 4+---/--+5			3 = -+-
// 3+------+2 /    y   z	4 = --+
//  | /    | /     |  /		5 = +-+
//  |/     |/      |/		6 = +++
// 0+------+1      *---x	7 = -++

static bool testFaceNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, const PxMatTransformV& transform1To0, const FloatVArg contactDist, 
	FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, FeatureStatus faceStatus, FeatureStatus& status)
{
	PX_UNUSED(polyData1);
		
	FloatV _minOverlap = FMax();//minOverlap;
	PxU32 _feature = 0;
	Vec3V _faceNormal = faceNormal;
	FloatV min0, max0;
	FloatV min1, max1;
	
	const Vec3V center1To0 = transform1To0.p;
		
#if PCM_USE_INTERNAL_OBJECT
	const Vec3V zeroV = V3Zero();
	const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
	const Vec3V internalCenter1In0 = transform1To0.transform(shapeSpaceCenter1);
	const FloatV internalRadius1 = FLoad(polyData1.mInternal.mInternalRadius);
	const Vec3V internalExtents1 = V3LoadU_SafeReadW(polyData1.mInternal.mInternalExtents);
	const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
#endif

	//in the local space of polyData0
	for(PxU32 i=0; i<polyData0.mNbPolygons; ++i)
	{
		const HullPolygonData& polygon = polyData0.mPolygons[i];

		const Vec3V minVert = V3LoadU(polyData0.mVerts[polygon.mMinIndex]);
		const FloatV planeDist = FLoad(polygon.mPlane.d);
		const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);

		//transform plane n to shape space
		const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map0->shape2Vertex, vertexSpacePlaneNormal);

		const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
		//ML::use this to avoid LHS
		min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
		max0 = FMul(FNeg(planeDist), magnitude);

		//normalize shape space normal
		const Vec3V n0 = V3Scale(shapeSpacePlaneNormal, magnitude);

		//calculate polyData1 projection
		//rotate polygon's normal into the local space of polyData1
		const Vec3V n1 = transform0To1.rotate(n0);

#if PCM_USE_INTERNAL_OBJECT
		//test internal object	
		//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
		//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
		//If polyData0 have scale, internalExtens1 will be 0.
		const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
		const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
		const FloatV internalTrans = V3Dot(internalCenter1In0, n0);

		const FloatV _min1 = FSub(internalTrans, radius);
		const FloatV _max1 = FAdd(internalTrans, radius);

		const FloatV _min = FMax(min0, _min1);
		const FloatV _max = FMin(max0, _max1);

		const FloatV _tempOverlap = FSub(_max, _min);
		//const FloatV _tempOverlap = FSub(max0, _min1);	

		//Internal object overlaps more than current min, so can skip it
		//because (a) it isn't a separating axis and (b) it isn't the smallest axis
		if(FAllGrtr(_tempOverlap, _minOverlap))
			continue;
#endif
		const FloatV translate = V3Dot(center1To0, n0);
		map1->doSupport(n1, min1, max1);

		min1 = FAdd(translate, min1);
		max1 = FAdd(translate, max1);

		const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));

		if(BAllEqTTTT(con))
			return false;

		const FloatV tempOverlap = FSub(max0, min1);

		if(FAllGrtr(_minOverlap, tempOverlap))
		{
			_minOverlap = tempOverlap;
			_feature = i;
			_faceNormal = n0;
		}
	}   

	if(FAllGrtr(minOverlap, _minOverlap))
	{
		faceNormal = _faceNormal; 
		minOverlap = _minOverlap;
		status = faceStatus;
	}

	feature = _feature;

	return true;
}

//plane is in the shape space of polyData
static void buildPartialHull(const PolygonalData& polyData, const SupportLocal* map, SeparatingAxes& validAxes, const Vec3VArg planeP, const Vec3VArg planeDir)
{		
	const FloatV zero = FZero();
	const Vec3V dir = V3Normalize(planeDir);
	for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
	{
		const HullPolygonData& polygon = polyData.mPolygons[i];
		const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
			
		Vec3V v0 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[polygon.mNbVerts - 1]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
		
		FloatV dist0 = V3Dot(dir, V3Sub(v0, planeP));

		for (PxU32 iStart = 0; iStart < polygon.mNbVerts; iStart++)
		{
			const Vec3V v1 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[iStart]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
				
			const FloatV dist1 = V3Dot(dir, V3Sub(v1, planeP));

			const BoolV con = BOr(FIsGrtr(dist0, zero), FIsGrtr(dist1, zero));

			//cull edge if either of the vertex will on the positive size of the plane
			if(BAllEqTTTT(con))
			{
				const Vec3V tempV = V3Sub(v0, v1); 
				PxVec3 temp;
				V3StoreU(tempV, temp);
				validAxes.addAxis(temp.getNormalized());
			}

			v0 = v1;
			dist0 = dist1;
		}
	}
}

static bool testEdgeNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, const PxMatTransformV& transform1To0, const FloatVArg contactDist,
	FloatV& minOverlap, Vec3V& edgeNormalIn0, FeatureStatus edgeStatus, FeatureStatus& status)
{
	FloatV overlap = minOverlap;
	FloatV min0, max0;
	FloatV min1, max1;
	const FloatV eps = FEps();

	const Vec3V shapeSpaceCenter0 = V3LoadU(polyData0.mCenter);
	const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);

#if PCM_USE_INTERNAL_OBJECT
	const Vec3V zeroV = V3Zero();
	const Vec3V internalCenter1In0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
	const FloatV internalRadius1 = FLoad(polyData1.mInternal.mInternalRadius);
	const Vec3V internalExtents1 = V3LoadU_SafeReadW(polyData1.mInternal.mInternalExtents);
	const Vec3V negInternalExtents1 = V3Neg(internalExtents1);

	const FloatV internalRadius0 = FLoad(polyData0.mInternal.mInternalRadius);
	const Vec3V internalExtents0 = V3LoadU_SafeReadW(polyData0.mInternal.mInternalExtents);
	const Vec3V negInternalExtents0 = V3Neg(internalExtents0);
#endif

	const Vec3V center1To0 = transform1To0.p;

	//in polyData0 shape space
	const Vec3V dir0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
	const Vec3V support0 = map0->doSupport(dir0);

	//in polyData1 shape space
	const Vec3V dir1 = transform0To1.rotate(V3Neg(dir0));
	const Vec3V support1 = map1->doSupport(dir1);
		
	const Vec3V support0In1 = transform0To1.transform(support0);
	const Vec3V support1In0 = transform1To0.transform(support1);

	SeparatingAxes mSA0;
	SeparatingAxes mSA1;
	mSA0.reset();
	mSA1.reset();
		
	buildPartialHull(polyData0, map0, mSA0, support1In0, dir0);
	buildPartialHull(polyData1, map1, mSA1, support0In1, dir1);

	const PxVec3* axe0 = mSA0.getAxes();
	const PxVec3* axe1 = mSA1.getAxes();
	const PxU32 numAxe0 = mSA0.getNumAxes();
	const PxU32 numAxe1 = mSA1.getNumAxes();
	for(PxU32 i=0; i < numAxe0; ++i)
	{
		//axe0[i] is in the shape space of polyData0
		const Vec3V v0 = V3LoadU(axe0[i]);

		for(PxU32 j=0; j< numAxe1; ++j)
		{
			//axe1[j] is in the shape space of polyData1
			const Vec3V v1 = V3LoadU(axe1[j]);

			const Vec3V dir = V3Cross(v0, transform1To0.rotate(v1));
			const FloatV lenSq = V3Dot(dir, dir);
			if(FAllGrtr(eps, lenSq))
				continue;

			//n0 is in polyData0's local space
			const Vec3V n0 = V3Scale(dir, FRsqrt(lenSq));
				
			//n1 is in polyData1's local space
			const Vec3V n1 = transform0To1.rotate(n0);

#if PCM_USE_INTERNAL_OBJECT
			//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
			//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
			//If polyData0 have scale, internalExtens1 will be 0.
			//vertex space n1
			const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
			const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);

			const FloatV internalTrans = V3Dot(internalCenter1In0, n0);

			const FloatV _min1 = FSub(internalTrans, radius);
			const FloatV _max1 = FAdd(internalTrans, radius);

			const Vec3V proj0 = V3Sel(V3IsGrtr(n0, zeroV), internalExtents0, negInternalExtents0);
			const FloatV radius0 = FMax(V3Dot(n0, proj0), internalRadius0);

			const FloatV _max0 = radius0;
			const FloatV _min0 = FNeg(radius0);

			PX_ASSERT(FAllGrtrOrEq(_max0, _min0));
			PX_ASSERT(FAllGrtrOrEq(_max1, _min1));

			const FloatV _min = FMax(_min0, _min1);
			const FloatV _max = FMin(_max0, _max1);

			const FloatV _tempOverlap = FSub(_max, _min);

			//Internal object overlaps more than current min, so can skip it
			//because (a) it isn't a separating axis and (b) it isn't the smallest axis
			if(FAllGrtr(_tempOverlap, overlap))
				continue;
#endif
			//get polyData0's projection
			map0->doSupport(n0, min0, max0);

			const FloatV translate = V3Dot(center1To0, n0);

			//get polyData1's projection
			map1->doSupport(n1, min1, max1);

			min1 = FAdd(translate, min1);
			max1 = FAdd(translate, max1);

			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
			if(BAllEqTTTT(con))
				return false;

			const FloatV tempOverlap = FSub(max0, min1);

#if PCM_USE_INTERNAL_OBJECT
			PX_ASSERT(FAllGrtrOrEq(tempOverlap, _tempOverlap));
#endif
			if(FAllGrtr(overlap, tempOverlap))
			{
				overlap = tempOverlap;
				edgeNormalIn0 = n0;
				status = edgeStatus;
			}
		}
	}

	minOverlap = overlap;
		
	return true;
}

//contactNormal is in the space of polyData0
static void generatedContacts(const PolygonalData& polyData0, const PolygonalData& polyData1, const HullPolygonData& referencePolygon, const HullPolygonData& incidentPolygon,
	const SupportLocal* map0, const SupportLocal* map1, const PxMatTransformV& transform0To1, PersistentContact* manifoldContacts, 
	PxU32& numContacts, const FloatVArg contactDist, PxRenderOutput* renderOutput)
{
	PX_UNUSED(renderOutput);
	const FloatV zero = FZero();

	const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;

	//transform the plane normal to shape space
	const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
		
	//this is the matrix transform all points to the 2d plane
	const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);

	const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;

	Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
	Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
	bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));
	FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));

	//Transform all the verts from vertex space to shape space
	map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);
	map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);

#if PCM_LOW_LEVEL_DEBUG
	PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, (PxU32)referencePolygon.mNbVerts, 0x00ff0000); 
	PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, (PxU32)incidentPolygon.mNbVerts,  0x0000ff00); 
#endif
	
	//This is used to calculate the project point when the 2D reference face points is inside the 2D incident face point
	const Vec3V sPoint = points1In0[0]; 
	
	PX_ASSERT(incidentPolygon.mNbVerts <= 64);
 
	Vec3V eps = Vec3V_From_FloatV(FEps());
	Vec3V max = Vec3V_From_FloatV(FMax());
	Vec3V nmax = V3Neg(max); 

	//transform reference polygon to 2d, calculate min and max
	Vec3V rPolygonMin = max;
	Vec3V rPolygonMax = nmax;
	for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
	{
		points0In0[i] = M33MulV3(rot, points0In0[i]);
		rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
		rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
	}
		
	rPolygonMin = V3Sub(rPolygonMin, eps);
	rPolygonMax = V3Add(rPolygonMax, eps);

	const FloatV d = V3GetZ(points0In0[0]);
	const FloatV rd = FAdd(d, contactDist);

	Vec3V iPolygonMin = max; 
	Vec3V iPolygonMax = nmax;

	PxU32 inside = 0;
	for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
	{
		const Vec3V vert1 = points1In0[i]; //this still in polyData1's local space
		const Vec3V a = transform0To1.transformInv(vert1);
		points1In0[i] = M33MulV3(rot, a);
		const FloatV z = V3GetZ(points1In0[i]);
		points1In0TValue[i] = FSub(z, d);
		points1In0[i] = V3SetZ(points1In0[i], d);
		iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
		iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
		if(FAllGrtr(rd, z))
		{
			points1In0Penetration[i] = true;

			if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
			{
				inside++;

				if (numContacts == PxContactBuffer::MAX_CONTACTS)
					return;
				
				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), points1In0TValue[i]);
				manifoldContacts[numContacts].mLocalPointA = vert1;
				manifoldContacts[numContacts].mLocalPointB = M33TrnspsMulV3(rot, points1In0[i]);
				manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
			}
		}
		else
		{
			points1In0Penetration[i] = false;
		}
	}

	if(inside == incidentPolygon.mNbVerts)
		return;

	inside = 0;
	iPolygonMin = V3Sub(iPolygonMin, eps);
	iPolygonMax = V3Add(iPolygonMax, eps);

	const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
	
	const Vec3V contactNormalIn1 = transform0To1.rotate(contactNormal);

	for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
	{
		if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
		{
			//const Vec3V vert0=Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]);
			const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
			const Vec3V a = transform0To1.transform(vert0);

			const FloatV nom = V3Dot(incidentNormal, V3Sub(sPoint, a)); 
			const FloatV denom = V3Dot(incidentNormal, contactNormalIn1);
			PX_ASSERT(FAllEq(denom, zero)==0);
			const FloatV t = FDiv(nom, denom);

			if(FAllGrtr(t, contactDist))
				continue;

			inside++;

			if (numContacts == PxContactBuffer::MAX_CONTACTS)
				return;
			
			const Vec3V projPoint = V3ScaleAdd(contactNormalIn1, t, a);
			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t);

			manifoldContacts[numContacts].mLocalPointA = projPoint;
			manifoldContacts[numContacts].mLocalPointB = vert0;
			manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
		}
	}

	if(inside == referencePolygon.mNbVerts)
		return;

	//(2) segment intersection
	for (PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
	{
		if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
			continue;

		const Vec3V ipA = points1In0[iStart];
		const Vec3V ipB = points1In0[iEnd];

		Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
		Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));

		const Vec3V iMin = V3Min(ipA, ipB);
		const Vec3V iMax = V3Max(ipA, ipB);

		for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
		{
			const Vec3V rpA = points0In0[rStart];
			const Vec3V rpB = points0In0[rEnd];

			const Vec3V rMin = V3Min(rpA, rpB);
			const Vec3V rMax = V3Max(rpA, rpB);
				
			const BoolV tempCon = BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
			const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
		
			if(BAllEqTTTT(con))
				continue;
					
			FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
			FloatV a2 = signed2DTriArea(rpA, rpB, ipB);

			if(FAllGrtr(zero, FMul(a1, a2)))
			{
				FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
				FloatV a4 = signed2DTriArea(ipA, ipB, rpB);

				if(FAllGrtr(zero, FMul(a3, a4)))
				{
					//these two segment intersect

					const FloatV t = FDiv(a1, FSub(a2, a1));

					const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri); 
					const Vec3V pAA = V3SetZ(pBB, d);
					const Vec3V pA = M33TrnspsMulV3(rot, pAA);
					const Vec3V pB = transform0To1.transform(M33TrnspsMulV3(rot, pBB));
					const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
					if(FAllGrtr(pen, contactDist))
						continue;

					if (numContacts == PxContactBuffer::MAX_CONTACTS)
						return;

					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
					manifoldContacts[numContacts].mLocalPointA = pB;
					manifoldContacts[numContacts].mLocalPointB = pA;
					manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
				}
			}
		}
	}
}

bool Gu::generateFullContactManifold(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, PersistentContact* manifoldContacts, PxU32& numContacts,
	const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, PxReal marginA, PxReal marginB, bool doOverlapTest, 
	PxRenderOutput* renderOutput, PxReal toleranceLength)
{
	const PxMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
	const PxMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
	
	if(doOverlapTest)
	{
		//if gjk fail, SAT based yes/no test
		FeatureStatus status = POLYDATA0;
		FloatV minOverlap = FMax();
		Vec3V minNormal = V3Zero();
	
		PxU32 feature0;
		//in the local space of polyData0, minNormal is in polyData0 space
		if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
			return false;
			
		PxU32 feature1;
		//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
		if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
			return false;

		bool doEdgeTest = false;
			
EdgeTest:
		if(doEdgeTest)
		{
			if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
				return false;

			if(status != EDGE)
				return true;
		}
			
		if(status == POLYDATA0)
		{
			//minNormal is in the local space of polydata0
		
			const HullPolygonData& referencePolygon = polyData0.mPolygons[feature0];
			const Vec3V n = transform0To1V.rotate(minNormal);
			const HullPolygonData& incidentPolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, n)];
				
			generatedContacts(polyData0, polyData1, referencePolygon, incidentPolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);
				
			if (numContacts > 0)
			{
				const Vec3V nn = V3Neg(n);
				//flip the contacts
				for(PxU32 i=0; i<numContacts; ++i)
				{
					const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
					manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
					manifoldContacts[i].mLocalPointA = localPointB;
					manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
				}
			}
		}
		else if(status == POLYDATA1)
		{
			//minNormal is in the local space of polydata1
			const HullPolygonData& referencePolygon = polyData1.mPolygons[feature1];
			const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, transform1To0V.rotate(minNormal))];
				
			//reference face is polyData1
			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);

		}
		else //if(status == EDGE0)
		{
			//minNormal is in the local space of polydata0
			
			const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, V3Neg(minNormal))];
			const HullPolygonData& referencePolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, transform0To1V.rotate(minNormal))];
			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
		}

		if(numContacts == 0 && !doEdgeTest)
		{
			doEdgeTest = true;
			goto EdgeTest;
		}
	}
	else
	{
		const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS;
		const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS;
		const PxReal toleranceA = PxClamp(marginA, lowerEps, upperEps);
		const PxReal toleranceB = PxClamp(marginB, lowerEps, upperEps);

		const Vec3V negNormal = V3Neg(normal);
		const Vec3V normalIn0 = transform0To1V.rotateInv(normal);

		PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, negNormal, closestB, toleranceB);
		PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, normalIn0, transform0To1V.transformInv(closestA),
			toleranceA);

		const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
		const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];

		const Vec3V referenceNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
		const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
		
		const FloatV referenceProject = FAbs(V3Dot(referenceNormal, negNormal));
		const FloatV incidentProject = FAbs(V3Dot(incidentNormal, normalIn0));

		if (FAllGrtrOrEq(referenceProject, incidentProject))
		{
			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
		}
		else
		{
			generatedContacts(polyData0, polyData1, incidentPolygon, referencePolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);

			if (numContacts > 0)
			{
				const Vec3V n = transform0To1V.rotate(incidentNormal);

				const Vec3V nn = V3Neg(n);
				//flip the contacts
				for (PxU32 i = 0; i<numContacts; ++i)
				{
					const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
					manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
					manifoldContacts[i].mLocalPointA = localPointB;
					manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
				}
			}
		}
	}
	
	return true;
}

//This function called by box/convex and convex/convex pcm function
bool Gu::addGJKEPAContacts(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PxMatTransformV& aToB, GjkStatus status,
	PersistentContact* manifoldContacts, const FloatV replaceBreakingThreshold, const FloatV tolerenceLength, GjkOutput& output,
	PersistentContactManifold& manifold)
{
	bool doOverlapTest = false;
	if (status == GJK_DEGENERATE)
	{
		const Vec3V normal = output.normal;
		const FloatV costheta = V3Dot(output.searchDir, normal);
		if (FAllGrtr(costheta, FLoad(0.9999f)))
		{
			const Vec3V centreA = relativeConvex->getCenter();
			const Vec3V centreB = localConvex->getCenter();

			const Vec3V dir = V3Normalize(V3Sub(centreA, centreB));
			const FloatV theta = V3Dot(dir, normal);

			if (FAllGrtr(theta, FLoad(0.707f)))
			{
				addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
			}
			else
			{
				doOverlapTest = true;
			}
		}
		else
		{
			doOverlapTest = true;
		}
	}
	else if (status == GJK_CONTACT)
	{
		addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
	}
	else
	{
		PX_ASSERT(status == EPA_CONTACT);

		status = epaPenetration(*relativeConvex, *localConvex, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
			true, tolerenceLength, output);
		if (status == EPA_CONTACT)
		{
			addManifoldPoint(manifoldContacts, manifold, output, aToB, replaceBreakingThreshold);
		}
		else
		{
			doOverlapTest = true;
		}
	}

	return doOverlapTest;
}

bool Gu::computeMTD(const PolygonalData& polyData0, const PolygonalData& polyData1, const SupportLocal* map0, const SupportLocal* map1, FloatV& penDepth, Vec3V& normal)
{
	using namespace aos;

	const PxMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
	const PxMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
	
	FeatureStatus status = POLYDATA0;
	FloatV minOverlap = FMax();
	Vec3V minNormal = V3Zero();
	const FloatV contactDist = FZero();

	PxU32 feature0;
	//in the local space of polyData0, minNormal is in polyData0 space
	if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
		return false;
		
	PxU32 feature1;
	//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
	if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
		return false;
	
	if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
		return false;

	penDepth = minOverlap;
	if(status == POLYDATA1)
	{
		//minNormal is in the local space of polydata1
		normal = map1->transform.rotate(minNormal);
	}
	else
	{
		PX_ASSERT(status == POLYDATA0 || status == EDGE);
		//ML: status == POLYDATA0 or status == EDGE, minNormal is in the local space of polydata0
		normal = V3Neg(map0->transform.rotate(minNormal));
	}

	return true;
}

