PhysX 3.4.1, APEX 1.4.1 Release @22845541v3.4.1

6 files changed, 200 insertions, 233 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
index 54cded49..db3908e8 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
@@ -36,7 +36,6 @@
 
 #define EPA_DEBUG	0
 
-
 namespace physx
 {
 namespace Gu
@@ -65,11 +64,11 @@ namespace Gu
 	public:
 		EPA(){}	
 		GjkStatus PenetrationDepth(const GjkConvex& a, const GjkConvex& b, const Ps::aos::Vec3V* PX_RESTRICT Q, const Ps::aos::Vec3V* PX_RESTRICT A, const Ps::aos::Vec3V* PX_RESTRICT B, const PxI32 size, Ps::aos::Vec3V& pa, Ps::aos::Vec3V& pb, Ps::aos::Vec3V& normal, Ps::aos::FloatV& penDepth, const bool takeCoreShape = false);
-		bool expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound);
-		bool expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound);
-		bool expandTriangle(PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound);
+		bool expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound);
+		bool expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound);
+		bool expandTriangle(PxI32& numVerts, const FloatVArg upperBound);
 
-		Facet* addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg lower2, const Ps::aos::FloatVArg upper2);
+		Facet* addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg upper);
 	
 		bool originInTetrahedron(const Ps::aos::Vec3VArg p1, const Ps::aos::Vec3VArg p2, const Ps::aos::Vec3VArg p3, const Ps::aos::Vec3VArg p4);
 
@@ -104,32 +103,6 @@ namespace Gu
 		support = V3Sub(tSupportA, tSupportB);
 	}
 
-	static FloatV calculatePlaneDist(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf)
-	{
-		const Vec3V pa0(aBuf[i0]);
-		const Vec3V pa1(aBuf[i1]);
-		const Vec3V pa2(aBuf[i2]);
-
-		const Vec3V pb0(bBuf[i0]);
-		const Vec3V pb1(bBuf[i1]);
-		const Vec3V pb2(bBuf[i2]);
-
-		const Vec3V p0 = V3Sub(pa0, pb0);
-		const Vec3V p1 = V3Sub(pa1, pb1);
-		const Vec3V p2 = V3Sub(pa2, pb2);
-		const Vec3V v1 = V3Sub(p1, p0);
-		const Vec3V v2 = V3Sub(p2, p0);
-		const Vec3V v3 = V3Sub(p2, p1);
-
-		const FloatV v1v1 = V3Dot(v1, v1);
-		const FloatV v2v2 = V3Dot(v2, v2);
-		const Vec3V v = V3Sel(FIsGrtr(v1v1, v2v2), v2, v1);
-		
-		const Vec3V planeNormal = V3Normalize(V3Cross(v, v3));
-		return  V3Dot(planeNormal, p0);
-	}
-
-
 	GjkStatus epaPenetration(const GjkConvex& a, const GjkConvex& b, PxU8* PX_RESTRICT aInd, PxU8* PX_RESTRICT bInd, PxU8 _size, Ps::aos::Vec3V& contactA, Ps::aos::Vec3V& contactB, Ps::aos::Vec3V& normal, Ps::aos::FloatV& penetrationDepth, const bool takeCoreShape)
 	{
 		using namespace Ps::aos;
@@ -224,9 +197,9 @@ namespace Gu
 	//ML: This function:
 	// (1)calculates the distance from orign((0, 0, 0)) to a triangle plane 
 	// (2) rejects triangle if the triangle is degenerate (two points are identical)
-	// (3) rejects triangle to be added into the heap if the plane distance is outside of the boundary[lower, upper]
-	PX_EPA_FORCE_INLINE Ps::aos::BoolV Facet::isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, 
-		const Ps::aos::FloatVArg lower, const Ps::aos::FloatVArg upper)
+	// (3) rejects triangle to be added into the heap if the plane distance is large than upper
+	Ps::aos::BoolV Facet::isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, 
+		const Ps::aos::FloatVArg upper)
 	{
 		using namespace Ps::aos;
 		const FloatV eps = FEps();
@@ -245,13 +218,8 @@ namespace Gu
 
 		const Vec3V v1 = V3Sub(p1, p0);
 		const Vec3V v2 = V3Sub(p2, p0);
-		const Vec3V v3 = V3Sub(p2, p1);
 
-		const FloatV v1v1 = V3Dot(v1, v1);
-		const FloatV v2v2 = V3Dot(v2, v2);
-		
-		const Vec3V v = V3Sel(FIsGrtr(v1v1, v2v2), v2, v1);
-		const Vec3V denormalizedNormal = V3Cross(v, v3);
+		const Vec3V denormalizedNormal = V3Cross(v1, v2);
 		FloatV norValue = V3Dot(denormalizedNormal, denormalizedNormal);
 		//if norValue < eps, this triangle is degenerate
 		const BoolV con = FIsGrtr(norValue, eps);
@@ -263,12 +231,13 @@ namespace Gu
 		
 		FStore(planeDist, &m_planeDist);
 
-		return BAnd(con, BAnd(FIsGrtrOrEq(planeDist, lower), FIsGrtrOrEq(upper, planeDist)));
+		return BAnd(con, FIsGrtrOrEq(upper, planeDist));
+		//return BAnd(con, BAnd(FIsGrtrOrEq(planeDist, lower), FIsGrtrOrEq(upper, planeDist)));
 	}
 
 	//ML: if the triangle is valid(not degenerate and within lower and upper bound), we need to add it into the heap. Otherwise, we just return
 	//the triangle so that the facet can be linked to other facets in the expanded polytope.
