PhysX 3.4, APEX 1.4 patch release @22121272

20 files changed, 196 insertions, 157 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/convex/GuConvexSupportTable.h b/PhysX_3.4/Source/GeomUtils/src/convex/GuConvexSupportTable.h
index 7b2f41a6..2f9044f6 100644
--- a/PhysX_3.4/Source/GeomUtils/src/convex/GuConvexSupportTable.h
+++ b/PhysX_3.4/Source/GeomUtils/src/convex/GuConvexSupportTable.h
@@ -43,9 +43,7 @@ namespace Gu
 	class CapsuleV;
 	class BoxV;
 	class ConvexHullV;
-	class ShrunkConvexHullV;
 	class ConvexHullNoScaleV;
-	class ShrunkConvexHullNoScaleV;
 
 	
 #if PX_VC 
@@ -66,7 +64,7 @@ namespace Gu
 		{
 		}
 
-		void setShapeSpaceCenterofMass(const Ps::aos::Vec3VArg _shapeSpaceCenterOfMass)
+		PX_FORCE_INLINE void setShapeSpaceCenterofMass(const Ps::aos::Vec3VArg _shapeSpaceCenterOfMass)
 		{
 			shapeSpaceCenterOfMass = _shapeSpaceCenterOfMass;
 		}
@@ -104,57 +102,15 @@ namespace Gu
 			return conv.supportLocal(dir, min, max);
 		}
 
-
 		void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts) const 
 		{
 			conv.populateVerts(inds, numInds, originalVerts, verts);
 		}
 
-		PX_FORCE_INLINE void populateVertsFast(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts) const 
-		{
-			conv.populateVerts(inds, numInds, originalVerts, verts);
-		}
-
 	protected:
 		SupportLocalImpl& operator=(const SupportLocalImpl&);
 
 	};
-
-	template <typename Convex, typename ShrunkConvex>
-	class SupportLocalShrunkImpl : public SupportLocal
-	{
-		SupportLocalShrunkImpl& operator=(const SupportLocalShrunkImpl&);
-	public:
-		const Convex& conv;
-		const ShrunkConvex& shrunkConvex;
-		SupportLocalShrunkImpl(const Convex& _conv, const Ps::aos::PsTransformV& _transform, const Ps::aos::Mat33V& _vertex2Shape, const Ps::aos::Mat33V& _shape2Vertex, const bool _isIdentityScale = true) : 
-		SupportLocal(_transform, _vertex2Shape, _shape2Vertex, _isIdentityScale), conv(_conv), 
-		shrunkConvex(static_cast<const ShrunkConvex&>(static_cast<const ConvexV&>(_conv)))	//ML: The types may or may not be related but they will share the base class (ConvexV) so to avoid
-																							//a compiler warning if we use reinterpret_cast unnecessarily, we static cast to base, then back up to 
-																							//the derived type. We guarantee that all types that are converted between are data compatible
-		{
-		}
-
-		Ps::aos::Vec3V doSupport(const Ps::aos::Vec3VArg dir) const
-		{
-			//return conv.supportVertsLocal(dir);
-			return conv.supportLocal(dir);
-		}
-
-		void doSupport(const Ps::aos::Vec3VArg dir, Ps::aos::FloatV& min, Ps::aos::FloatV& max) const
-		{
-			return conv.supportLocal(dir, min, max);
-		}
-
-		void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts) const 
-		{
-			conv.populateVerts(inds, numInds, originalVerts, verts);
-		}
-
-		PX_COMPILE_TIME_ASSERT(sizeof(Convex) == sizeof(ShrunkConvex));
-
-	};
-
 }
 
 }
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
index 6eede077..54cded49 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPA.cpp
@@ -36,6 +36,7 @@
 
 #define EPA_DEBUG	0
 
+
 namespace physx
 {
 namespace Gu
@@ -422,13 +423,17 @@ namespace Gu
 		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
-		const Vec3V n =V3Neg(facet->getPlaneNormal());
+		Vec3V planeNormal = facet->getPlaneNormal();
+
+		const Vec3V normalDir = V3Sub(a.getCenter(), b.getCenter());
+		if (FAllGrtr(zero, V3Dot(normalDir, planeNormal)))
+			planeNormal = V3Neg(planeNormal);
 		
 		if(takeCoreShape)
 		{
 			pa = _pa;
 			pb = _pb;
-			normal = n;
+			normal = planeNormal;
 			penDepth = FNeg(dist);
 		}
 		else
@@ -440,12 +445,27 @@ namespace Gu
 			const FloatV marginA = FSel(aQuadratic, a.getMargin(), zero);
 			const FloatV marginB = FSel(bQuadratic, b.getMargin(), zero);
 			const FloatV sumMargin = FAdd(marginA, marginB);
-			pa = V3NegScaleSub(n, marginA, _pa);
-			pb = V3ScaleAdd(n, marginB, _pb);
-			normal = n;
+			pa = V3NegScaleSub(planeNormal, marginA, _pa);
+			pb = V3ScaleAdd(planeNormal, marginB, _pb);
+			normal = planeNormal;
 			penDepth = FNeg(FAdd(dist, sumMargin));
-			
+
 #if	EPA_DEBUG
+
+			PxVec3 tempA, tempB;
+
+			V3StoreU(pa, tempA);
+			V3StoreU(pb, tempB);
+
+			if (!tempA.isFinite())
+			{
+				Ps::debugBreak();
+			}
+
+			if (!tempB.isFinite())
+			{
+				Ps::debugBreak();
+			}
 			const Vec3V v = V3Sub(_pb, _pa);
 			const FloatV length = V3Length(v);
 			const Vec3V cn = V3ScaleInv(v, length); 
@@ -554,15 +574,13 @@ namespace Gu
 			}
 		}
 
