PhysX 3.4.1, APEX 1.4.1 Release @22845541v3.4.1

17 files changed, 456 insertions, 281 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
index b93df8e3..e66f601e 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
@@ -927,8 +927,9 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
 	const PsTransformV curRTrans(transf1.transformInv(transf0));
 	const PsMatTransformV aToB(curRTrans);
 
-	const FloatV boxMargin0 = Gu::CalculatePCMBoxMargin(boxExtents0);
-	const FloatV boxMargin1 = Gu::CalculatePCMBoxMargin(boxExtents1);
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV boxMargin0 = Gu::CalculatePCMBoxMargin(boxExtents0, toleranceLength);
+	const FloatV boxMargin1 = Gu::CalculatePCMBoxMargin(boxExtents1, toleranceLength);
 	const FloatV minMargin = FMin(boxMargin0, boxMargin1);
 
 	const PxU32 initialContacts = manifold.mNumContacts;
@@ -958,7 +959,7 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
 			if(numContacts > 0)
 			{
 			
-				manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+				manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
 				const Vec3V worldNormal = transfV1.rotate(Vec3V_From_Vec4V(manifold.mContactPoints[0].mLocalNormalPen));
 				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transfV1);
 #if	PCM_LOW_LEVEL_DEBUG
@@ -971,6 +972,7 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
 				const Vec3V zeroV = V3Zero();
 				BoxV box0(zeroV, boxExtents0);
 				BoxV box1(zeroV, boxExtents1);
+
 				Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // these will be in the local space of B
 				FloatV penDep = FZero(); 
 				manifold.mNumWarmStartPoints = 0;
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
index 2f3965eb..03126c61 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
@@ -42,6 +42,7 @@
 #include "GuPCMShapeConvex.h"
 #include "GuContactBuffer.h"
 
+
 namespace physx
 {
 
@@ -53,7 +54,7 @@ namespace Gu
 static bool fullContactsGenerationBoxConvex(const PxVec3& halfExtents, const BoxV& box, ConvexHullV& convexHull, const PsTransformV& transf0, const PsTransformV& transf1, 
 									PersistentContact* manifoldContacts, ContactBuffer& contactBuffer, Gu::PersistentContactManifold& manifold, Vec3VArg normal, 
 									const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg contactDist, const bool idtScale, const bool doOverlapTest, Cm::RenderOutput* renderOutput,
-									const FloatVArg toleranceScale)
+									const PxReal toleranceScale)
 {
 	Gu::PolygonalData polyData0;
 	PCMPolygonalBox polyBox0(halfExtents);
@@ -72,13 +73,13 @@ static bool fullContactsGenerationBoxConvex(const PxVec3& halfExtents, const Box
 		static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
 
 	PxU32 numContacts = 0;
-	if(generateFullContactManifold(polyData0, polyData1, &map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, box.getMargin(), convexHull.getMargin(), 
+	if(generateFullContactManifold(polyData0, polyData1, &map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, box.getMarginF(), convexHull.getMarginF(), 
 		doOverlapTest, renderOutput, toleranceScale))
 	{
 		if (numContacts > 0)
 		{
 			//reduce contacts
-			manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+			manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceScale);
 
 #if	PCM_LOW_LEVEL_DEBUG
 			manifold.drawManifold(*renderOutput, transf0, transf1);
@@ -185,9 +186,10 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 	const PsTransformV curRTrans(transf1.transformInv(transf0));
 	const PsMatTransformV aToB(curRTrans);
 
+	const PxReal tolerenceLength = params.mToleranceLength;
 	const Gu::ConvexHullData* hullData = shapeConvex.hullData;
-	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
-	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, tolerenceLength);
+	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents, tolerenceLength);
 
 	const FloatV minMargin = FMin(convexMargin, boxMargin);//FMin(boxMargin, convexMargin);
 	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
@@ -212,6 +214,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
 		const bool idtScale = shapeConvex.scale.isIdentity();
 		Gu::ShrunkConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
+		convexHull.setMaxMargin(shapeConvex.maxMargin);
 		Gu::BoxV box(zeroV, boxExtents);
 		
 		const RelativeConvex<BoxV> convexA(box, aToB);
@@ -236,7 +239,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 		if(status == GJK_DEGENERATE)
 		{
 			return fullContactsGenerationBoxConvex(shapeBox.halfExtents, box, convexHull, transf0, transf1, manifoldContacts, contactBuffer, 
-				manifold, normal, closestA, closestB, contactDist, idtScale, true, renderOutput, FLoad(params.mToleranceLength));
+				manifold, normal, closestA, closestB, contactDist, idtScale, true, renderOutput, params.mToleranceLength);
 		}
 		else if(status == GJK_NON_INTERSECT)
 		{
@@ -261,7 +264,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 			if (fullContactGen || doOverlapTest)
 			{
 				return fullContactsGenerationBoxConvex(shapeBox.halfExtents, box, convexHull, transf0, transf1, manifoldContacts, contactBuffer, 
-					manifold, normal, closestA, closestB, contactDist, idtScale, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
+					manifold, normal, closestA, closestB, contactDist, idtScale, doOverlapTest, renderOutput, params.mToleranceLength);
 			}
 			else
 			{
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp
index ecb94a66..9606b959 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp
@@ -48,7 +48,7 @@ namespace Gu
 
 static bool fullContactsGenerationCapsuleBox(const CapsuleV& capsule, const BoxV& box, const PxVec3 halfExtents,  const PsMatTransformV& aToB, const PsTransformV& transf0, const PsTransformV& transf1,
 								PersistentContact* manifoldContacts, PxU32& numContacts, ContactBuffer& contactBuffer, PersistentContactManifold& manifold, Vec3V& normal, const Vec3VArg closest,
-								const FloatVArg boxMargin, const FloatVArg contactDist, const bool doOverlapTest, const FloatVArg toleranceScale)
+								const PxReal boxMargin, const FloatVArg contactDist, const bool doOverlapTest, const PxReal toleranceScale)
 {
 
 	PolygonalData polyData;
@@ -112,7 +112,8 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
 
 	const PxU32 initialContacts = manifold.mNumContacts;
 
-	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents, toleranceLength);
 	
 	const FloatV minMargin = FMin(boxMargin, capsuleRadius);
 
@@ -124,7 +125,6 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
 
 	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
 
-	PX_UNUSED(bLostContacts);
 	if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))	
 	{
 
@@ -139,8 +139,6 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
 		const PsMatTransformV aToB(curRTrans);
 		
 		BoxV box(transf1.p, boxExtents);
-		box.setMargin(zero);
-		
 		//transform capsule into the local space of box
 		CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
 		LocalConvex<CapsuleV> convexA(capsule);
@@ -159,7 +157,7 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
 		else if(status == GJK_DEGENERATE)
 		{
 			return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents,  aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer,
-				manifold, normal, closestB, box.getMargin(), contactDist, true, FLoad(params.mToleranceLength));
+				manifold, normal, closestB, box.getMarginF(), contactDist, true, params.mToleranceLength);
 		}
 		else 
 		{
@@ -199,7 +197,7 @@ bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
 			if(initialContacts == 0 || bLostContacts || doOverlapTest)
 			{
 				return fullContactsGenerationCapsuleBox(capsule, box, shapeBox.halfExtents,  aToB, transf0, transf1, manifoldContacts, numContacts, contactBuffer, manifold, normal, 
-					closestB, box.getMargin(), contactDist, doOverlapTest, FLoad(params.mToleranceLength));
+					closestB, box.getMarginF(), contactDist, doOverlapTest, params.mToleranceLength);
 			}
 			else
 			{
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
index 1daf1bd6..5079b32a 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
@@ -50,7 +50,7 @@ namespace Gu
 
 static bool fullContactsGenerationCapsuleConvex(const CapsuleV& capsule, const ConvexHullV& convexHull,  const PsMatTransformV& aToB, const PsTransformV& transf0,const PsTransformV& transf1,
 								PersistentContact* manifoldContacts, ContactBuffer& contactBuffer, const bool idtScale, PersistentContactManifold& manifold, Vec3VArg normal, 
-								const Vec3VArg closest, const FloatVArg tolerance, const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const FloatVArg toleranceScale)
+								const Vec3VArg closest, const PxReal tolerance, const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale)
 {
 
 	PX_UNUSED(renderOutput);
@@ -122,8 +122,8 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
 	const PsTransformV curRTrans(transf1.transformInv(transf0));
 	const PsMatTransformV aToB(curRTrans);
 	
-
-	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
+	const PxReal toleranceLength = params.mToleranceLength ;
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
 	const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius);
 	const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
 	