-	Facet* EPA::addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg lower2, const Ps::aos::FloatVArg upper2)
+	Facet* EPA::addFacet(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::FloatVArg upper)
 	{
 		using namespace Ps::aos;
 		PX_ASSERT(i0 != i1 && i0 != i2 && i1 != i2);
@@ -281,7 +250,7 @@ namespace Gu
 		Facet * facet = PX_PLACEMENT_NEW(&facetBuf[facetId],Facet(i0, i1, i2));
 		facet->m_FacetId = PxU8(facetId);
 
-		const  BoolV validTriangle = facet->isValid2(i0, i1, i2, aBuf, bBuf, lower2, upper2);
+		const  BoolV validTriangle = facet->isValid2(i0, i1, i2, aBuf, bBuf, upper);
 
 		if(BAllEqTTTT(validTriangle))
 		{
@@ -351,7 +320,7 @@ namespace Gu
 		}
 	}
 
-	bool EPA::expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound)
+	bool EPA::expandPoint(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound)
 	{
 		const Vec3V x = V3UnitX();
 		Vec3V q0 = V3Sub(aBuf[0], bBuf[0]);
@@ -359,11 +328,11 @@ namespace Gu
 		doSupport(a, b, x, aBuf[1], bBuf[1], q1);
 		if (V3AllEq(q0, q1))
 			return false;
-		return expandSegment(a, b, numVerts, lowerBound, upperBound);
+		return expandSegment(a, b, numVerts, upperBound);
 	}
 
 	//ML: this function use the segement to create a triangle
-	bool EPA::expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound)
+	bool EPA::expandSegment(const GjkConvex& a, const GjkConvex& b, PxI32& numVerts, const FloatVArg upperBound)
 	{
 		const Vec3V q0 = V3Sub(aBuf[0], bBuf[0]);
 		const Vec3V q1 = V3Sub(aBuf[1], bBuf[1]);
@@ -389,23 +358,23 @@ namespace Gu
 		Vec3V q2;
 		doSupport(a, b, n, aBuf[2], bBuf[2], q2);
 
-		return expandTriangle(numVerts, lowerBound, upperBound);
+		return expandTriangle(numVerts, upperBound);
 	}
 
-	bool EPA::expandTriangle(PxI32& numVerts, const FloatVArg lowerBound, const FloatVArg upperBound)
+	bool EPA::expandTriangle(PxI32& numVerts, const FloatVArg upperBound)
 	{
 		numVerts = 3;
 
-		Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, lowerBound, upperBound);
-		Facet * PX_RESTRICT f1 = addFacet(1, 0, 2, lowerBound, upperBound);
+		Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, upperBound);
+		Facet * PX_RESTRICT f1 = addFacet(1, 0, 2, upperBound);
 
-		if(f0 == NULL || f1 == NULL || heap.empty())
+		if(heap.empty())
 			return false;
 
 		f0->link(0, f1, 0);
 		f0->link(1, f1, 2);
 		f0->link(2, f1, 1);
-		
+
 		return true;
 	}
 
@@ -414,21 +383,16 @@ namespace Gu
 	//For example, we treat sphere/capsule as a point/segment in the support function for GJK/EPA, so that the core shape for sphere/capsule is a point/segment. For PCM, we need 
 	//to take the point from the core shape because this will allows us recycle the contacts more stably. For SQ sweeps, we need to take the point on the surface of the sphere/capsule 
 	//when we calculate MTD because this is what will be reported to the user. Therefore, the takeCoreShape flag will be set to be false in SQ.