-		const FloatV eps = FEps();
+	
 		const FloatV tenPerc = FLoad(0.1f);
 		const FloatV minMargin = FMin(a.getMinMargin(), b.getMinMargin());
 		const FloatV eps2 = FMul(minMargin, tenPerc);
 
 		Facet* PX_RESTRICT facet = NULL;
-		Facet* PX_RESTRICT bestFacet = NULL;
-
-		bool hasMoreFacets = false;
+	
 		Vec3V tempa, tempb, q;
 
 		do 
@@ -574,7 +592,6 @@ namespace Gu
 			
 			if (!facet->isObsolete()) 
 			{
-				bestFacet = facet;
 				Ps::prefetchLine(edgeBuffer.m_pEdges);
 				Ps::prefetchLine(edgeBuffer.m_pEdges,128);
 				Ps::prefetchLine(edgeBuffer.m_pEdges,256);
@@ -594,6 +611,7 @@ namespace Gu
 				//the plane normal, which means the distance should be positive. However, if the origin isn't contained in the polytope, dist
 				//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
@@ -608,13 +626,14 @@ namespace Gu
 				}
 
 				//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.
+				//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(eps, dif);
-				if(BAllEqTTTT(degeneratedCondition))
+				const BoolV degeneratedCondition = FIsGrtr(eps2, dif);
+				if (BAllEqTTTT(degeneratedCondition))
 				{
 					calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape);
-					return EPA_DEGENERATE;
+
+					return EPA_CONTACT;
 				}
 
 				aBuf[numVertsLocal]=tempa;
@@ -677,17 +696,8 @@ namespace Gu
 			}
 			facetManager.freeID(facet->m_FacetId);
 
-			hasMoreFacets = (heap.size() > 0);
-			//after polytope expansion, we don't have a better facet to work with so that we should process the best facet
-			//this sometime won't produce the MTD but the result seems close enough so we accept it as contact.
-			//test case in BenchMark_GJKEPABoxConvex
-			if(hasMoreFacets && FAllGrtr( heap.top()->getPlaneDist(), upper_bound))
-			{
-				calculateContactInformation(aBuf, bBuf, bestFacet, a, b, pa, pb, normal, penDepth, takeCoreShape);
-				return EPA_CONTACT;
-			}
 		}
-		while(hasMoreFacets && numVertsLocal != MaxSupportPoints);
+		while((heap.size() > 0) && FAllGrtr(upper_bound, heap.top()->getPlaneDist()) && numVertsLocal != MaxSupportPoints);
 
 		calculateContactInformation(aBuf, bBuf, facet, a, b, pa, pb, normal, penDepth, takeCoreShape);
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
index 946b2b8e..dee06e90 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuEPAFacet.h
@@ -261,7 +261,7 @@ namespace Gu
 		const FloatV p0dv2 = V3Dot(p0, v2);
 
 		const FloatV det = FNegScaleSub(v1dv2, v1dv2, FMul(v1dv1, v2dv2));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative
-		const FloatV recip = FRecip(det);
+		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);
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuGJKType.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuGJKType.h
index d81a8ca3..e93dd66d 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuGJKType.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuGJKType.h
@@ -57,7 +57,7 @@ namespace Gu
 		PX_FORCE_INLINE Ps::aos::BoolV	isMarginEqRadius()	const { return mConvex.isMarginEqRadius();	}
 		PX_FORCE_INLINE bool			getMarginIsRadius()	const { return mConvex.getMarginIsRadius();	}
 		PX_FORCE_INLINE Ps::aos::FloatV	getMargin()			const { return mConvex.getMargin();			}
-		PX_FORCE_INLINE Ps::aos::Vec3V	getCenter()			const { return mConvex.getCenter();			}
+		
 
 		template <typename Convex>
 		PX_FORCE_INLINE const Convex& getConvex() const { return static_cast<const Convex&>(mConvex); }