@@ -150,7 +150,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
 		manifold.setRelativeTransform(curRTrans);
 		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);  
 		ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
-		convexHull.setMargin(zero);
+		convexHull.setMargin(0.f);
 	
 		//transform capsule(a) into the local space of convexHull(b)
 		CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
@@ -181,7 +181,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
 		else if(status == GJK_DEGENERATE)
 		{
 			return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, 
-				closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength));
+				closestB, convexHull.getMarginF(), contactDist, true, renderOutput, params.mToleranceLength);
 		}
 		else 
 		{
@@ -242,7 +242,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
 			if(initialContacts == 0 || bLostContacts || doOverlapTest)
 			{
 				return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, 
-					closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
+					closestB, convexHull.getMarginF(), contactDist, doOverlapTest, renderOutput, params.mToleranceLength);
 			}
 			else
 			{
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
index 82636e10..10e014a5 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
@@ -40,6 +40,7 @@
 #include "GuPCMContactGen.h"
 #include "GuContactBuffer.h"
 
+
 namespace physx
 {
 
@@ -52,7 +53,7 @@ namespace Gu
 static bool fullContactsGenerationConvexConvex(const ConvexHullV& convexHull0, Gu::ConvexHullV& convexHull1, 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 FloatVArg toleranceScale)
+											   const FloatVArg contactDist, const bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale)
 {
 	Gu::PolygonalData polyData0, polyData1;
 	getPCMConvexData(convexHull0, idtScale0, polyData0);
@@ -70,14 +71,14 @@ static bool fullContactsGenerationConvexConvex(const ConvexHullV& convexHull0, G
 
 	PxU32 numContacts = 0;
 
-	if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMargin(), 
-		convexHull1.getMargin(), doOverlapTest, renderOutput, toleranceScale))
+	if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMarginF(), 
+		convexHull1.getMarginF(), doOverlapTest, renderOutput, toleranceScale))
 	{
 		
 		if (numContacts > 0)
 		{
 			//reduce contacts
-			manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+			manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceScale);
 
 			const Vec3V worldNormal = manifold.getWorldNormal(transf1);
 
@@ -185,6 +186,7 @@ static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull0, Gu::ShrunkConv
 
 			//Add contact to manifold
 			manifold.addManifoldPoint(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+
 		}
 		else
 		{
@@ -220,11 +222,12 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 	const Gu::ConvexHullData* hullData0 = shapeConvex0.hullData;
 	const Gu::ConvexHullData* hullData1 = shapeConvex1.hullData;
 
-	const FloatV convexMargin0 = Gu::CalculatePCMConvexMargin(hullData0, vScale0);
-	const FloatV convexMargin1 = Gu::CalculatePCMConvexMargin(hullData1, vScale1);
+	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));
 	
@@ -233,12 +236,15 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 	//ML: after refreshContactPoints, we might lose some contacts
 	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
 
-	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin))
+	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin) )
 	{
+
 		GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
 
 		const bool idtScale0 = shapeConvex0.scale.isIdentity();
 		const bool idtScale1 = shapeConvex1.scale.isIdentity();
+		const float maxMargin0 = shapeConvex0.maxMargin;
+		const float maxMargin1 = shapeConvex1.maxMargin;
 		const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x);
 		const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x);
 		const Vec3V zeroV = V3Zero();
@@ -246,7 +252,10 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 
 		Gu::ShrunkConvexHullV convexHull0(hullData0, V3LoadU(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0);
 		Gu::ShrunkConvexHullV convexHull1(hullData1, V3LoadU(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1);
-	
+
+		convexHull0.setMaxMargin(maxMargin0);
+		convexHull1.setMaxMargin(maxMargin1);
+
 		Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
 		FloatV penDep =  FZero();
 
@@ -266,7 +275,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 		if(status == GJK_DEGENERATE)
 		{
 			return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, 
-				manifold, normal, closestA, closestB, contactDist, true, renderOutput, FLoad(params.mToleranceLength));
+				manifold, normal, closestA, closestB, contactDist, true, renderOutput, params.mToleranceLength);
 		}
 		else if(status == GJK_NON_INTERSECT)
 		{
@@ -281,6 +290,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 			const bool doOverlapTest = addGJKEPAContacts(convexHull0, convexHull1, aToB,
 				status, manifoldContacts, replaceBreakingThreshold, closestA, closestB, normal, penDep, manifold);
 			
+			//manifold.drawManifold(*renderOutput, transf0, transf1);
 			//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,
@@ -289,7 +299,7 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 			if (fullContactGen  || doOverlapTest)
 			{
 				return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, 
-					manifold, normal, closestA, closestB, contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
+					manifold, normal, closestA, closestB, contactDist, doOverlapTest, renderOutput, params.mToleranceLength);
 			}
 			else
 			{
@@ -316,7 +326,5 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 	return false;
 	
 }
-
-
 }
 }
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
index 8ba4e2d7..979d75be 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
@@ -208,11 +208,9 @@ bool Gu::pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS)
 	const bool idtScaleConvex = getPCMConvexData(shape0, convexScaling, hullAABB, polyData);
 
 	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
-	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
-	const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON);
-	const FloatV toleranceLength = FLoad(params.mToleranceLength);
-	const FloatV toleranceMargin = FMul(epsilon, toleranceLength);
-	const FloatV minMargin = FMin(convexMargin, toleranceMargin);
+	
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV minMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
 
 	const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
 	Gu::ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, shapeConvex.scale.isIdentity());