-	static void calculateContactInformation(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, const Facet* facet, const GjkConvex& a, const GjkConvex& b, Vec3V& pa, Vec3V& pb, Vec3V& normal, FloatV& penDepth, const bool takeCoreShape)
+	static void calculateContactInformation(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Facet* facet, const GjkConvex& a, const GjkConvex& b, Vec3V& pa, Vec3V& pb, Vec3V& normal, FloatV& penDepth, const bool takeCoreShape)
 	{
 		const FloatV zero = FZero();
 		Vec3V _pa, _pb;
 		facet->getClosestPoint(aBuf, bBuf, _pa, _pb);
 		//dist > 0 means two shapes are penetrated. If dist < 0(when origin isn't inside the polytope), two shapes status are unknown
 		const FloatV dist = facet->getPlaneDist();
-		//planeNormal is pointing from B to A, however, the normal we are expecting is from A to B which match the GJK margin intersect case, therefore,
-		//we need to flip the normal
-		Vec3V planeNormal = facet->getPlaneNormal();
-
-		const Vec3V normalDir = V3Sub(a.getCenter(), b.getCenter());
-		if (FAllGrtr(zero, V3Dot(normalDir, planeNormal)))
-			planeNormal = V3Neg(planeNormal);
 		
+		const Vec3V planeNormal = V3Neg(facet->getPlaneNormal());
+
 		if(takeCoreShape)
 		{
 			pa = _pa;
@@ -496,7 +460,6 @@ namespace Gu
 	
 		const FloatV _max = FMax();
 		FloatV upper_bound(_max);
-		FloatV lower_bound(FNeg(_max));
 	
 		aBuf[0]=A[0]; aBuf[1]=A[1]; aBuf[2]=A[2]; aBuf[3]=A[3];
 		bBuf[0]=B[0]; bBuf[1]=B[1]; bBuf[2]=B[2]; bBuf[3]=B[3];
@@ -511,7 +474,7 @@ namespace Gu
 		case 1:
 			{
 				// Touching contact. Yes, we have a collision and the penetration will be zero
-				if(!expandPoint(a, b, numVertsLocal, lower_bound, upper_bound))
+				if(!expandPoint(a, b, numVertsLocal, upper_bound))
 					return EPA_FAIL;
 				break;
 			}
@@ -519,7 +482,7 @@ namespace Gu
 			{
 				// We have a line segment inside the Minkowski sum containing the
 				// origin. we need to construct two back to back triangles which link to each other
-				if(!expandSegment(a, b, numVertsLocal, lower_bound, upper_bound))
+				if(!expandSegment(a, b, numVertsLocal, upper_bound))
 					return EPA_FAIL;
 				break;
 			}
@@ -527,7 +490,7 @@ namespace Gu
 			{
 				// We have a triangle inside the Minkowski sum containing
 				// the origin. We need to construct two back to back triangles which link to each other
-				if(!expandTriangle(numVertsLocal, lower_bound, upper_bound))
+				if(!expandTriangle(numVertsLocal, upper_bound))
 					return EPA_FAIL;
 
 				break;
@@ -537,9 +500,29 @@ namespace Gu
 			{
 				//check for input face normal. All face normals in this tetrahedron should be all pointing either inwards or outwards. If all face normals are pointing outward, we are good to go. Otherwise, we need to 
 				//shuffle the input vertexes and make sure all face normals are pointing outward
-				const FloatV planeDistf0 = calculatePlaneDist(0, 1, 2, aBuf, bBuf);
-				
-				if(FAllGrtr(zero, planeDistf0))
+				const Vec3V pa0(aBuf[0]);
+				const Vec3V pa1(aBuf[1]);
+				const Vec3V pa2(aBuf[2]);
+				const Vec3V pa3(aBuf[3]);
+
+				const Vec3V pb0(bBuf[0]);
+				const Vec3V pb1(bBuf[1]);
+				const Vec3V pb2(bBuf[2]);
+				const Vec3V pb3(bBuf[3]);
+
+				const Vec3V p0 = V3Sub(pa0, pb0);
+				const Vec3V p1 = V3Sub(pa1, pb1);
+				const Vec3V p2 = V3Sub(pa2, pb2);
+				const Vec3V p3 = V3Sub(pa3, pb3);
+
+				const Vec3V v1 = V3Sub(p1, p0);
+				const Vec3V v2 = V3Sub(p2, p0);
+
+				const Vec3V planeNormal = V3Normalize(V3Cross(v1, v2));
+
+				const FloatV signDist = V3Dot(planeNormal, V3Sub(p3, p0));
+
+				if (FAllGrtr(signDist, zero))
 				{
 					//shuffle the input vertexes
 					const Vec3V tempA0 = aBuf[2];
@@ -549,19 +532,20 @@ namespace Gu
 					aBuf[1] = tempA0;
 					bBuf[1] = tempB0;
 				}
-				Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, lower_bound, upper_bound);
-				Facet * PX_RESTRICT f1 = addFacet(0, 3, 1, lower_bound, upper_bound);
-				Facet * PX_RESTRICT f2 = addFacet(0, 2, 3, lower_bound, upper_bound);
-				Facet * PX_RESTRICT f3 = addFacet(1, 3, 2, lower_bound, upper_bound);
-				PX_ASSERT(f0->m_planeDist >= 0.f);
-				PX_ASSERT(f1->m_planeDist >= 0.f);
-				PX_ASSERT(f2->m_planeDist >= 0.f);
-				PX_ASSERT(f3->m_planeDist >= 0.f);
-
-				if((f0==NULL) || (f1==NULL) || (f2==NULL) || (f3==NULL) || heap.empty())
-					return EPA_FAIL;
 
+				Facet * PX_RESTRICT f0 = addFacet(0, 1, 2, upper_bound);
+				Facet * PX_RESTRICT f1 = addFacet(0, 3, 1, upper_bound);
+				Facet * PX_RESTRICT f2 = addFacet(0, 2, 3, upper_bound);
+				Facet * PX_RESTRICT f3 = addFacet(1, 3, 2, upper_bound);
 			
+				if (heap.empty())
+					return EPA_FAIL;
+
+				PX_ASSERT(f0->m_planeDist >= -1e-2f);
+				PX_ASSERT(f1->m_planeDist >= -1e-2f);
+				PX_ASSERT(f2->m_planeDist >= -1e-2f);
+				PX_ASSERT(f3->m_planeDist >= -1e-2f);
+
 				f0->link(0, f1, 2);
 				f0->link(1, f3, 2);
 				f0->link(2, f2, 0);
@@ -580,7 +564,7 @@ namespace Gu
 		const FloatV eps2 = FMul(minMargin, tenPerc);
 
 		Facet* PX_RESTRICT facet = NULL;
-	
+
 		Vec3V tempa, tempb, q;
 
 		do 
@@ -612,29 +596,16 @@ namespace Gu
 				//might be negative
 				const FloatV dist = V3Dot(q, planeNormal);
 
-				//update the upper bound to the minimum between exisiting upper bound and the distance if distance is positive
-				upper_bound = FSel(FIsGrtrOrEq(dist, zero), FMin(upper_bound, dist), upper_bound);
-				//lower bound is the plane distance, which will be the smallest among all the facets in the prority queue
-				lower_bound = planeDist;
-				//if the plane distance and the upper bound is within a small tolerance, which means we found the MTD
-				const BoolV con0 = FIsGrtrOrEq(eps2, FAbs(FSub(upper_bound, lower_bound)));
+				const BoolV con0 = FIsGrtrOrEq(eps2, FAbs(FSub(dist, planeDist)));
 
-				if(BAllEqTTTT(con0))
+				if (BAllEqTTTT(con0))
 				{
 					calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape);
 					return EPA_CONTACT;
 				}
 
-				//if planeDist is the same as dist, which means the support point we get out from the Mincowsky sum is one of the point
-				//in the triangle facet, we should exist because we can't progress further. We can use the fact as contact information
-				const FloatV dif = FSub(dist, planeDist);
-				const BoolV degeneratedCondition = FIsGrtr(eps2, dif);
-				if (BAllEqTTTT(degeneratedCondition))
-				{
-					calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape);
-
-					return EPA_CONTACT;
-				}
+				//update the upper bound to the minimum between existing upper bound and the distance
+				upper_bound = FMin(upper_bound, dist);
 
 				aBuf[numVertsLocal]=tempa;
 				bBuf[numVertsLocal]=tempb;
@@ -668,17 +639,18 @@ namespace Gu
 					return EPA_DEGENERATE;
 				}
 