@@ -66,6 +66,7 @@ namespace Gu
 		virtual Ps::aos::Vec3V support(const Ps::aos::Vec3VArg v) const = 0;
 		virtual Ps::aos::Vec3V support(const Ps::aos::Vec3VArg dir, PxI32& index, Ps::aos::FloatV* marginDif) const = 0;
 		virtual Ps::aos::FloatV getSweepMargin() const = 0;
+		virtual Ps::aos::Vec3V	getCenter() const = 0;
 		virtual ~GjkConvexBase(){}
 	
 	
@@ -117,6 +118,8 @@ namespace Gu
 			return getConvex<Convex>().supportLocal(dir, index, marginDif);
 		}
 
+		virtual Ps::aos::Vec3V getCenter() const { return getConvex<Convex>().getCenter(); }
+
 		//ML: we can't force inline function, otherwise win modern will throw compiler error
 		PX_INLINE LocalConvex<typename Shrink<Convex>::Type > getShrunkConvex() const 
 		{ 
@@ -149,6 +152,8 @@ namespace Gu
 			return getConvex<Convex>().supportRelative(dir, mAToB, mAToBTransposed, index, marginDif);
 		}
 
+		virtual Ps::aos::Vec3V getCenter() const { return mAToB.transform(getConvex<Convex>().getCenter()); }
+
 		PX_FORCE_INLINE Ps::aos::PsMatTransformV& getRelativeTransform(){ return mAToB; }
 
 		//ML: we can't force inline function, otherwise win modern will throw compiler error
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
index 5edb3705..6787a93e 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecBox.h
@@ -58,20 +58,16 @@ namespace Gu
 	class CapsuleV;
 
 
-	PX_FORCE_INLINE void CalculateBoxMargin(const Ps::aos::Vec3VArg extent, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin,
-		const PxReal marginR = BOX_MARGIN_RATIO, const PxReal minMarginR = BOX_MIN_MARGIN_RATIO)
+	PX_FORCE_INLINE void CalculateBoxMargin(const Ps::aos::Vec3VArg extent, PxReal& minExtent, PxReal& minMargin, PxReal& sweepMargin,
+		 const PxReal minMarginR = BOX_MIN_MARGIN_RATIO)
 	{
 		using namespace Ps::aos;
 
 		const FloatV min = V3ExtractMin(extent);
+		FStore(min, &minExtent);
 
-		const FloatV margin_ = FMul(min, FLoad(marginR));
-		const FloatV minMargin_ = FMul(min, FLoad(minMarginR));
-		const FloatV sweepMargin_ = FMul(min, FLoad(BOX_SWEEP_MARGIN_RATIO));
-
-		FStore(margin_, &margin);
-		FStore(minMargin_, &minMargin);
-		FStore(sweepMargin_, &sweepMargin);
+		minMargin = minExtent * minMarginR;
+		sweepMargin = minExtent * BOX_SWEEP_MARGIN_RATIO;
 	}
 
 	PX_FORCE_INLINE Ps::aos::FloatV CalculateBoxTolerance(const Ps::aos::Vec3VArg extent)
@@ -106,8 +102,9 @@ namespace Gu
 		PX_FORCE_INLINE BoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) : 
 			ConvexV(ConvexType::eBOX, origin), extents(extent)
 		{
-			CalculateBoxMargin(extent, margin, minMargin, sweepMargin);
+			CalculateBoxMargin(extent, minExtent, minMargin, sweepMargin);
 			marginDif = Ps::aos::FZero();
+			margin = 0.f;
 		}
 																		
 		PX_FORCE_INLINE BoxV(const PxGeometry& geom) : ConvexV(ConvexType::eBOX, Ps::aos::V3Zero())
@@ -116,8 +113,9 @@ namespace Gu
 			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
 			const Vec3V extent = Ps::aos::V3LoadU(boxGeom.halfExtents);
 			extents = extent;
-			CalculateBoxMargin(extent, margin, minMargin, sweepMargin, BOX_MARGIN_CCD_RATIO, BOX_MIN_MARGIN_CCD_RATIO);
+			CalculateBoxMargin(extent, minExtent, minMargin, sweepMargin, BOX_MIN_MARGIN_CCD_RATIO);
 			marginDif = Ps::aos::FZero();