@@ -249,12 +247,9 @@ bool Gu::pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS)
 	const Vec3V p0 = V3LoadA(&transform0.p.x);
 
 	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
-	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
 
-	const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON);
-	const FloatV toleranceLength = FLoad(params.mToleranceLength);
-	const FloatV toleranceMargin = FMul(epsilon, toleranceLength);
-	const FloatV minMargin = FMin(boxMargin, toleranceMargin);
+	const PxReal toranceLength = params.mToleranceLength;
+	const FloatV minMargin = Gu::CalculatePCMBoxMargin(boxExtents, toranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
 
 	Gu::BoxV boxV(V3Zero(), boxExtents);
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp
index ca8f9028..ea12eb4e 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp
@@ -192,19 +192,15 @@ bool Gu::pcmContactConvexMesh(GU_CONTACT_METHOD_ARGS)
 	PxBounds3 hullAABB;
 	PolygonalData polyData;
 	const bool idtScaleConvex = getPCMConvexData(shape0, convexScaling, hullAABB, polyData);
-
-	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
-	const FloatV convexMargin = CalculatePCMConvexMargin(hullData, vScale);
 	const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
 
-	const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON);
-	const FloatV toleranceLength = FLoad(params.mToleranceLength);
-	const FloatV toleranceMargin = FMul(epsilon, toleranceLength);
-	const FloatV minMargin = FMin(convexMargin, toleranceMargin);
+	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	
+	const PxReal toleranceScale = params.mToleranceLength;
+	const FloatV minMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceScale, GU_PCM_MESH_MANIFOLD_EPSILON);
 	
 	ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex);
 
-
 	if(idtScaleConvex)
 	{
 		SupportLocalImpl<Gu::ConvexHullNoScaleV> convexMap(static_cast<ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, true);
@@ -240,11 +236,8 @@ bool Gu::pcmContactBoxMesh(GU_CONTACT_METHOD_ARGS)
 	Cm::FastVertex2ShapeScaling idtScaling;
 
 	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
-	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
-	const FloatV epsilon = FLoad(GU_PCM_MESH_MANIFOLD_EPSILON);
-	const FloatV toleranceLength = FLoad(params.mToleranceLength);
-	const FloatV toleranceMargin = FMul(epsilon, toleranceLength);
-	const FloatV minMargin = FMin(boxMargin, toleranceMargin);
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV minMargin = Gu::CalculatePCMBoxMargin(boxExtents, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);
 
 	BoxV boxV(V3Zero(), boxExtents);
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h
index 0d7e3c73..31c34142 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h
@@ -46,15 +46,15 @@ namespace Gu
 	//full contact gen code for box/convexhull vs convexhull
 	bool generateFullContactManifold(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1, Gu::SupportLocal* map0, Gu::SupportLocal* map1, Gu::PersistentContact* manifoldContacts, 
 		PxU32& numContacts, const Ps::aos::FloatVArg contactDist, const Ps::aos::Vec3VArg normal, const Ps::aos::Vec3VArg closestA, const Ps::aos::Vec3VArg closestB, 
-		const Ps::aos::FloatVArg toleranceA, const Ps::aos::FloatVArg toleranceB, bool doOverlapTest, Cm::RenderOutput* renderOutput, const Ps::aos::FloatVArg toleranceScale);
+		const PxReal toleranceA, const PxReal toleranceB, bool doOverlapTest, Cm::RenderOutput* renderOutput, const PxReal toleranceScale);
 
 	//full contact gen code for capsule vs convexhulll
 	bool generateFullContactManifold(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, const Ps::aos::PsMatTransformV& aToB,  Gu::PersistentContact* manifoldContacts, 
-		PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg tolerance, bool doOverlapTest, const Ps::aos::FloatVArg toleranceScale);
+		PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const PxReal tolerance, bool doOverlapTest, const PxReal toleranceScale);
 
 	//full contact gen code for capsule vs box
 	bool generateCapsuleBoxFullContactManifold(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, const Ps::aos::PsMatTransformV& aToB, Gu::PersistentContact* manifoldContacts, PxU32& numContacts,
-		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg boxMargin, const bool doOverlapTest, const Ps::aos::FloatVArg toeranceScale);
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const Ps::aos::Vec3VArg closest, const PxReal boxMargin, const bool doOverlapTest, const PxReal toeranceScale);
 
 	//MTD code for box/convexhull vs box/convexhull
 	bool computeMTD(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1,  SupportLocal* map0, SupportLocal* map1, Ps::aos::FloatV& penDepth, Ps::aos::Vec3V& normal);
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
index de5d3b1b..5b593869 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
@@ -566,8 +566,8 @@ namespace Gu
 	}
 
 	bool generateFullContactManifold(PolygonalData& polyData0, PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1,  PersistentContact* manifoldContacts, PxU32& numContacts,
-		const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg marginA, const FloatVArg marginB, const bool doOverlapTest, 
-		Cm::RenderOutput* renderOutput, const Ps::aos::FloatVArg toleranceScale)
+		const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const PxReal marginA, const PxReal marginB, const bool doOverlapTest, 
+		Cm::RenderOutput* renderOutput, const PxReal toleranceLength)
 	{
 	
 		const PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
@@ -657,19 +657,50 @@ EdgeTest:
 		}
 		else
 		{
-			const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE);
-			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS));
-			const FloatV toleranceA = FMax(FMul(marginA, eps), lowerEps);
-			
-			const FloatV toleranceB = FMax(FMul(marginB, eps), lowerEps);
+			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);
 
-			const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, V3Neg(normal), closestB, toleranceB);
-			const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, transform0To1V.rotateInv(normal), transform0To1V.transformInv(closestA), toleranceA);
+			const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, negNormal, closestB, toleranceB);
+			const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, normalIn0, transform0To1V.transformInv(closestA),
+				toleranceA);
 
 			const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
 			const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];
-			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
-			
+
+			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;
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
index 188ea3eb..ae732263 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
@@ -306,7 +306,7 @@ namespace Gu
 
 
 	bool generateCapsuleBoxFullContactManifold(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
-		const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const FloatVArg margin, const bool doOverlapTest, const FloatVArg toleranceScale)
+		const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const PxReal margin, const bool doOverlapTest, const PxReal toleranceScale)
 	{
 		
 		const PxU32 originalContacts = numContacts;
@@ -324,11 +324,12 @@ namespace Gu
 		}
 		else
 		{
-			const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE);
-			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS));
-			const FloatV tolerance = FMax(FMul(margin, eps), lowerEps);
+			const PxReal lowerEps = toleranceScale * PCM_WITNESS_POINT_LOWER_EPS;
+			const PxReal upperEps = toleranceScale * PCM_WITNESS_POINT_UPPER_EPS;
+			const PxReal tolerance = PxClamp(margin, lowerEps, upperEps);
 
-			referencePolygon = &polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance)];
+			const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance);
+			referencePolygon = &polyData.mPolygons[featureIndex];
 			
 		}
 