-				Facet *firstFacet = addFacet(edge->getTarget(), edge->getSource(),index, lower_bound, upper_bound);
+				Facet *firstFacet = addFacet(edge->getTarget(), edge->getSource(),index, upper_bound);
 				PX_ASSERT(firstFacet);
 				firstFacet->link(0, edge->getFacet(), edge->getIndex());
 
 				Facet * PX_RESTRICT lastFacet = firstFacet;
 
+#if EPA_DEBUG
 				bool degenerate = false;
 				for(PxU32 i=1; (i<bufferSize) && (!degenerate); ++i)
 				{
 					edge=edgeBuffer.Get(i);
-					Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(),index, lower_bound, upper_bound);
+					Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(),index, upper_bound);
 					PX_ASSERT(newFacet);
 					const bool b0 = newFacet->link(0, edge->getFacet(), edge->getIndex());
 					const bool b1 = newFacet->link(2, lastFacet, 1);
@@ -686,11 +658,18 @@ namespace Gu
 					lastFacet = newFacet;
 				}
 
-				if(degenerate)
+				if (degenerate)
+					Ps::debugBreak();
+#else
+				for (PxU32 i = 1; i<bufferSize; ++i)
 				{
-					calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape);
-					return EPA_DEGENERATE;
+					edge = edgeBuffer.Get(i);
+					Facet* PX_RESTRICT newFacet = addFacet(edge->getTarget(), edge->getSource(), index, upper_bound);
+					newFacet->link(0, edge->getFacet(), edge->getIndex());
+					newFacet->link(2, lastFacet, 1);
+					lastFacet = newFacet;
 				}
+#endif
 
 				firstFacet->link(2, lastFacet, 1);
 			}
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
index dee06e90..690a0b81 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
@@ -42,6 +42,8 @@
 #define PX_EPA_FORCE_INLINE PX_FORCE_INLINE
 #endif
 
+#define EPA_DEBUG	0
+
 namespace physx
 {
 #define MaxEdges 32
@@ -103,8 +105,8 @@ namespace Gu
 		PX_FORCE_INLINE Ps::aos::FloatV getPlaneDist(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf) const; 
 
 		//check to see whether the triangle is a valid triangle, calculate plane normal and plane distance at the same time
-		PX_EPA_FORCE_INLINE Ps::aos::BoolV isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, 
-		const Ps::aos::FloatVArg lower, const Ps::aos::FloatVArg upper);
+		Ps::aos::BoolV isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, 
+		const Ps::aos::FloatVArg upper);
 
 		//return the absolute value for the plane distance from origin 
 		PX_FORCE_INLINE Ps::aos::FloatV getPlaneDist() const 