+			margin = 0.f;
 		}
 
 		/**
@@ -208,6 +206,7 @@ namespace Gu
 
 		Ps::aos::Vec3V  extents;
 		Ps::aos::FloatV marginDif;
+		PxReal			minExtent;
 	};
 }	//PX_COMPILE_TIME_ASSERT(sizeof(Gu::BoxV) == 96);
 
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvex.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvex.h
index a04e2777..84f2b7ec 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvex.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvex.h
@@ -64,6 +64,7 @@ namespace Gu
 			minMargin = 0.f;
 			sweepMargin = 0.f;
 			center = Ps::aos::V3Zero();
+			maxMargin = PX_MAX_F32;
 		}
 
 		PX_FORCE_INLINE ConvexV(const ConvexType::Type type_, const Ps::aos::Vec3VArg center_) : type(type_), bMarginIsRadius(false)
@@ -73,9 +74,9 @@ namespace Gu
 			margin = 0.f;
 			minMargin = 0.f;
 			sweepMargin = 0.f;
+			maxMargin = PX_MAX_F32;
 		}
 
-
 		//everytime when someone transform the object, they need to up
 		PX_FORCE_INLINE void setCenter(const Ps::aos::Vec3VArg _center)
 		{
@@ -90,6 +91,15 @@ namespace Gu
 		PX_FORCE_INLINE void setMargin(const PxReal margin_)
 		{
 			margin = margin_;
+			//compare with margin and choose the smallest one
+			margin = PxMin<PxReal>(maxMargin, margin);
+		}
+
+		PX_FORCE_INLINE void setMaxMargin(const PxReal maxMargin_)
+		{
+			maxMargin = maxMargin_;
+			//compare with margin and choose the smallest one
+			margin = PxMin<PxReal>(maxMargin, margin);
 		}
 
 		PX_FORCE_INLINE void setMinMargin(const Ps::aos::FloatVArg minMargin_)
@@ -137,12 +147,19 @@ namespace Gu
 			return bMarginIsRadius;
 		}
 
+		PX_FORCE_INLINE PxReal getMarginF() const
+		{
+			return margin;
+		}
+
+
 	protected:
 		~ConvexV(){}
 		Ps::aos::Vec3V center;
-		PxReal margin;		//margin is the amount by which we shrunk the shape for a convex or box. If the shape are sphere/capsule, margin is the radius
-		PxReal minMargin;	//minMargin is some percentage of marginBase, which is used to determine the termination condition for gjk
-		PxReal sweepMargin;// sweepMargin minMargin is some percentage of marginBase, which is used to determine the termination condition for gjkRaycast
+		PxReal margin;				//margin is the amount by which we shrunk the shape for a convex or box. If the shape are sphere/capsule, margin is the radius
+		PxReal minMargin;			//minMargin is some percentage of marginBase, which is used to determine the termination condition for gjk
+		PxReal sweepMargin;			//sweepMargin minMargin is some percentage of marginBase, which is used to determine the termination condition for gjkRaycast
+		PxReal maxMargin;			//the shrunk amount defined by the application
 		ConvexType::Type	type;
 		bool bMarginIsRadius;
 	};
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
index 6014d120..1c2fd660 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHull.h
@@ -91,7 +91,7 @@ namespace Gu
 		return M33MulM33(trans, rot);
 	}
 
-	PX_SUPPORT_FORCE_INLINE void ConstructSkewMatrix(const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg rotation, Ps::aos::Mat33V& vertex2Shape, Ps::aos::Mat33V& shape2Vertex, const bool idtScale) 
+	PX_SUPPORT_FORCE_INLINE void ConstructSkewMatrix(const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg rotation, Ps::aos::Mat33V& vertex2Shape, Ps::aos::Mat33V& shape2Vertex, Ps::aos::Vec3V& center, const bool idtScale) 
 	{
 		using namespace Ps::aos;
 
@@ -141,6 +141,9 @@ namespace Gu
 
 				//shape2Vertex = M33Inverse(vertex2Shape);
 			}
+
+			//transform center to shape space
+			center = M33MulV3(vertex2Shape, center);
 		}
 	}
 
@@ -193,7 +196,7 @@ namespace Gu
 			verts = tempVerts;
 			numVerts = _hullData->mNbHullVertices;
 			CalculateConvexMargin( _hullData, margin, minMargin, sweepMargin, scale);
-			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, idtScale);
+			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);*/
 
@@ -215,7 +218,7 @@ namespace Gu
 			verts = tempVerts;
 			numVerts = hData->mNbHullVertices;
 			CalculateConvexMargin( hData, margin, minMargin, sweepMargin, vScale);
-			ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, idtScale);
+			ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, center, idtScale);
 
 			data = hData->mBigConvexRawData;
 		}
@@ -226,7 +229,7 @@ namespace Gu
 			
 			const PxVec3* tempVerts = _hullData->getHullVertices();
 			CalculateConvexMargin(_hullData, margin, minMargin, sweepMargin, scale);
-			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, idtScale);
+			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
 
 			verts = tempVerts;
 			numVerts = _hullData->mNbHullVertices;
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
index cea44d8f..d4a09435 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecConvexHullNoScale.h
@@ -131,7 +131,7 @@ namespace Gu
 		}
 
 