@@ -344,7 +345,7 @@ namespace Gu
 
 		//capsule is in the local space of polyData
 	bool generateFullContactManifold(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts,
-		const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const FloatVArg margin, const bool doOverlapTest, const Ps::aos::FloatVArg toleranceScale)
+		const FloatVArg contactDist, Vec3V& normal, const Vec3VArg closest, const PxReal margin, const bool doOverlapTest, const PxReal toleranceLength)
 	{
 		const PxU32 originalContacts = numContacts;
 
@@ -374,11 +375,13 @@ namespace Gu
 			const PxU32 faceContacts = numContacts - originalContacts;
 			if(faceContacts < 2)
 			{
-				const FloatV eps = FLoad(PCM_WITNESS_POINT_SCALE);
-				const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_LOWER_EPS));
-				const FloatV toleranceA = FMax(FMul(margin, eps), lowerEps);
+			
+				const PxReal lowerEps = toleranceLength * PCM_WITNESS_POINT_LOWER_EPS;
+				const PxReal upperEps = toleranceLength * PCM_WITNESS_POINT_UPPER_EPS;
+				const PxReal tolerance = PxClamp(margin, lowerEps, upperEps);
 
-				const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, toleranceA)];
+				const PxU32 featureIndex = getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, tolerance);
+				const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[featureIndex];
 				generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB,  manifoldContacts, numContacts, contactDist, tNormal);
 			}
 		}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
index 8469481e..5171a4fc 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
@@ -36,10 +36,10 @@
 #include "GuVecCapsule.h"
 #include "GuConvexSupportTable.h"
 
-//Percentage of margin
-#define	PCM_WITNESS_POINT_SCALE			1e-1f
 //The smallest epsilon we will permit (scaled by PxTolerancesScale.length)
-#define	PCM_WITNESS_POINT_LOWER_EPS		1e-3f
+#define	PCM_WITNESS_POINT_LOWER_EPS		1e-2f
+//The largest epsilon we will permit (scaled by PxTolerancesScale.length)
+#define	PCM_WITNESS_POINT_UPPER_EPS		5e-2f
 
 namespace physx
 {
@@ -273,39 +273,73 @@ namespace Gu
 
 	}
 
-	PX_FORCE_INLINE PxU32 getWitnessPolygonIndex(const Gu::PolygonalData& polyData,  SupportLocal* map, const Ps::aos::Vec3VArg normal,
-		const Ps::aos::Vec3VArg closest, const Ps::aos::FloatVArg tolerance)
+	PX_FORCE_INLINE PxU32 getWitnessPolygonIndex(const Gu::PolygonalData& polyData, SupportLocal* map, const Ps::aos::Vec3VArg normal,
+		const Ps::aos::Vec3VArg closest, const PxReal tolerance)
 	{
 		using namespace Ps::aos;
 
+		PxReal pd[256];
+		//first pass : calculate the smallest distance from the closest point to the polygon face
+
 		//transform the closest p to vertex space
 		const Vec3V p = M33MulV3(map->shape2Vertex, closest);
 		PxU32 closestFaceIndex = 0;
-		Vec4V plane = V4LoadU(&polyData.mPolygons[0].mPlane.n.x);
-		Vec3V n = Vec3V_From_Vec4V(plane);
-		Vec3V bestN = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
-		FloatV bestProj = V3Dot(bestN, normal);
-		FloatV d = V4GetW(plane);
-		FloatV pd = FAbs(FAdd(d, V3Dot(n, p))); 
-		FloatV minDist = pd;
 
-		for(PxU32 i=1; i< polyData.mNbPolygons; ++i)
+		PxVec3 closestP;
+		V3StoreU(p, closestP);
+
+		const PxReal eps = -tolerance;
+
+		PxPlane plane = polyData.mPolygons[0].mPlane;
+		PxReal dist = plane.distance(closestP);
+		PxReal minDist = dist >= eps ? dist : PX_MAX_F32;
+		pd[0] = minDist;
+		PxReal maxDist = dist;
+		PxU32 maxFaceIndex = 0;
+
+		for (PxU32 i = 1; i < polyData.mNbPolygons; ++i)
+		{
+			plane = polyData.mPolygons[i].mPlane;
+			dist = plane.distance(closestP);
+			pd[i] = dist >= eps ? dist : PX_MAX_F32;
+			if (minDist > pd[i])
+			{
+				minDist = pd[i];
+				closestFaceIndex = i;
+			}
+			if (dist > maxDist)
+			{
+				maxDist = dist;
+				maxFaceIndex = i;
+			}
+		}
+
+		if (minDist == PX_MAX_F32)
+			return maxFaceIndex;
+
+		//second pass : select the face which has the normal most close to the gjk/epa normal
+		Vec4V plane4 = V4LoadU(&polyData.mPolygons[closestFaceIndex].mPlane.n.x);
+		Vec3V n = Vec3V_From_Vec4V(plane4);
+		n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
+		FloatV bestProj = V3Dot(n, normal);
+
+		const PxU32 firstPassIndex = closestFaceIndex;
+
+		for (PxU32 i = 0; i< polyData.mNbPolygons; ++i)
 		{
-			plane = V4LoadU(&polyData.mPolygons[i].mPlane.n.x);
-			n = Vec3V_From_Vec4V(plane);
-			d = V4GetW(plane);
-			pd = FAbs(FAdd(d, V3Dot(n, p))); 
-		
 			//if the difference between the minimum distance and the distance of p to plane i is within tolerance, we use the normal to chose the best plane 
-			if (FAllGrtrOrEq(tolerance, FSub(pd, minDist)))
+			if ((tolerance >(pd[i] - minDist)) && (firstPassIndex != i))
 			{
-				//rotate the plane normal to shape space
+				plane4 = V4LoadU(&polyData.mPolygons[i].mPlane.n.x);
+				n = Vec3V_From_Vec4V(plane4);
+				//rotate the plane normal to shape space. We can roate normal into the vertex space. The reason for
+				//that is because it will lose some numerical precision if the object has large scale and this algorithm
+				//will choose different face
 				n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
 				FloatV proj = V3Dot(n, normal);
 
-				if(FAllGrtr(bestProj, proj))
+				if (FAllGrtr(bestProj, proj))
 				{
-					minDist= pd;
 					closestFaceIndex = i;
 					bestProj = proj;
 				}
@@ -315,7 +349,6 @@ namespace Gu
 		return closestFaceIndex;
 	}
 
-
 	//ML: this function is shared by the sphere/capsule vs convex hulls full contact gen, capsule in the local space of polyData
 	PX_FORCE_INLINE bool testPolyDataAxis(const Gu::CapsuleV& capsule, const Gu::PolygonalData& polyData, SupportLocal* map,  const Ps::aos::FloatVArg contactDist, Ps::aos::FloatV& minOverlap, Ps::aos::Vec3V& separatingAxis)
 	{
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp
index be08d46d..308cc0c8 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp
@@ -65,7 +65,8 @@ bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS)
 
 	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
 
-	const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents);
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents, toleranceLength);
 	const FloatV projectBreakingThreshold = FMul(boxMargin, FLoad(0.2f));
 	const PxU32 initialContacts = manifold.mNumContacts;
 	