@@ -119,7 +121,7 @@ namespace Gu
 		}
 
 		//calculate the closest points for a shape pair
-		void getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB) const;
+		void getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB);
 		
 		//performs a flood fill over the boundary of the current polytope. 
 		void silhouette(const Ps::aos::Vec3VArg w, const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, EdgeBuffer& edgeBuffer, EPAFacetManager& manager);
@@ -136,7 +138,11 @@ namespace Gu
 			EPAFacetManager& manager);
 
 		Ps::aos::Vec3V m_planeNormal;																										//16
-		PxF32 m_planeDist;																													//20
+		PxF32 m_planeDist;
+#if EPA_DEBUG
+		PxF32 m_lambda1;
+		PxF32 m_lambda2;
+#endif
 		
 		Facet* PX_RESTRICT m_adjFacets[3]; //the triangle adjacent to edge i in this triangle												//32
 		PxI8 m_adjEdges[3]; //the edge connected with the corresponding triangle															//35
@@ -234,10 +240,10 @@ namespace Gu
 	};
 
 	//ML: calculate MTD points for a shape pair
-	PX_FORCE_INLINE void Facet::getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB) const 
+	PX_FORCE_INLINE void Facet::getClosestPoint(const Ps::aos::Vec3V* PX_RESTRICT aBuf, const Ps::aos::Vec3V* PX_RESTRICT bBuf, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB)
 	{
 		using namespace Ps::aos;
-		
+
 		const Vec3V pa0(aBuf[m_indices[0]]);
 		const Vec3V pa1(aBuf[m_indices[1]]);
 		const Vec3V pa2(aBuf[m_indices[2]]);
@@ -250,28 +256,34 @@ namespace Gu
 		const Vec3V p1 = V3Sub(pa1, pb1);
 		const Vec3V p2 = V3Sub(pa2, pb2);
 
-		const Vec3V v1 = V3Sub(p1, p0);
-		const Vec3V v2 = V3Sub(p2, p0);
+		const Vec3V v0 = V3Sub(p1, p0);
+		const Vec3V v1 = V3Sub(p2, p0);
 
-		const FloatV v1dv1 = V3Dot(v1, v1);
-		const FloatV v1dv2 = V3Dot(v1, v2);
-		const FloatV v2dv2 = V3Dot(v2, v2);
+		const Vec3V closestP = V3Scale(m_planeNormal, FLoad(m_planeDist));
+		const Vec3V v2 = V3Sub(closestP, p0);
+		//calculate barycentric coordinates
+		const FloatV d00 = V3Dot(v0, v0);
+		const FloatV d01 = V3Dot(v0, v1);
+		const FloatV d11 = V3Dot(v1, v1);
 
-		const FloatV p0dv1 = V3Dot(p0, v1); //V3Dot(V3Sub(p0, origin), v1);
-		const FloatV p0dv2 = V3Dot(p0, v2);
+		const FloatV d20 = V3Dot(v2, v0);
+		const FloatV d21 = V3Dot(v2, v1);
 
-		const FloatV det = FNegScaleSub(v1dv2, v1dv2, FMul(v1dv1, v2dv2));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative
+		const FloatV det = FNegScaleSub(d01, d01, FMul(d00, d11));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative
 		const FloatV recip = FSel(FIsGrtr(det, FEps()), FRecip(det), FZero());
 
-		const FloatV lambda1 = FMul(FNegScaleSub(p0dv1, v2dv2, FMul(p0dv2, v1dv2)), recip);
-		const FloatV lambda2 = FMul(FNegScaleSub(p0dv2, v1dv1, FMul(p0dv1, v1dv2)), recip);
+		const FloatV lambda1 = FMul(FNegScaleSub(d01, d21, FMul(d11, d20)), recip);
+		const FloatV lambda2 = FMul(FNegScaleSub(d01, d20, FMul(d00, d21)), recip);
+
+#if EPA_DEBUG
+		FStore(lambda1, &m_lambda1);
+		FStore(lambda2, &m_lambda2);
+#endif
 
 		const Vec3V a0 = V3Scale(V3Sub(pa1, pa0), lambda1);
 		const Vec3V a1 = V3Scale(V3Sub(pa2, pa0), lambda2);
-		const Vec3V b0 = V3Scale(V3Sub(pb1, pb0), lambda1);
-		const Vec3V b1 = V3Scale(V3Sub(pb2, pb0), lambda2);
 		closestA = V3Add(V3Add(a0, a1), pa0);
-		closestB = V3Add(V3Add(b0, b1), pb0);
+		closestB = V3Sub(closestA, closestP);
 	}
 
 	//ML: create adjacency informations for both facets
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
index 6787a93e..fb90d9c6 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
@@ -59,7 +59,7 @@ namespace Gu
 
 
 	PX_FORCE_INLINE void CalculateBoxMargin(const Ps::aos::Vec3VArg extent, PxReal& minExtent, PxReal& minMargin, PxReal& sweepMargin,
-		 const PxReal minMarginR = BOX_MIN_MARGIN_RATIO)
+		const PxReal minMarginR = BOX_MIN_MARGIN_RATIO)
 	{
 		using namespace Ps::aos;
 
@@ -80,11 +80,20 @@ namespace Gu
 	}
 
 	//This method is called in the PCM contact gen for the refreshing contacts 