-		// PT: TODO: is there a difference between 'originalVerts' and the 'verts' class member? Also why is this a member function at all?
+		//This funcation is just to load the PxVec3 to Vec3V. However, for GuVecConvexHul.h, this is used to transform all the verts from vertex space to shape space
 		PX_SUPPORT_INLINE void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts_)const
 		{
 			using namespace Ps::aos;
@@ -163,7 +163,6 @@ namespace Gu
 			using namespace Ps::aos;
 		
 			//transform dir into the shape space
-//			const Vec3V _dir = aTob.rotateInv(dir);//relTra.rotateInv(dir);
 			const Vec3V _dir = aTobT.rotate(dir);//relTra.rotateInv(dir);
 			const Vec3V maxPoint = supportLocal(_dir);
 			//translate maxPoint from shape space of a back to the b space
@@ -189,7 +188,6 @@ namespace Gu
 			using namespace Ps::aos;
 
 			//transform dir from b space to the shape space of a space
-//			const Vec3V _dir = aTob.rotateInv(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
 			const Vec3V _dir = aTobT.rotate(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
 			const Vec3V p = supportLocal(_dir, index, marginDif);
 			//transfrom from a to b space
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
index 076606d4..f8a214d1 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkBox.h
@@ -79,6 +79,7 @@ namespace Gu
 
 		PX_INLINE ShrunkBoxV(const PxGeometry& geom) : BoxV(geom)
 		{
+			margin = minExtent * BOX_MARGIN_CCD_RATIO;
 			initialiseMarginDif();
 		}
 
@@ -92,6 +93,8 @@ namespace Gu
 		PX_FORCE_INLINE ShrunkBoxV(const Ps::aos::Vec3VArg origin, const Ps::aos::Vec3VArg extent) : 
 																				BoxV(origin, extent)
 		{
+			//calculate margin
+			margin = minExtent * BOX_MARGIN_RATIO;
 			initialiseMarginDif();
 		}
 		
@@ -178,9 +181,6 @@ namespace Gu
 			const PxReal tempMarginDif = sqrtf(sqMargin * 3.f);
 			const PxReal marginDif_ = tempMarginDif - margin;
 			marginDif = FLoad(marginDif_);
-			/*	const FloatV sqMargin = FMul(margin, margin);
-			const FloatV tempMarginDif =  FSqrt(FAdd(sqMargin, FAdd(sqMargin, sqMargin)));
-			marginDif = FSub(tempMarginDif, margin);*/
 		}
 	};
 }	
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHull.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHull.h
index c0dec847..50ee49dc 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHull.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHull.h
@@ -78,18 +78,26 @@ namespace Gu
 		{
 		}
 
-		PX_SUPPORT_INLINE ShrunkConvexHullV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, const bool idtScale):
-													ConvexHullV(_hullData, _center, scale, scaleRot, idtScale)
+		PX_SUPPORT_INLINE ShrunkConvexHullV(const Gu::ConvexHullData* hullData_, const Ps::aos::Vec3VArg center_, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot, 
+			const bool idtScale):
+			ConvexHullV(hullData_, center_, scale, scaleRot, idtScale)
 		{
 		}
 
-
-
-
 		PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* marginDif) const
 		{
+			using namespace Ps::aos;
+
+			if (getMarginF() > 0.f)
+			{
+				//p is in the shape space
+				return planeShift(PxU32(index), getMargin(), marginDif);
+			}
+			else
+			{
+				return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index]));
+			}
 			
-			return planeShift(PxU32(index), getMargin(), marginDif);
 		}
 
 		//get the support point in vertex space
@@ -155,8 +163,15 @@ namespace Gu
 			//get the extreme point index
 			const PxU32 maxIndex = supportVertexIndex(_dir);
 			index = PxI32(maxIndex);
-			//p is in the shape space
-			return planeShift(maxIndex, getMargin(), marginDif);
+			if (getMarginF() > 0.f)
+			{
+				//p is in the shape space
+				return planeShift(maxIndex, getMargin(), marginDif);
+			}
+			else
+			{
+				return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index]));
+			}
 		}
 
 	
@@ -173,7 +188,6 @@ namespace Gu
 			//transfrom from a to b space
 			return aTob.transform(p);
 		}
-
 	};
 
 }
diff --git a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHullNoScale.h b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHullNoScale.h
index f64577c6..c6f43f51 100644
--- a/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHullNoScale.h
+++ b/PhysX_3.4/Source/GeomUtils/src/gjk/GuVecShrunkConvexHullNoScale.h
@@ -60,8 +60,9 @@ namespace Gu
 		{
 		}
 
-		PX_SUPPORT_INLINE ShrunkConvexHullNoScaleV(const Gu::ConvexHullData* _hullData, const Ps::aos::Vec3VArg _center, const Ps::aos::Vec3VArg scale, const Ps::aos::QuatVArg scaleRot):
-													ShrunkConvexHullV(_hullData, _center, scale, scaleRot, true)
+		PX_SUPPORT_INLINE ShrunkConvexHullNoScaleV(const Gu::ConvexHullData* hullData_, const Ps::aos::Vec3VArg center_, const Ps::aos::Vec3VArg scale, 
+			const Ps::aos::QuatVArg scaleRot):
+			ShrunkConvexHullV(hullData_, center_, scale, scaleRot, true)
 		{
 		}
 
