text: //
  // 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-2021 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 "geomutils/GuContactBuffer.h"
  
  #include "GuGJKPenetration.h"
  #include "GuEPA.h"
  #include "GuVecConvexHull.h"
  #include "GuVecConvexHullNoScale.h"
  #include "GuGeometryUnion.h"   
  
  #include "GuContactMethodImpl.h"
  #include "GuPCMShapeConvex.h"
  #include "GuPCMContactGen.h"
  
  
  namespace physx
  {
  using namespace Ps::aos;
  
  namespace Gu
  {
  
  static bool fullContactsGenerationConvexConvex(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PsTransformV& transf0, const PsTransformV& transf1, 
  											   const bool idtScale0, const bool idtScale1, PersistentContact* manifoldContacts,  ContactBuffer& contactBuffer, 
  											   PersistentContactManifold& manifold, Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB,
  											   const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceLength)
  {
  	Gu::PolygonalData polyData0, polyData1;
  	const ConvexHullV& convexHull0 = relativeConvex->getConvex<ConvexHullV>();
  	const ConvexHullV& convexHull1 = localConvex->getConvex<ConvexHullV>();
  	getPCMConvexData(convexHull0, idtScale0, polyData0);
  	getPCMConvexData(convexHull1, idtScale1, polyData1);
  
  	PxU8 buff0[sizeof(SupportLocalImpl<ConvexHullV>)];
  	PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)];
  
  	SupportLocal* map0 = (idtScale0 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull0), transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)) : 
  		static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullV>)(convexHull0, transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)));
  
  	SupportLocal* map1 = (idtScale1 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull1), transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)) : 
  		static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull1, transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)));
  
  
  	PxU32 numContacts = 0;
  
  	if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMarginF(), 
  		convexHull1.getMarginF(), doOverlapTest, renderOutput, toleranceLength))
  	{
  		
  		if (numContacts > 0)
  		{
  			//reduce contacts
  			manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
  
  			const Vec3V worldNormal = manifold.getWorldNormal(transf1);
  
  			//add the manifold contacts;
  			manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
  
  
  #if	PCM_LOW_LEVEL_DEBUG
  			manifold.drawManifold(*renderOutput, transf0, transf1);
  #endif
  		}
  		else
  		{
  			//if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
  			if (!doOverlapTest)
  			{
  				const Vec3V worldNormal = manifold.getWorldNormal(transf1);
  
  				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
  			}
  
  		}
  		return true;
  		
  	}
  
  	return false;
  
  }
  
  static bool generateOrProcessContactsConvexConvex(const GjkConvex* relativeConvex, const GjkConvex* localConvex, const PsTransformV& transf0, const PsTransformV& transf1,
  	const PsMatTransformV& aToB, GjkStatus status, GjkOutput& output, PersistentContactManifold& manifold, ContactBuffer& contactBuffer,
  	const PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
  	const bool idtScale0, const bool idtScale1, const PxReal toleranceLength, Cm::RenderOutput* renderOutput)
  {
  
  	if (status == GJK_NON_INTERSECT)
  	{
  		return false;
  	}
  	else
  	{
  		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
  
  		const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
  
  		const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
  
  		//addGJKEPAContacts will increase the number of contacts in manifold. If status == EPA_CONTACT, we need to run epa algorithm and generate closest points, normal and
  		//pentration. If epa doesn't degenerate, we will store the contacts information in the manifold. Otherwise, we will return true to do the fallback test
  		const bool doOverlapTest = addGJKEPAContacts(relativeConvex, localConvex, aToB, status, manifoldContacts, replaceBreakingThreshold, FLoad(toleranceLength), output, manifold);
  		
  #if PCM_LOW_LEVEL_DEBUG
  		manifold.drawManifold(*renderOutput, transf0, transf1);
  #endif
  		//ML: after we refresh the contacts(newContacts) and generate a GJK/EPA contacts(we will store that in the manifold), if the number of contacts is still less than the original contacts,
  		//which means we lose too mang contacts and we should regenerate all the contacts in the current configuration
  		//Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
  		//which means we should throw away the existing contacts and do full contact gen
  		const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, output.normal)) || (manifold.mNumContacts < initialContacts);
  
  		if (fullContactGen || doOverlapTest)
  		{
  			return fullContactsGenerationConvexConvex(relativeConvex, localConvex, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer,
  				manifold, output.normal, output.closestA, output.closestB, contactDist, doOverlapTest, renderOutput, toleranceLength);
  		}
  		else
  		{
  			const Vec3V newLocalNor = V3Add(localNor, output.normal);
  			const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
  			//const Vec3V worldNormal = transf1.rotate(normal);
  			manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
  			return true;
  		}
  	}
  }
  
  static bool convexHullNoScale0(const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PsTransformV& transf0, const PsTransformV& transf1,
  	const PsMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, ContactBuffer& contactBuffer,
  	const PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
  	const bool idtScale1, const PxReal toleranceLength, Cm::RenderOutput* renderOutput)
  {
  
  	const RelativeConvex<ConvexHullNoScaleV> convexA(static_cast<const ConvexHullNoScaleV&>(convexHull0), aToB);
  	if(idtScale1)
  	{
  		const LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
  		GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
  						manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
  
  		return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
  			contactBuffer, initialContacts, minMargin, contactDist, true, true, toleranceLength, renderOutput);
  	
  	}
  	else
  	{
  		const LocalConvex<ConvexHullV> convexB(convexHull1);
  		GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullNoScaleV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
  					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
  
  
  		return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
  			contactBuffer, initialContacts, minMargin, contactDist, true, false, toleranceLength, renderOutput);
  	}
  }
  
  static bool convexHullHasScale0(const ConvexHullV& convexHull0, const ConvexHullV& convexHull1, const PsTransformV& transf0, const PsTransformV& transf1,
  	const PsMatTransformV& aToB, GjkOutput& output, PersistentContactManifold& manifold, ContactBuffer& contactBuffer,
  	const PxU32 initialContacts, const FloatV minMargin, const FloatV contactDist,
  	const bool idtScale1, const PxReal toleranceLength, Cm::RenderOutput* renderOutput)
  {
  	
  	RelativeConvex<ConvexHullV> convexA(convexHull0, aToB);
  	if(idtScale1)
  	{
  		LocalConvex<ConvexHullNoScaleV> convexB(static_cast<const ConvexHullNoScaleV&>(convexHull1));
  		GjkStatus status = gjkPenetration< RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, true,
  						manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,output);
  		
  		return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
  			contactBuffer, initialContacts, minMargin, contactDist, false, true, toleranceLength, renderOutput);
  	
  	}
  	else
  	{
  		LocalConvex<ConvexHullV> convexB(convexHull1);
  		GjkStatus status = gjkPenetration<RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, contactDist, true,
  					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, output);
  
  		return generateOrProcessContactsConvexConvex(&convexA, &convexB, transf0, transf1, aToB, status, output, manifold,
  			contactBuffer, initialContacts, minMargin, contactDist, false, false, toleranceLength, renderOutput);
  	}
  }
  
  
  bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
  {
  	const PxConvexMeshGeometryLL& shapeConvex0 = shape0.get<const PxConvexMeshGeometryLL>();
  	const PxConvexMeshGeometryLL& shapeConvex1 = shape1.get<const PxConvexMeshGeometryLL>();
  	PersistentContactManifold& manifold = cache.getManifold();
  
  	Ps::prefetchLine(shapeConvex0.hullData);
  	Ps::prefetchLine(shapeConvex1.hullData);
  
  	PX_ASSERT(transform1.q.isSane());
  	PX_ASSERT(transform0.q.isSane());
  
  	const Vec3V vScale0 = V3LoadU_SafeReadW(shapeConvex0.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
  	const Vec3V vScale1 = V3LoadU_SafeReadW(shapeConvex1.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
  	const FloatV contactDist = FLoad(params.mContactDistance);
  
  	//Transfer A into the local space of B
  	const PsTransformV transf0 = loadTransformA(transform0);
  	const PsTransformV transf1 = loadTransformA(transform1);
  	const PsTransformV curRTrans(transf1.transformInv(transf0));
  	const PsMatTransformV aToB(curRTrans);
  	
  	const Gu::ConvexHullData* hullData0 = shapeConvex0.hullData;
  	const Gu::ConvexHullData* hullData1 = shapeConvex1.hullData;
  
  	const PxReal toleranceLength = params.mToleranceLength;
  	const FloatV convexMargin0 = Gu::CalculatePCMConvexMargin(hullData0, vScale0, toleranceLength);
  	const FloatV convexMargin1 = Gu::CalculatePCMConvexMargin(hullData1, vScale1, toleranceLength);
  	
  	const PxU32 initialContacts = manifold.mNumContacts;
  
  	const FloatV minMargin = FMin(convexMargin0, convexMargin1);
  	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
  	
  	manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
  
  	//ML: after refreshContactPoints, we might lose some contacts
  	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
  
  	const Vec3V extent0 = V3Mul(V3LoadU(hullData0->mInternal.mExtents), vScale0);
  	const Vec3V extent1 = V3Mul(V3LoadU(hullData0->mInternal.mExtents), vScale1);
  
  	const FloatV radiusA = V3Length(extent0);
  	const FloatV radiusB = V3Length(extent1);
  
  	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, transf0.q, transf1.q, minMargin, radiusA, radiusB))
  	{
  		manifold.setRelativeTransform(curRTrans, transf0.q, transf1.q);
  		
  		const bool idtScale0 = shapeConvex0.scale.isIdentity();
  		const bool idtScale1 = shapeConvex1.scale.isIdentity();
  		const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x);
  		const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x);
  		
  		Gu::ConvexHullV convexHull0(hullData0, V3LoadU(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0);
  		Gu::ConvexHullV convexHull1(hullData1, V3LoadU(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1);
  
  		GjkOutput output;
  		
  		if(idtScale0)
  		{
  			return convexHullNoScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
  				contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
  		}
  		else
  		{
  			return convexHullHasScale0(convexHull0, convexHull1, transf0, transf1, aToB, output, manifold,
  				contactBuffer, initialContacts, minMargin, contactDist, idtScale1, toleranceLength, renderOutput);
  		}
  	
  	}
  	else if(manifold.getNumContacts()> 0)
  	{
  		const Vec3V worldNormal =  manifold.getWorldNormal(transf1);
  		manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
  #if	PCM_LOW_LEVEL_DEBUG
  		manifold.drawManifold(*renderOutput, transf0, transf1);
  #endif
  		return true;
  	}
  
  	return false;
  	
  }
  }
  }
  