-	PX_FORCE_INLINE Ps::aos::FloatV CalculatePCMBoxMargin(const Ps::aos::Vec3VArg extent)
+	PX_FORCE_INLINE Ps::aos::FloatV CalculatePCMBoxMargin(const Ps::aos::Vec3VArg extent, const PxReal toleranceLength, const PxReal toleranceMarginRatio = BOX_MARGIN_RATIO)
 	{
 		using namespace Ps::aos;
 	
 		const FloatV min = V3ExtractMin(extent);//FMin(V3GetX(extent), FMin(V3GetY(extent), V3GetZ(extent)));
+		const FloatV toleranceMargin = FLoad(toleranceLength * toleranceMarginRatio);
+		return FMin(FMul(min, FLoad(BOX_MARGIN_RATIO)), toleranceMargin);
+	}
+
+	PX_FORCE_INLINE Ps::aos::FloatV CalculateMTDBoxMargin(const Ps::aos::Vec3VArg extent)
+	{
+		using namespace Ps::aos;
+
+		const FloatV min = V3ExtractMin(extent);//FMin(V3GetX(extent), FMin(V3GetY(extent), V3GetZ(extent)));
 		return FMul(min, FLoad(BOX_MARGIN_RATIO));
 	}
 
@@ -106,7 +115,8 @@ namespace Gu
 			marginDif = Ps::aos::FZero();
 			margin = 0.f;
 		}
-																		
+		
+		//this constructor is used by the CCD system
 		PX_FORCE_INLINE BoxV(const PxGeometry& geom) : ConvexV(ConvexType::eBOX, Ps::aos::V3Zero())
 		{
 			using namespace Ps::aos;
@@ -125,8 +135,13 @@ namespace Gu
 		{
 		}
 
+		PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength)
+		{
+			minMargin = PxMin(toleranceLength * BOX_MIN_MARGIN_RATIO, minMargin);
+		}
+
 		//! Assignment operator
-		PX_INLINE const BoxV& operator=(const BoxV& other)
+		PX_FORCE_INLINE const BoxV& operator=(const BoxV& other)
 		{
 			center	= other.center;
 			extents	= other.extents;
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
index 1c2fd660..527d1fbd 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
@@ -48,35 +48,51 @@ namespace Gu
 #define CONVEX_MARGIN_RATIO			0.1f
 #define CONVEX_MIN_MARGIN_RATIO		0.05f
 #define	CONVEX_SWEEP_MARGIN_RATIO	0.025f
+#define TOLERANCE_MARGIN_RATIO		0.08f
+#define TOLERANCE_MIN_MARGIN_RATIO	0.05f
+
 
 	//This margin is used in Persistent contact manifold
-	PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale)
+	PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale, 
+		const PxReal toleranceLength, const PxReal toleranceRatio = TOLERANCE_MIN_MARGIN_RATIO)
 	{
+		
 		using namespace Ps::aos;
 		const Vec3V extents= V3Mul(V3LoadU(hullData->mInternal.mExtents), scale);
 		const FloatV min = V3ExtractMin(extents);
+		const FloatV toleranceMargin = FLoad(toleranceLength * toleranceRatio);
 		//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
-		return FMul(min, FLoad(0.25f));
+		return FMin(FMul(min, FLoad(0.25f)), toleranceMargin);
 	}
 
-
-	//This margin is used in PCM contact gen
-	PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const Gu::ConvexHullData* hullData, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin, const Ps::aos::Vec3VArg scale)
+	PX_SUPPORT_FORCE_INLINE Ps::aos::FloatV CalculateMTDConvexMargin(const Gu::ConvexHullData* hullData, const Ps::aos::Vec3VArg scale)
 	{
 		using namespace Ps::aos;
 		const Vec3V extents = V3Mul(V3LoadU(hullData->mInternal.mExtents), scale);
 		const FloatV min = V3ExtractMin(extents);
+		//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
+		return FMul(min, FLoad(0.25f));
+	}
 
-		//Margin is used in the plane shifting for the shrunk convex hull
-		const FloatV margin_ = FMul(min, FLoad(CONVEX_MARGIN_RATIO));
-		//minMargin is used in the GJK termination condition
-		const FloatV minMargin_ = FMul(min, FLoad(CONVEX_MIN_MARGIN_RATIO));
 