@@ -69,7 +70,17 @@ namespace Gu
 	
 		PX_FORCE_INLINE Ps::aos::Vec3V supportPoint(const PxI32 index, Ps::aos::FloatV* marginDif) const
 		{
-			return planeShift(PxU32(index), getMargin(), marginDif);
+			using namespace Ps::aos;
+
+			if (getMarginF() > 0)
+			{
+				//p is in the shape space
+				return planeShift(PxU32(index), getMargin(), marginDif);
+			}
+			else
+			{
+				return V3LoadU_SafeReadW(verts[index]);
+			}
 		}
 
 		//get the support point in vertex space
@@ -129,8 +140,15 @@ namespace Gu
 			//get the extreme point index
 			const PxU32 maxIndex = supportVertexIndex(dir);
 			index = PxI32(maxIndex);
-			//p is in the shape space
-			return planeShift(maxIndex, getMargin(), marginDif);
+			if (getMarginF() > 0)
+			{
+				//p is in the shape space
+				return planeShift(maxIndex, getMargin(), marginDif);
+			}
+			else
+			{
+				return V3LoadU_SafeReadW(verts[index]);
+			}
 		}
 
 		PX_SUPPORT_INLINE Ps::aos::Vec3V supportRelative(	const Ps::aos::Vec3VArg dir, const Ps::aos::PsMatTransformV& aTob,
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
index b12925ca..b93df8e3 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
@@ -969,14 +969,14 @@ bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
 			else
 			{
 				const Vec3V zeroV = V3Zero();
-				ShrunkBoxV box0(zeroV, boxExtents0);
-				ShrunkBoxV box1(zeroV, boxExtents1);
+				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;
-				RelativeConvex<ShrunkBoxV> convexA(box0, aToB);
-				LocalConvex<ShrunkBoxV> convexB(box1);
-				GjkStatus status = gjkPenetration<RelativeConvex<ShrunkBoxV>, LocalConvex<ShrunkBoxV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+				RelativeConvex<BoxV> convexA(box0, aToB);
+				LocalConvex<BoxV> convexB(box1);
+				GjkStatus status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<BoxV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
 					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
 
 				if(status == EPA_CONTACT)
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
index 4cd93506..2f3965eb 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
@@ -106,7 +106,7 @@ static bool fullContactsGenerationBoxConvex(const PxVec3& halfExtents, const Box
 
 }
 
-static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull, Gu::ShrunkBoxV& box, const PsMatTransformV& aToB, GjkStatus status,
+static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull, Gu::BoxV& box, const PsMatTransformV& aToB, GjkStatus status,
 	Gu::PersistentContact* manifoldContacts, const FloatV replaceBreakingThreshold, Vec3V& closestA, Vec3V& closestB, Vec3V& normal, FloatV& penDep,
 	Gu::PersistentContactManifold& manifold)
 {
@@ -203,7 +203,7 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 
 	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin))	
 	{
-
+		
 		GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
 
 		Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
@@ -211,20 +211,20 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 		
 		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
 		const bool idtScale = shapeConvex.scale.isIdentity();
-		Gu::ShrunkConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
-		Gu::ShrunkBoxV box(zeroV, boxExtents);
+		Gu::ShrunkConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
+		Gu::BoxV box(zeroV, boxExtents);
 		
-		const RelativeConvex<ShrunkBoxV> convexA(box, aToB);
+		const RelativeConvex<BoxV> convexA(box, aToB);
 		if(idtScale)
 		{
 			const LocalConvex<ShrunkConvexHullNoScaleV> convexB(*PX_SCONVEX_TO_NOSCALECONVEX(&convexHull));
-			status = gjkPenetration<RelativeConvex<ShrunkBoxV>, LocalConvex<ShrunkConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+			status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<ShrunkConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
 				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
 		}
 		else
 		{
 			const LocalConvex<ShrunkConvexHullV> convexB(convexHull);
-			status = gjkPenetration<RelativeConvex<ShrunkBoxV>, LocalConvex<ShrunkConvexHullV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+			status = gjkPenetration<RelativeConvex<BoxV>, LocalConvex<ShrunkConvexHullV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
 				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
 
 		} 
@@ -244,20 +244,30 @@ bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
 		}
 		else
 		{
+			const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
+
 			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
 			
 			//addGJKEPAContacts will increase the number of contacts in manifold. If status == EPA_CONTACT, we need to run epa algorithm and generate closest points, normal and
 			//pentration. If epa doesn't degenerate, we will store the contacts information in the manifold. Otherwise, we will return true to do the fallback test
 			const bool doOverlapTest = addGJKEPAContacts(convexHull, box, aToB, status, manifoldContacts, replaceBreakingThreshold, closestA, closestB, normal, penDep, manifold);
 
-			if ((initialContacts == 0) || (manifold.mNumContacts < initialContacts) || doOverlapTest)
+			//ML: after we refresh the contacts(newContacts) and generate a GJK/EPA contacts(we will store that in the manifold), if the number of contacts is still less than the original contacts,
+			//which means we lose too mang contacts and we should regenerate all the contacts in the current configuration
+			//Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
+			//which means we should throw away the existing contacts and do full contact gen
+			const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, normal)) || (manifold.mNumContacts < initialContacts);
+
+			if (fullContactGen || doOverlapTest)
 			{
 				return fullContactsGenerationBoxConvex(shapeBox.halfExtents, box, convexHull, transf0, transf1, manifoldContacts, contactBuffer, 
 					manifold, normal, closestA, closestB, contactDist, idtScale, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
 			}
 			else
 			{
-				const Vec3V worldNormal = transf1.rotate(normal);
+				const Vec3V newLocalNor = V3Add(localNor, normal);
+				const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
+				//const Vec3V worldNormal = transf1.rotate(normal);
 				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
 				return true;
 			}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
index fff3a19d..1daf1bd6 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
@@ -149,7 +149,7 @@ bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
 
 		manifold.setRelativeTransform(curRTrans);
 		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);  