@@ -74,8 +75,6 @@ bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS)
 	const PxU32 newContacts = manifold.mNumContacts;
 	const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
 
-	//PX_UNUSED(bLostContacts);
-	//if(bLostContacts || manifold.invalidate_BoxConvex(curTransf, boxMargin))
 	if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, boxMargin, FLoad(0.2f)))
 	{
 		//ML:localNormal is the local space of plane normal, however, because shape1 is box and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp
index d0d3daec..215e43c7 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp
@@ -60,14 +60,14 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
 	
 	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
 	const Gu::ConvexHullData* hullData = shapeConvex.hullData;
-	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
-	
 
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
+	
 	//in world space
 	const Vec3V planeNormal = V3Normalize(QuatGetBasisVector0(transf1.q));
 	const Vec3V negPlaneNormal = V3Neg(planeNormal);
 	
-
 	const FloatV contactDist = FLoad(params.mContactDistance);
 
 	//const FloatV replaceBreakingThreshold = FMul(convexMargin, FLoad(0.001f));
@@ -80,8 +80,6 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
 	const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
 
 	
-	PX_UNUSED(bLostContacts);
-	//if(bLostContacts || manifold.invalidate_BoxConvex(curTransf, convexMargin))
 	if(bLostContacts || manifold.invalidate_PrimitivesPlane(curTransf, convexMargin, FLoad(0.2f)))
 	{
 		const PsMatTransformV aToB(curTransf);
@@ -126,7 +124,7 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
 				if(numContacts >= Gu::ContactBuffer::MAX_CONTACTS)
 				{
 					//ML: number of contacts are more than MAX_CONTACTS, we need to force contact reduction
-					manifold.reduceBatchContacts(manifoldContacts, numContacts);
+					manifold.reduceBatchContacts(manifoldContacts, numContacts, toleranceLength);
 					numContacts = GU_MANIFOLD_CACHE_SIZE;
 					for(PxU32 j=0; j<GU_MANIFOLD_CACHE_SIZE; ++j)
 					{
@@ -137,7 +135,7 @@ bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
 		}
 
 		//reduce contacts
-		manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+		manifold.addBatchManifoldContacts(manifoldContacts, numContacts, toleranceLength);
 		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
 #if	PCM_LOW_LEVEL_DEBUG
 		manifold.drawManifold(*renderOutput, transf0, transf1);
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp
index 10358836..5efd3e98 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp
@@ -136,8 +136,8 @@ bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS)
 	const PsTransformV curRTrans(transf1.transformInv(transf0));
 	const PsMatTransformV aToB(curRTrans);
 	
-	
-	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
+	const PxReal toleranceLength = params.mToleranceLength;
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale, toleranceLength);
 
 	const PxU32 initialContacts = manifold.mNumContacts;
 	const FloatV minMargin = FMin(convexMargin, sphereRadius);
@@ -169,7 +169,7 @@ bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS)
 		const bool idtScale = shapeConvex.scale.isIdentity();
 		//use the original shape
 		ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
-		convexHull.setMargin(zero);
+		convexHull.setMargin(0.f);
 		//transform capsule into the local space of convexHull
 		CapsuleV capsule(aToB.p, sphereRadius);
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
index fde67300..7a0e68a2 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
@@ -689,23 +689,26 @@ namespace physx
 			points1In0[i] = V3SetZ(points1In0[i], d);
 			iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
 			iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
-			if(FAllGrtr(rd, z))
+
+			bool penetrated = false;
+			
+			if (FAllGrtr(rd, z))
 			{
-				points1In0Penetration[i] = true;
+				penetrated = true;
 
-				if(contains(points0In0, 3, points1In0[i], rPolygonMin, rPolygonMax))
+				if (contains(points0In0, 3, points1In0[i], rPolygonMin, rPolygonMax))
 				{
 					inside++;
-					
+
 					//add this contact to the buffer
 					const FloatV t = V3Dot(contactNormal, V3Sub(triangle.verts[0], vert1));
-					const Vec3V projectPoint = V3ScaleAdd(contactNormal, t, vert1); 
+					const Vec3V projectPoint = V3ScaleAdd(contactNormal, t, vert1);
 					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), FNeg(t));
 					numManifoldContacts = addMeshContacts(manifoldContacts, vert1, projectPoint, localNormalPen, triangleIndex, numManifoldContacts);
-					
+
 					//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
 					const PxU32 numContacts = numManifoldContacts - previousContacts;
-					if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
+					if (numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
 					{
 						//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
 						//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
@@ -714,7 +717,8 @@ namespace physx
 					}
 				}
 			}
-			
+
+			points1In0Penetration[i] = penetrated;
 		}
 
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp
index f93c703e..9ce3e9c0 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp
@@ -860,7 +860,8 @@ void Gu::PersistentContactManifold::addManifoldContactsToContactBuffer(Gu::Conta
 	GU_MANIFOLD_CACHE_SIZE, we will need to do contact reduction while we are storing the chosen manifold contacts from the manifold contact list to the manifold contact
 	buffer.
 */
-void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentContact* manifoldContacts, const PxU32 numPoints)
+void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentContact* manifoldContacts, 
+	const PxU32 numPoints, const PxReal toleranceLength)
 {
 
 	if(numPoints <= GU_MANIFOLD_CACHE_SIZE)
@@ -875,7 +876,7 @@ void Gu::PersistentContactManifold::addBatchManifoldContacts(const PersistentCon
 	}
 	else
 	{
-		reduceBatchContacts(manifoldContacts, numPoints);
+		reduceBatchContacts(manifoldContacts, numPoints, toleranceLength);
 		mNumContacts = GU_MANIFOLD_CACHE_SIZE;
 	}
 }  
@@ -967,12 +968,16 @@ void Gu::PersistentContactManifold::reduceBatchContactsCluster(const PersistentC
 
 
 	maxDist = nmax;
-	index = PxU32(-1);
+	index = GU_MANIFOLD_INVALID_INDEX;
 	v = V3Sub(mContactPoints[1].mLocalPointB, mContactPoints[0].mLocalPointB);
-	Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen)));
+
+	const Vec3V cn0 = Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen);
+	Vec3V norm = V3Cross(v, cn0);
+	const FloatV sqLen = V3Dot(norm, norm);
+	norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(norm, FSqrt(sqLen)), cn0);
 
 	FloatV minDist = max;
-	PxU32 index1 = PxU32(-1);
+	PxU32 index1 = GU_MANIFOLD_INVALID_INDEX;
 	
 
 	//calculate the min and max point away from the segment