-		const FloatV sweepMargin_ = FMul(min, FLoad(CONVEX_SWEEP_MARGIN_RATIO));
+	//This minMargin is used in PCM contact gen
+	PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const Gu::ConvexHullData* hullData, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin,
+		const Ps::aos::Vec3VArg scale)
+	{
+		using namespace Ps::aos;
+		
+		const Vec3V extents = V3Mul(V3LoadU(hullData->mInternal.mExtents), scale);
+		const FloatV min_ = V3ExtractMin(extents);
 
-		FStore(margin_, &margin);
-		FStore(minMargin_, &minMargin);
-		FStore(sweepMargin_, &sweepMargin);
+		PxReal minExtent;
+		FStore(min_, &minExtent);
+
+		//Margin is used in the plane shifting for the shrunk convex hull and acceptanceTolerance for overlap
+		margin = minExtent * CONVEX_MARGIN_RATIO;
+		//minMargin is used in the GJK termination condition
+		minMargin = minExtent * CONVEX_MIN_MARGIN_RATIO;
+		//this is used for GJKRaycast
+		sweepMargin = minExtent * CONVEX_SWEEP_MARGIN_RATIO;
 	}
 
 	PX_SUPPORT_FORCE_INLINE Ps::aos::Mat33V ConstructSkewMatrix(const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg rotation) 
@@ -170,39 +186,36 @@ namespace Gu
 		{
 		public:
 			PxU32 m[8];
-			PX_FORCE_INLINE TinyBitMap()			{ m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0;	}
-			PX_FORCE_INLINE void set(PxU8 v)		{ m[v>>5] |= 1<<(v&31);											}
-			PX_FORCE_INLINE bool get(PxU8 v) const	{ return (m[v>>5] & 1<<(v&31)) != 0;							}
+			PX_FORCE_INLINE TinyBitMap() { m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0; }
+			PX_FORCE_INLINE void set(PxU8 v) { m[v >> 5] |= 1 << (v & 31); }
+			PX_FORCE_INLINE bool get(PxU8 v) const { return (m[v >> 5] & 1 << (v & 31)) != 0; }
 		};
 
 
-		public:
+	public:
 		/**
 		\brief Constructor
 		*/
-		PX_SUPPORT_INLINE ConvexHullV(): ConvexV(ConvexType::eCONVEXHULL)
+		PX_SUPPORT_INLINE ConvexHullV() : ConvexV(ConvexType::eCONVEXHULL)
 		{
 		}
 
-		PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, const bool idtScale):
+		PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot,
+			const bool idtScale) :
 			ConvexV(ConvexType::eCONVEXHULL, _center)
 		{
 			using namespace Ps::aos;
 
-			hullData = _hullData;	
+			hullData = _hullData;
 			const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices();
-			//const PxU8* PX_RESTRICT polyInds = _hullData->getFacesByVertices8();
-			//const HullPolygonData* PX_RESTRICT polygons = _hullData->mPolygons;
 			verts = tempVerts;
 			numVerts = _hullData->mNbHullVertices;
-			CalculateConvexMargin( _hullData, margin, minMargin, sweepMargin, scale);
+			CalculateConvexMargin(_hullData, margin, minMargin, sweepMargin, scale);
 			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
-			/*skewScale = Mat33V temp(V3Scale(trans.col0, V3GetX(scale)), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale)));
-			skewRot = QuatGetMat33V(scaleRot);*/
-
 			data = _hullData->mBigConvexRawData;
 		}
 
+		//this is used by CCD system
 		PX_SUPPORT_INLINE ConvexHullV(const PxGeometry& geom) : ConvexV(ConvexType::eCONVEXHULL, Ps::aos::V3Zero())
 		{
 			using namespace Ps::aos;
@@ -213,20 +226,22 @@ namespace Gu
 			const QuatV vRot = QuatVLoadU(&convexGeom.scale.rotation.x);
 			const bool idtScale = convexGeom.scale.isIdentity();
 
-			hullData = hData;	
+			hullData = hData;
 			const PxVec3* PX_RESTRICT tempVerts = hData->getHullVertices();
 			verts = tempVerts;
 			numVerts = hData->mNbHullVertices;
-			CalculateConvexMargin( hData, margin, minMargin, sweepMargin, vScale);
+			CalculateConvexMargin(hData, margin, minMargin, sweepMargin, vScale);
 			ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, center, idtScale);
 
 			data = hData->mBigConvexRawData;
+
 		}
 
-		PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, const bool idtScale)
-		{	
+		PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale,
+			const Ps::aos::QuatVArg scaleRot, const bool idtScale)
+		{
 			using namespace Ps::aos;
-			
+
 			const PxVec3* tempVerts = _hullData->getHullVertices();
 			CalculateConvexMargin(_hullData, margin, minMargin, sweepMargin, scale);
 			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
@@ -234,21 +249,31 @@ namespace Gu
 			verts = tempVerts;
 			numVerts = _hullData->mNbHullVertices;
 			//rot = _rot;	
-			
+
 			center = _center;
 
-		//	searchIndex = 0;
+			//	searchIndex = 0;
 			data = _hullData->mBigConvexRawData;
 
 			hullData = _hullData;
-			if(_hullData->mBigConvexRawData)
+			if (_hullData->mBigConvexRawData)
 			{
 				Ps::prefetchLine(hullData->mBigConvexRawData->mValencies);
-				Ps::prefetchLine(hullData->mBigConvexRawData->mValencies,128);
+				Ps::prefetchLine(hullData->mBigConvexRawData->mValencies, 128);
 				Ps::prefetchLine(hullData->mBigConvexRawData->mAdjacentVerts);
 			}
 		}
 