-		ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
+		ConvexHullV convexHull(hullData, V3LoadU(hullData->mCenterOfMass), vScale, vQuat, idtScale);
 		convexHull.setMargin(zero);
 	
 		//transform capsule(a) into the local space of convexHull(b)
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
index af6fc3d1..82636e10 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
@@ -43,6 +43,7 @@
 namespace physx
 {
 
+
 	using namespace Ps::aos;
 
 namespace Gu
@@ -144,8 +145,8 @@ static GjkStatus convexHullHasScale0(ShrunkConvexHullV& convexHull0, ShrunkConve
 }
 
 
-static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull0, Gu::ShrunkConvexHullV& convexHull1, const PsMatTransformV& aToB, GjkStatus status, 
-	Gu::PersistentContact* manifoldContacts, const FloatV replaceBreakingThreshold, Vec3V& closestA, Vec3V& closestB, Vec3V& normal, FloatV& penDep, 
+static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull0, Gu::ShrunkConvexHullV& convexHull1, const PsMatTransformV& aToB, 
+	GjkStatus& status, Gu::PersistentContact* manifoldContacts, const FloatV replaceBreakingThreshold, Vec3V& closestA, Vec3V& closestB, Vec3V& normal, FloatV& penDep, 
 	Gu::PersistentContactManifold& manifold)
 {
 	bool doOverlapTest = false;
@@ -232,7 +233,6 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 	//ML: after refreshContactPoints, we might lose some contacts
 	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
 
-	PX_UNUSED(bLostContacts);
 	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin))
 	{
 		GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
@@ -242,9 +242,11 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 		const QuatV vQuat0 = QuatVLoadU(&shapeConvex0.scale.rotation.x);
 		const QuatV vQuat1 = QuatVLoadU(&shapeConvex1.scale.rotation.x);
 		const Vec3V zeroV = V3Zero();
-		Gu::ShrunkConvexHullV convexHull0(hullData0, zeroV, vScale0, vQuat0, idtScale0);
-		Gu::ShrunkConvexHullV convexHull1(hullData1, zeroV, vScale1, vQuat1, idtScale1);
 
+
+		Gu::ShrunkConvexHullV convexHull0(hullData0, V3LoadU(hullData0->mCenterOfMass), vScale0, vQuat0, idtScale0);
+		Gu::ShrunkConvexHullV convexHull1(hullData1, V3LoadU(hullData1->mCenterOfMass), vScale1, vQuat1, idtScale1);
+	
 		Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
 		FloatV penDep =  FZero();
 
@@ -273,19 +275,28 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 		else
 		{
 			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
-			
-			const bool doOverlapTest = addGJKEPAContacts(convexHull0, convexHull1, aToB, status, manifoldContacts, replaceBreakingThreshold, closestA, closestB, normal, penDep, manifold);
 
+			const Vec3V localNor = manifold.mNumContacts ? manifold.getLocalNormal() : V3Zero();
+			
+			const bool doOverlapTest = addGJKEPAContacts(convexHull0, convexHull1, aToB,
+				status, manifoldContacts, replaceBreakingThreshold, closestA, closestB, normal, penDep, manifold);
+			
 			//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
-			if (initialContacts == 0 || (manifold.mNumContacts < initialContacts) || doOverlapTest)
+			//Also, we need to look at the existing contacts, if the existing contacts has very different normal than the GJK/EPA contacts,
+			//which means we should throw away the existing contacts and do full contact gen
+			const bool fullContactGen = FAllGrtr(FLoad(0.707106781f), V3Dot(localNor, normal)) || (manifold.mNumContacts < initialContacts);
+			if (fullContactGen  || doOverlapTest)
 			{
 				return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, 
 					manifold, normal, closestA, closestB, contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
 			}
 			else
 			{
-				const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+				const Vec3V newLocalNor = V3Add(localNor, normal);
+				const Vec3V worldNormal = V3Normalize(transf1.rotate(newLocalNor));
+				
+				//const Vec3V worldNormal = manifold.getWorldNormal(transf1);
 				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
 				return true;
 			}
@@ -306,5 +317,6 @@ bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
 	
 }
 
+
 }
 }
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
index 48f53645..8ba4e2d7 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
@@ -219,13 +219,13 @@ bool Gu::pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS)
 
 	if(idtScaleConvex)
 	{
-		SupportLocalShrunkImpl<Gu::ConvexHullNoScaleV, Gu::ShrunkConvexHullNoScaleV> convexMap(static_cast<ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
+		SupportLocalImpl<Gu::ConvexHullNoScaleV> convexMap(static_cast<ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
 		return Gu::PCMContactConvexHeightfield(polyData, &convexMap, minMargin, hullAABB, shapHeightField, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling, 
 			idtScaleConvex, multiManifold, renderOutput);
 	}
 	else
 	{
-		SupportLocalShrunkImpl<Gu::ConvexHullV, Gu::ShrunkConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
+		SupportLocalImpl<Gu::ConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex);
 		return Gu::PCMContactConvexHeightfield(polyData, &convexMap, minMargin, hullAABB, shapHeightField, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling, 
 			idtScaleConvex, multiManifold, renderOutput);
 	}
@@ -266,7 +266,7 @@ bool Gu::pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS)
 
 	Mat33V identity =  M33Identity();
 	//SupportLocalImpl<Gu::BoxV> boxMap(boxV, boxTransform, identity, identity);