@@ -1066,112 +1071,179 @@ void Gu::PersistentContactManifold::reduceBatchContactsCluster(const PersistentC
 /*
 	This function is for box/convexhull full contact generation. If the numPoints > 4, we will reduce the contact points to 4
 */
-void Gu::PersistentContactManifold::reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints)
+void Gu::PersistentContactManifold::reduceBatchContacts(const PersistentContact* manifoldPoints,
+	const PxU32 numPoints, const PxReal tolereanceLength)
 {
 	using namespace Ps::aos;
-	
-	bool chosen[64];
-	physx::PxMemZero(chosen, sizeof(bool)*numPoints);
+
+	PxU8 chosenIndices[4];
+	PxU8 candidates[64];
+
+	const FloatV zero = FZero();
 	const FloatV max = FMax();
 	const FloatV nmax = FNeg(max);
-	FloatV maxDist = V4GetW(manifoldPoints[0].mLocalNormalPen);
-	PxI32 index = 0;
-	//keep the deepest point
-	for(PxU32 i=1; i<numPoints; ++i)
+	FloatV maxPen = V4GetW(manifoldPoints[0].mLocalNormalPen);
+	FloatV minPen = nmax;
+	PxU32 index = 0;
+	candidates[0] = 0;
+	PxU32 candidateIndex = 0;
+
+	PxU32 nbCandiates = numPoints;
+	//keep the deepest point, candidateIndex will be the same as index
+	for (PxU32 i = 1; i<numPoints; ++i)
 	{
+		//at the begining candidates and indices will be the same
+		candidates[i] = PxU8(i);
+
 		const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
-		if(FAllGrtr(maxDist, pen))
+		minPen = FMax(minPen, pen);
+		if (FAllGrtr(maxPen, pen))
 		{
-			maxDist = pen;
-			index = PxI32(i);
+			maxPen = pen;
+			index = i;
+			candidateIndex = i;
 		}
 	}
+
 	//keep the deepest points in the first position
-	mContactPoints[0] =  manifoldPoints[index];
-	chosen[index] = true;
+	chosenIndices[0] = PxU8(index);
 
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
+	//indices[index] = nbCandiates;
 
 	//calculate the furthest away points
-	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, mContactPoints[0].mLocalPointB);
-	maxDist = V3Dot(v, v);
-	index = 0;
+	Vec3V v = V3Sub(manifoldPoints[candidates[0]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
+	FloatV maxDist = V3Dot(v, v);
+	index = candidates[0];
+	candidateIndex = 0;
 
-	for(PxU32 i=1; i<numPoints; ++i)
+	for (PxU32 i = 1; i<nbCandiates; ++i)
 	{
-		v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+		v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
 		const FloatV d = V3Dot(v, v);
-		if(FAllGrtr(d, maxDist))
+		if (FAllGrtr(d, maxDist))
 		{
 			maxDist = d;
-			index = PxI32(i);
+			index = candidates[i];
+			candidateIndex = i;
 		}
 	}
 
-	//PX_ASSERT(chosen[index] == false);
-	mContactPoints[1] =  manifoldPoints[index];
-	chosen[index] = true;
+	chosenIndices[1] = PxU8(index);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
 
+	v = V3Sub(manifoldPoints[chosenIndices[1]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
+	const Vec3V cn0 = Vec3V_From_Vec4V(manifoldPoints[chosenIndices[0]].mLocalNormalPen);
+	Vec3V norm = V3Cross(v, cn0);
+	const FloatV sqLen = V3Dot(norm, norm);
+	norm = V3Sel(FIsGrtr(sqLen, zero), V3ScaleInv(norm, FSqrt(sqLen)), cn0);
 
+	//reset maxDist and index
 	maxDist = nmax;
-	index = -1;
-
-	v = V3Sub(mContactPoints[1].mLocalPointB, mContactPoints[0].mLocalPointB);
-	const Vec3V vCross = V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen));
-	const FloatV sqLen = V3Dot(vCross, vCross);
-	const Vec3V norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(vCross, FSqrt(sqLen)), V3Zero());
-
+	index = GU_MANIFOLD_INVALID_INDEX;
+	candidateIndex = GU_MANIFOLD_INVALID_INDEX;
 	FloatV minDist = max;
-	PxI32 index1 = -1;
-	
+	PxU32 index1 = GU_MANIFOLD_INVALID_INDEX;
+	PxU32 candidateIndex1 = GU_MANIFOLD_INVALID_INDEX;
+
 
 	//calculate the min and max point away from the segment
-	for(PxU32 i=0; i<numPoints; ++i)
+	for (PxU32 i = 0; i<nbCandiates; ++i)
 	{
-		if(!chosen[i])
+		v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
+		const FloatV d = V3Dot(v, norm);
+		if (FAllGrtr(d, maxDist))
 		{
-			v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
-			const FloatV d = V3Dot(v, norm);
-			if(FAllGrtr(d, maxDist))
-			{
-				maxDist = d;
-				index = PxI32(i);
-			}
+			maxDist = d;
+			index = candidates[i];
+			candidateIndex = i;
+		}
 
-			if(FAllGrtr(minDist, d))
-			{
-				minDist = d;
-				index1 = PxI32(i);
-			}
+		if (FAllGrtr(minDist, d))
+		{
+			minDist = d;
+			index1 = candidates[i];
+			candidateIndex1 = i;
 		}
+
 	}
 
-	mContactPoints[2] =  manifoldPoints[index];
-	chosen[index] = true;
+
+	chosenIndices[2] = PxU8(index);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
+	if (nbCandiates == candidateIndex1)
+		candidateIndex1 = candidateIndex;
 
 	//if min and max in the same side, chose again
 	const FloatV temp = FMul(minDist, maxDist);
-	if(FAllGrtr(temp, FZero()))
+	if (FAllGrtr(temp, zero))
 	{
 		//chose again
 		maxDist = nmax;
-		for(PxU32 i=0; i<numPoints; ++i)
+		for (PxU32 i = 0; i < nbCandiates; ++i)
 		{
-			if(!chosen[i])
+			v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
+			const FloatV d = V3Dot(v, norm);
+			if (FAllGrtr(d, maxDist))
 			{
-				v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
-				const FloatV d = V3Dot(v, norm);
-				if(FAllGrtr(d, maxDist))
+				maxDist = d;
+				index1 = candidates[i];
+				candidateIndex1 = i;
+			}
+		}
+
+	}
+
+	chosenIndices[3] = PxU8(index1);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex1] = candidates[nbCandiates];
+
+	const FloatV eps = FLoad(tolereanceLength * 0.02f);
+	const BoolV con = BAnd(FIsGrtr(eps, maxPen), FIsGrtr(minPen, eps));
+	if (BAllEqTTTT(con))
+	{
+		//post process 
+		for (PxU32 i = 0; i < 4; ++i)
+		{
+			FloatV pen = V4GetW(manifoldPoints[chosenIndices[i]].mLocalNormalPen);
+
+			if (FAllGrtr(pen, eps))
+			{
+				candidateIndex = GU_MANIFOLD_INVALID_INDEX;
+				for (PxU32 j = 0; j < nbCandiates; ++j)
 				{
-					maxDist = d;
-					index1 = PxI32(i);
+					const FloatV pen1 = V4GetW(manifoldPoints[candidates[j]].mLocalNormalPen);
+					if (FAllGrtr(pen, pen1) && FAllGrtr(eps, pen1))
+					{
+						pen = pen1;
+						candidateIndex = j;
+					}
+				}
+				if (candidateIndex < nbCandiates)
+				{
+					const PxU8 originalIndex = chosenIndices[i];
+					chosenIndices[i] = candidates[candidateIndex];
+					candidates[candidateIndex] = originalIndex;
 				}
 			}
+			mContactPoints[i] = manifoldPoints[chosenIndices[i]];
+		}
+	}
+	else
+	{
+		for (PxU32 i = 0; i < 4; ++i)
+		{
+			mContactPoints[i] = manifoldPoints[chosenIndices[i]];
 		}
 	}