+
+		PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength)
+		{
+			const PxReal toleranceMinMargin = toleranceLength * TOLERANCE_MIN_MARGIN_RATIO;
+			const PxReal toleranceMargin = toleranceLength * TOLERANCE_MARGIN_RATIO;
+
+			margin = PxMin(margin, toleranceMargin);
+			minMargin = PxMin(minMargin, toleranceMinMargin);
+		}
+
 		PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* /*marginDif*/)const
 		{
 			using namespace Ps::aos;
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
index d4a09435..dd910b66 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
@@ -52,78 +52,6 @@ namespace Gu
 		{
 		}
 
-		PX_SUPPORT_INLINE ConvexHullNoScaleV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot)												
-		{
-			PX_UNUSED(scaleRot);
-			PX_UNUSED(_center);
-
-			using namespace Ps::aos;
-
-			hullData = _hullData;	
-			const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices();
-			verts = tempVerts;
-			numVerts = _hullData->mNbHullVertices;
-			CalculateConvexMargin( _hullData, margin, minMargin, sweepMargin, scale);
-			data = _hullData->mBigConvexRawData;
-
-			PxU8* startAddress = reinterpret_cast<PxU8*>(_hullData->mPolygons);
-			PxI32 totalPrefetchBytes = PxI32((_hullData->getFacesByVertices8() + _hullData->mNbHullVertices * 3) - startAddress);
-
-			//Prefetch core data
-			
-			while(totalPrefetchBytes > 0)
-			{
-				totalPrefetchBytes -= 128;
-				Ps::prefetchLine(startAddress);
-				startAddress += 128;
-			}
-
-			if(data)
-			{
-				PxI32 totalSize = PxI32(data->mNbSamples + data->mNbVerts * sizeof(Gu::Valency) + data->mNbAdjVerts);
-				startAddress = data->mSamples;
-				while(totalSize > 0)
-				{
-					totalSize -= 128;
-					Ps::prefetchLine(startAddress);
-					startAddress += 128;
-				}
-			}
-			
-		}
-
-
-	/*	PX_SUPPORT_INLINE ConvexHullNoScaleV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::FloatVArg _margin, const Ps::aos::FloatVArg _minMargin, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot)
-		{
-			PX_UNUSED(scaleRot);
-			PX_UNUSED(_center);
-			PX_UNUSED(scale);
-
-			using namespace Ps::aos;
-
-			hullData = _hullData;
-			margin = _margin;
-			minMargin = _minMargin;
-	
-			const PxVec3* tempVerts = _hullData->getHullVertices();
-			const PxU8* PX_RESTRICT polyInds = _hullData->getFacesByVertices8();
-			const HullPolygonData* PX_RESTRICT polygons = _hullData->mPolygons;
-			verts = tempVerts;
-			numVerts = _hullData->mNbHullVertices;
-
-			Ps::prefetchLine(tempVerts);
-			Ps::prefetchLine(tempVerts,128);
-			Ps::prefetchLine(tempVerts,256);
-
-			Ps::prefetchLine(polyInds);
-			Ps::prefetchLine(polyInds,128);
-
-			Ps::prefetchLine(polygons);
-			Ps::prefetchLine(polygons, 128);
-			Ps::prefetchLine(polygons, 256);
-		}*/
-
-
 		PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* /*marginDif*/)const
 		{
 			using namespace Ps::aos;
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
index f8a214d1..4f3686a6 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
@@ -77,6 +77,7 @@ namespace Gu
 		{
 		}
 
+		//this is used by CCD system
 		PX_INLINE ShrunkBoxV(const PxGeometry& geom) : BoxV(geom)
 		{
 			margin = minExtent * BOX_MARGIN_CCD_RATIO;
@@ -90,8 +91,8 @@ namespace Gu
 		\param extent Extents/radii of the obb.
 		*/
 
-		PX_FORCE_INLINE ShrunkBoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) : 
-																				BoxV(origin, extent)
+		PX_FORCE_INLINE ShrunkBoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) 
+			: BoxV(origin, extent)
 		{
 			//calculate margin
 			margin = minExtent * BOX_MARGIN_RATIO;
@@ -105,6 +106,13 @@ namespace Gu
 		{
 		}
 
+		PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength)
+		{
+			BoxV::resetMargin(toleranceLength);
+			margin = PxMin(toleranceLength * BOX_MARGIN_RATIO, margin);
+			initialiseMarginDif();
+		}
+
 		//! Assignment operator
 		PX_INLINE const ShrunkBoxV& operator=(const ShrunkBoxV& other)
 		{