-	SupportLocalShrunkImpl<Gu::BoxV, Gu::ShrunkBoxV> boxMap(boxV, boxTransform, identity, identity, true);
+	SupportLocalImpl<Gu::BoxV> boxMap(boxV, boxTransform, identity, identity, true);
 
 	return Gu::PCMContactConvexHeightfield(polyData, &boxMap, minMargin, hullAABB, shapHeightField, transform0, transform1, params.mContactDistance, contactBuffer, 
 		idtScaling, true, multiManifold, renderOutput);
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
index 4937f9b8..de5d3b1b 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
@@ -42,7 +42,6 @@ using namespace physx;
 using namespace Gu;
 using namespace Ps::aos;
 
-
 	//Precompute the convex data
 	//     7+------+6			0 = ---
 	//     /|     /|			1 = +--
@@ -566,7 +565,6 @@ 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)
@@ -659,15 +657,14 @@ EdgeTest:
 		}
 		else
 		{
-			const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
-			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_EPS));
+			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);
 
-			//use gjk normal to get the faceIndex(status == GJK_CONTACT)
-			const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, V3Neg(normal), closestB, toleranceA);
-			const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, transform0To1V.rotateInv(normal), transform0To1V.transformInv(closestA), toleranceB);
+			const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, V3Neg(normal), closestB, toleranceB);
+			const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, transform0To1V.rotateInv(normal), transform0To1V.transformInv(closestA), toleranceA);
 
 			const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
 			const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
index a9e6c87d..188ea3eb 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
@@ -324,8 +324,8 @@ namespace Gu
 		}
 		else
 		{
-			const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
-			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_EPS));
+			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);
 
 			referencePolygon = &polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance)];
@@ -374,8 +374,8 @@ namespace Gu
 			const PxU32 faceContacts = numContacts - originalContacts;
 			if(faceContacts < 2)
 			{
-				const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
-				const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_EPS));
+				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 Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, toleranceA)];
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
index 90375aeb..8469481e 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
@@ -36,8 +36,10 @@
 #include "GuVecCapsule.h"
 #include "GuConvexSupportTable.h"
 
-#define	PCM_WITNESS_POINT_EPS				1e-1f
-#define	PCM_WITNESS_POINT_ABSOLUTE_EPS		1e-5f
+//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
 
 namespace physx
 {
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
index 9bc30b14..fde67300 100644
--- a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
@@ -1098,8 +1098,6 @@ namespace physx
 	
 		using namespace Ps::aos;
 
-		const FloatV threshold = FLoad(0.7071f);//about 45 degree0
-		PX_UNUSED(threshold);
 		{
 				
 			FeatureStatus status = POLYDATA0;
@@ -1139,10 +1137,10 @@ namespace physx
 				{
 					const Gu::HullPolygonData* referencePolygon = &polyData.mPolygons[feature1];
 			
-					const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(referencePolygon->mPlane.n)));
-					const Vec3V nContactNormal = V3Neg(contactNormal);
-					const FloatV cosTheta = V3Dot(nContactNormal, triNormal);
-
+					const FloatV cosTheta = V3Dot(V3Neg(minNormal), triNormal);
+					
+					const FloatV threshold = FLoad(0.707106781f);//about 45 degree0
+					
 					if(FAllGrtr(cosTheta, threshold))
 					{
 						patchNormal = triNormal;