-
-	mContactPoints[3] = manifoldPoints[index1];
 }
-
 /*
 	This function is for capsule full contact generation. If the numPoints > 2, we will reduce the contact points to 2
 */
@@ -1540,7 +1612,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 	const FloatV max = FMax();
 	const FloatV nmax = FNeg(max);
 	FloatV maxDis = nmax;
-	PxI32 index = -1;
+	PxU32 index = GU_MANIFOLD_INVALID_INDEX;
 
 	PCMContactPatch* currentPatch = &patch;
 	while(currentPatch)
@@ -1552,7 +1624,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 			if(FAllGrtr(v, maxDis))
 			{
 				maxDis = v;
-				index = PxI32(i);
+				index = i;
 			}
 		}
 		currentPatch = currentPatch->mNextPatch;
@@ -1568,7 +1640,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 	//calculate the furthest away points
 	Vec3V v = V3Sub(manifoldContactExt[patch.mStartIndex].mLocalPointB, contact0);
 	maxDis = V3Dot(v, v);
-	index = PxI32(patch.mStartIndex);
+	index = patch.mStartIndex;
 
 	currentPatch = &patch;
 	while(currentPatch)
@@ -1580,7 +1652,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 			if(FAllGrtr(d, maxDis))
 			{
 				maxDis = d;
-				index = PxI32(i);
+				index = i;
 			}
 		}
 		currentPatch = currentPatch->mNextPatch;
@@ -1594,11 +1666,14 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
 
 	maxDis = nmax;
-	index = -1;
+	index = GU_MANIFOLD_INVALID_INDEX;
 	v = V3Sub(contact1, contact0);
-	Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen)));
+	const Vec3V cn0 = Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen);
+	Vec3V norm = V3Cross(v, cn0);
+	const FloatV sqLen = V3Dot(norm, norm);
+	norm = V3Sel(FIsGrtr(sqLen, FZero()), V3ScaleInv(norm, FSqrt(sqLen)), cn0);
 	FloatV minDis = max;
-	PxI32 index1 = -1;
+	PxU32 index1 = GU_MANIFOLD_INVALID_INDEX;
 	
 
 	//calculate the point furthest way to the segment
@@ -1615,13 +1690,13 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 				if(FAllGrtr(d, maxDis))
 				{
 					maxDis = d;
-					index = PxI32(i);
+					index = i;
 				}
 
 				if(FAllGrtr(minDis, d))
 				{
 					minDis = d;
-					index1 = PxI32(i);
+					index1 = i;
 				}
 			}
 		}
@@ -1654,7 +1729,7 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 					if(FAllGrtr(d, maxDis))
 					{
 						maxDis = d;
-						index1 = PxI32(i);
+						index1 = i;
 					}
 				}
 			}
@@ -1678,8 +1753,6 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 		inds[a] = 0;
 	}
 
-	
-
 	{
 		currentPatch = &patch;
 
@@ -1714,10 +1787,6 @@ Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(c
 			maxPen = FMin(maxPen, pens[i]);
 		}
 	}
-
-
-
-
 	return maxPen;
 }
 
@@ -1725,125 +1794,163 @@ PxU32 Gu::SinglePersistentContactManifold::reduceContacts(MeshPersistentContact*
 {
 	using namespace Ps::aos;  
 
-	bool* chosen = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*numPoints));
-	physx::PxMemZero(chosen, sizeof(bool)*numPoints);
-	FloatV max = FMax();
-	FloatV maxDis = max;
-	PxU32 index = 0xffffffff;
+	PxU8 chosenIndices[GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE];
+	PxU8* candidates = reinterpret_cast<PxU8*>(PxAlloca(sizeof(PxU8) * numPoints));
+
+	const FloatV max = FMax();
+	const FloatV nmax = FNeg(max);
+	const FloatV zero = FZero();
+	FloatV maxPen = V4GetW(manifoldPoints[0].mLocalNormalPen);
+	PxU32 index = 0;
+	candidates[0] = 0;
+	PxU32 candidateIndex = 0;
+	PxU32 nbCandiates = numPoints;
+
 	MeshPersistentContact newManifold[GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE];
 
 	//keep the deepest point
-	for(PxU32 i=0; i<numPoints; ++i)
+	for(PxU32 i=1; i<numPoints; ++i)
 	{
+		//at the begining candidates and indices will be the same
+		candidates[i] = PxU8(i);
+
 		const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
-		if(FAllGrtr(maxDis, pen))
+		if(FAllGrtr(maxPen, pen))
 		{
-			maxDis = pen;
+			maxPen = pen;
 			index = i;
+			candidateIndex = i;
 		}
 	}
+
 	//keep the deepest points in the first position
-	newManifold[0] = manifoldPoints[index];
-	chosen[index] = true;
+	chosenIndices[0] = PxU8(index);
+
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
 
+	//keep the deepest points in the first position
+	newManifold[0] = manifoldPoints[chosenIndices[0]];
 
 	//calculate the furthest away points
-	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, newManifold[0].mLocalPointB);
-	maxDis = V3Dot(v, v);
-	index = 0;
+	Vec3V v = V3Sub(manifoldPoints[candidates[0]].mLocalPointB, newManifold[0].mLocalPointB);
+	maxPen = V3Dot(v, v);
+	index = candidates[0];
+	candidateIndex = 0;
 
-	for(PxU32 i=1; i<numPoints; ++i)
+	for(PxU32 i=1; i<nbCandiates; ++i)
 	{
-		v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
+		v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, manifoldPoints[chosenIndices[0]].mLocalPointB);
 		const FloatV d = V3Dot(v, v);
-		if(FAllGrtr(d, maxDis))
+		if(FAllGrtr(d, maxPen))
 		{
-			maxDis = d;
-			index = i;
+			maxPen = d;
+			index = candidates[i];
+			candidateIndex = i;
 		}  
 	}
 
-	//PX_ASSERT(chosen[index] == false);
-	newManifold[1] = manifoldPoints[index];
-	chosen[index] = true;
 
+	chosenIndices[1] = PxU8(index);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
+
+	newManifold[1] = manifoldPoints[chosenIndices[1]];
 
-	maxDis = FNeg(max);
-	index = 0xffffffff;
-	v = V3Sub(newManifold[1].mLocalPointB, newManifold[0].mLocalPointB);
-	Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(newManifold[0].mLocalNormalPen)));
-	FloatV minDis = max;
-	PxU32 index1 = 0xffffffff;
 	
+	v = V3Sub(newManifold[1].mLocalPointB, newManifold[0].mLocalPointB);
+	const Vec3V cn0 = Vec3V_From_Vec4V(newManifold[0].mLocalNormalPen);
+	Vec3V norm = V3Cross(v, cn0);
+	const FloatV sqLen = V3Dot(norm, norm);
+	norm = V3Sel(FIsGrtr(sqLen, zero), V3ScaleInv(norm, FSqrt(sqLen)), cn0);
+
+	FloatV maxDist = nmax;
+	FloatV minDist = max;
+	index = GU_MANIFOLD_INVALID_INDEX;
+	PxU32 index1 = GU_MANIFOLD_INVALID_INDEX;
+	PxU32 candidateIndex1 = GU_MANIFOLD_INVALID_INDEX;
 
 	//calculate the point furthest way to the segment
-	for(PxU32 i=0; i<numPoints; ++i)
+	for(PxU32 i=0; i<nbCandiates; ++i)
 	{
-		if(!chosen[i])
+		v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, newManifold[0].mLocalPointB);
+		const FloatV d = V3Dot(v, norm);
+		if(FAllGrtr(d, maxDist))
 		{
-			v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
-			const FloatV d = V3Dot(v, norm);
-			if(FAllGrtr(d, maxDis))
-			{
-				maxDis = d;
-				index = i;
-			}
+			maxDist = d;
+			index = candidates[i];
+			candidateIndex = i;
+		}
 
-			if(FAllGrtr(minDis, d))
-			{
-				minDis = d;
-				index1 = i;
-			}
+		if(FAllGrtr(minDist, d))
+		{
+			minDist = d;
+			index1 = candidates[i];
+			candidateIndex1 = i;
 		}
+		
 	}
 
-	//PX_ASSERT(chosen[index] == false && chosen[index1]== false);
+	chosenIndices[2] = PxU8(index);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
 
-	chosen[index] = true;
-	newManifold[2] = manifoldPoints[index];
+	newManifold[2] = manifoldPoints[chosenIndices[2]];
+
+	if (nbCandiates == candidateIndex1)
+		candidateIndex1 = candidateIndex;
 	
-	const FloatV temp = FMul(minDis, maxDis);
+	const FloatV temp = FMul(minDist, maxDist);
 	if(FAllGrtr(temp, FZero()))
 	{
-		//chose the something further away from newManifold[2] 
-		maxDis = FNeg(max);
-		for(PxU32 i=0; i<numPoints; ++i)
+		//chose again
+		maxDist = nmax;
+		for (PxU32 i = 0; i < nbCandiates; ++i)
 		{
-			if(!chosen[i])
+			v = V3Sub(manifoldPoints[candidates[i]].mLocalPointB, newManifold[0].mLocalPointB);
+			const FloatV d = V3Dot(v, norm);
+			if(FAllGrtr(d, maxDist))
 			{
-				v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
-				const FloatV d = V3Dot(v, norm);
-				if(FAllGrtr(d, maxDis))
-				{
-					maxDis = d;
-					index1 = i;
-				}
+				maxDist = d;
+				index1 = candidates[i];
+				candidateIndex1 = i;
 			}
 		}
-
 	}
 
-	newManifold[3]= manifoldPoints[index1];
-	chosen[index1] = true;
+	chosenIndices[3] = PxU8(index1);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex1] = candidates[nbCandiates];
 
-	maxDis = max;
-	index = 0xffffffff;
+	newManifold[3]= manifoldPoints[chosenIndices[3]];
+
+	maxDist = max;
+	index = GU_MANIFOLD_INVALID_INDEX;
+	candidateIndex = GU_MANIFOLD_INVALID_INDEX;
 	//choose the 5 point, second deepest in the left overlap point
-	for (PxU32 i = 0; i < numPoints; ++i)
+	for (PxU32 i = 0; i < nbCandiates; ++i)
 	{
-		if (!chosen[i])
+		const FloatV pen = V4GetW(manifoldPoints[candidates[i]].mLocalNormalPen);
+		if (FAllGrtr(maxDist, pen))
 		{
-			const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
-			if (FAllGrtr(maxDis, pen))
-			{
-				maxDis = pen;
-				index = i;
-			}
+			maxDist = pen;
+			index = candidates[i];
+			candidateIndex = i;
 		}
+		
 	}
 
-	newManifold[4] = manifoldPoints[index];
-	chosen[index] = true;
+	chosenIndices[4] = PxU8(index);
+	//move the chosen indices out of the candidates indices
+	nbCandiates = nbCandiates - 1;
+	candidates[candidateIndex] = candidates[nbCandiates];
+
+	newManifold[4] = manifoldPoints[chosenIndices[4]];
+
 
 	//copy the new manifold back
 	for(PxU32 i=0; i<GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE; ++i)
@@ -1854,7 +1961,6 @@ PxU32 Gu::SinglePersistentContactManifold::reduceContacts(MeshPersistentContact*
 	return GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
 }
 
-
 Ps::aos::FloatV Gu::SinglePersistentContactManifold::refreshContactPoints(const Ps::aos::PsMatTransformV& aToB, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg /*contactOffset*/)
 {
 	using namespace Ps::aos;
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h
index e06622da..b2491174 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h
@@ -52,6 +52,8 @@ namespace physx
 #define GU_MAX_MANIFOLD_SIZE 6
 #define GU_MESH_CONTACT_REDUCTION_THRESHOLD	16
 
+#define GU_MANIFOLD_INVALID_INDEX	0xffffffff
+
 
 //ML: this is used to compared with the shape's margin to decide the final tolerance used in the manifold to validate the existing contacts.
 //In the case of big shape and relatively speaking small triangles in the mesh, we need to take a smaller margin. This helps because the PCM 
@@ -317,13 +319,13 @@ public:
 	void addBatchManifoldContacts2( const PersistentContact* manifoldPoints, const PxU32 numPoints);//max two points of contacts             
 	
 	//This function is used in the box/convexhull full manifold contact generation(maximum 4 points). 
-	void addBatchManifoldContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints);
+	void addBatchManifoldContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints, const PxReal toleranceLength);
 	//This function is using the cluster algorithm to reduce contacts
 	void reduceBatchContactsCluster(const PersistentContact* manifoldPoints, const PxU32 numPoints);
 	//This function is called by addBatchManifoldContacts2 to reduce the manifold contacts to 2 points;
 	void reduceBatchContacts2(const PersistentContact* manifoldPoints, const PxU32 numPoints);
 	//This function is called by addBatchManifoldContacts to reduce the manifold contacts to 4 points
-	void reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints);
+	void reduceBatchContacts(const PersistentContact* manifoldPoints, const PxU32 numPoints, const PxReal toleranceLength);
 
 	//This function is used for incremental manifold contact reduction for box/convexhull
 	PxU32 reduceContactsForPCM(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen);