Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

33 files changed, 13154 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
new file mode 100644
index 00000000..28417e66
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxBox.cpp
@@ -0,0 +1,1029 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+#include "GuVecBox.h"
+#include "GuVecShrunkBox.h"
+#include "GuGeometryUnion.h"
+
+#include "GuConvexHelper.h"
+#include "GuPCMShapeConvex.h"
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactGenUtil.h"
+
+
+namespace physx
+{
+
+namespace Gu
+{
+
+	static void getIncidentPolygon(Ps::aos::Vec3V* pts, Ps::aos::Vec3V& faceNormal, const Ps::aos::Vec3VArg axis, const Ps::aos::PsMatTransformV& transf1To0, 
+							   const Ps::aos::Vec3VArg extents)
+	{
+
+		using namespace Ps::aos;
+
+		const FloatV zero = FZero();
+
+		FloatV ex = V3GetX(extents);
+		FloatV ey = V3GetY(extents);
+		FloatV ez = V3GetZ(extents);
+
+		const Vec3V u0 = transf1To0.getCol0();
+		const Vec3V u1 = transf1To0.getCol1();
+		const Vec3V u2 = transf1To0.getCol2();
+
+		//calcaulte the insident face for b
+		const FloatV d0 = V3Dot(u0, axis);
+		const FloatV d1 = V3Dot(u1, axis);
+		const FloatV d2 = V3Dot(u2, axis);
+
+		const FloatV absd0 = FAbs(d0);
+		const FloatV absd1 = FAbs(d1);
+		const FloatV absd2 = FAbs(d2);
+
+		Vec3V r0, r1, r2;
+
+
+		if(FAllGrtrOrEq(absd0, absd1) && FAllGrtrOrEq(absd0, absd2))
+		{
+			//the incident face is on u0
+			const BoolV con = FIsGrtr(d0, zero);
+			faceNormal = V3Sel(con, V3Neg(u0), u0);
+			ex = FSel(con, FNeg(ex), ex);
+			r0 = V3Scale(u0, ex);
+			r1 = V3Scale(u1, ey);
+			r2 = V3Scale(u2, ez);
+
+			const Vec3V temp0 = V3Add(transf1To0.p, r0);
+			const Vec3V temp1 = V3Add(r1, r2);
+			const Vec3V temp2 = V3Sub(r1, r2);
+			
+
+			pts[0] = V3Add(temp0, temp1);	// (-x/x,  y,  z)
+			pts[1] = V3Add(temp0, temp2);	// (-x/x,  y, -z)
+			pts[2] = V3Sub(temp0, temp1);	// (-x/x, -y, -z)
+			pts[3] = V3Sub(temp0, temp2);	// (-x/x, -y,  z)
+
+		}
+		else if(FAllGrtrOrEq(absd1, absd2))
+		{
+			//the incident face is on u1
+			const BoolV con = FIsGrtr(d1, zero);
+			faceNormal = V3Sel(con, V3Neg(u1), u1);
+			ey = FSel(con, FNeg(ey), ey);
+			r0 = V3Scale(u0, ex);
+			r1 = V3Scale(u1, ey);
+			r2 = V3Scale(u2, ez);
+
+			const Vec3V temp0 = V3Add(transf1To0.p, r1);
+			const Vec3V temp1 = V3Add(r0, r2);
+			const Vec3V temp2 = V3Sub(r0, r2);
+			
+			pts[0] = V3Add(temp0, temp1);	// (x,  -y/y,  z)
+			pts[1] = V3Add(temp0, temp2);	// (x,  -y/y, -z)
+			pts[2] = V3Sub(temp0, temp1);	// (-x, -y/y, -z)
+			pts[3] = V3Sub(temp0, temp2);	// (-x, -y/y,  z)
+
+		}
+		else
+		{
+			//the incident face is on u2
+			const BoolV con = FIsGrtr(d2, zero);
+			faceNormal = V3Sel(con, V3Neg(u2), u2);
+			ez = FSel(con, FNeg(ez), ez);
+			r0 = V3Scale(u0, ex);
+			r1 = V3Scale(u1, ey);
+			r2 = V3Scale(u2, ez);
+
+			const Vec3V temp0 = V3Add(transf1To0.p, r2);
+			const Vec3V temp1 = V3Add(r0, r1);
+			const Vec3V temp2 = V3Sub(r0, r1);
+			
+			pts[0] = V3Add(temp0, temp1);	// ( x,   y,  z)
+			pts[1] = V3Add(temp0, temp2);	// ( x,  -y,  z)
+			pts[2] = V3Sub(temp0, temp1);	// (-x,  -y,  z)
+			pts[3] = V3Sub(temp0, temp2);	// (-x,   y,  z)
+		}
+	}
+
+
+	//p0 and p1 is in the local space of AABB
+	static bool intersectSegmentAABB(const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg d, const Ps::aos::Vec3VArg max, const Ps::aos::Vec3VArg min, Ps::aos::FloatV& tmin, Ps::aos::FloatV& tmax)
+	{
+		using namespace Ps::aos;
+		
+		const Vec3V eps = V3Load(1e-6);
+		const Vec3V absV = V3Abs(d);
+		const FloatV one = FOne();
+		const Vec3V zero = V3Zero();
+		const Vec3V fMax = Vec3V_From_FloatV(FMax());
+
+		FloatV tminf = FZero();
+		FloatV tmaxf = one;
+		const BoolV isParallel = V3IsGrtr(eps, absV);
+		const BoolV isOutsideOfRange = BOr(V3IsGrtr(p0, max), V3IsGrtr(min, p0));
+		//const BoolV isParallelAndOutOfRange = BAnd(isParallel, isOutsideOfRange);
+
+		if(!BAllEqFFFF(BAnd(isParallel, isOutsideOfRange)))
+		{
+			return false;
+		}
+
+		const Vec3V odd = V3RecipFast(d);
+		const Vec3V t1 = V3Sel(isParallel, zero, V3Mul(V3Sub(min, p0), odd));
+		const Vec3V t2 = V3Sel(isParallel, fMax, V3Mul(V3Sub(max, p0), odd));
+		
+		const Vec3V tt1 = V3Min(t1, t2);
+		const Vec3V tt2 = V3Max(t1, t2);
+
+		const FloatV ft1 = V3ExtractMax(tt1);
+		const FloatV ft2 = V3ExtractMin(tt2);
+
+		tminf = FMax(ft1, tminf);
+		tmaxf = FMin(tmaxf, ft2);
+
+		tmin = tminf;
+		tmax = tmaxf;
+
+		const BoolV con0 = FIsGrtr(tminf, tmaxf);
+		const BoolV con1 = FIsGrtr(tminf, one);
+		const BoolV isNotIntersect = BOr(con0, con1);
+		return BAllEqFFFF(isNotIntersect) == 1;
+	}
+
+
+	//pts, faceNormal and contact normal are in the local space of new space
+	static void calculateContacts( const Ps::aos::FloatVArg extentX, const Ps::aos::FloatVArg extentY, Ps::aos::Vec3V* pts, const Ps::aos::Vec3VArg incidentFaceNormalInNew, const Ps::aos::Vec3VArg localNormal, Gu::PersistentContact* manifoldContacts, PxU32& numContacts, const Ps::aos::FloatVArg contactDist)
+	{
+		using namespace Ps::aos;
+
+		const FloatV zero = FZero();
+		const FloatV max = FMax();
+
+		const FloatV nExtentX = FNeg(extentX);
+		const FloatV nExtentY = FNeg(extentY);
+		
+		bool pPenetration[4];
+		bool pArea[4];
+
+		Vec3V bmin = V3Splat(max);
+		Vec3V bmax = V3Neg(bmin);
+
+		const Vec3V bound = V3Merge(extentX, extentY, max);
+
+
+		//get the projection point of pts
+		for(PxU32 i=0; i< 4; ++i)
+		{
+			bmin = V3Min(bmin, pts[i]);
+			bmax = V3Max(bmax, pts[i]);
+			const FloatV z = FNeg(V3GetZ(pts[i]));
+			if(FAllGrtr(contactDist, z))
+			{
+	
+				pPenetration[i] = true;
+
+				const Vec3V absPt= V3Abs(pts[i]);
+				const BoolV con = V3IsGrtrOrEq(bound, absPt);
+				if(BAllEqTTTT(con))
+				{
+					pArea[i] = true;
+				
+					//Add the point to the manifold
+					manifoldContacts[numContacts].mLocalPointA = V3SetZ(pts[i], zero); //transformNewTo0.transform(localPointA);
+					manifoldContacts[numContacts].mLocalPointB = pts[i];//transform1ToNew.transformInv(pts[i]);
+					manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), z);
+				}
+				else
+				{
+					pArea[i] = false;
+				}
+			}
+			else
+			{
+				pPenetration[i] = false;
+				pArea[i] = false;
+			}
+
+		}
+
+		if(numContacts == 4)
+			return;
+
+		//if(pPenetration[0] && pPenetration[1] && pPenetration[2] && pPenetration[3])
+		{
+			//if(!pArea[0] || !pArea[1] || !pArea[2] || !pArea[3])
+			{
+				const FloatV denom = V3GetZ(incidentFaceNormalInNew);
+				{
+					const Vec3V q0 = V3Merge(extentX, extentY, zero);
+
+					if(contains(pts, 4, q0, bmin, bmax))
+					{
+						const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
+						const FloatV t = FDiv(nom, denom);
+						const FloatV pen = FNeg(t);
+						if(FAllGrtr(contactDist, pen))
+						{
+							manifoldContacts[numContacts].mLocalPointA = q0;
+							manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t); 
+							manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+						}
+					}
+				}
+			
+				{
+					const Vec3V q0 = V3Merge(extentX, nExtentY, zero);
+					if(contains(pts, 4, q0, bmin, bmax))
+					{
+						const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
+						const FloatV t = FDiv(nom, denom);
+						const FloatV pen = FNeg(t);
+						if(FAllGrtr(contactDist, pen))
+						{
+							manifoldContacts[numContacts].mLocalPointA = q0;
+							manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
+							manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+						}
+					}
+				}
+				
+				{
+					const Vec3V q0 = V3Merge( nExtentX, extentY, zero);
+					if(contains(pts, 4, q0, bmin, bmax))
+					{
+						const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
+						const FloatV t = FDiv(nom, denom);
+						const FloatV pen = FNeg(t);
+						if(FAllGrtr(contactDist, pen))
+						{
+							manifoldContacts[numContacts].mLocalPointA = q0;
+							manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
+							manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+						}
+					}
+				}
+			
+				{
+					const Vec3V q0 = V3Merge(nExtentX, nExtentY, zero);
+					
+					if(contains(pts, 4, q0, bmin, bmax))
+					{
+						const FloatV nom = V3Dot(incidentFaceNormalInNew, V3Sub(pts[0], q0));
+						const FloatV t = FDiv(nom, denom);
+						const FloatV pen = FNeg(t);
+						if(FAllGrtr(contactDist, pen))
+						{
+							manifoldContacts[numContacts].mLocalPointA = q0;
+							manifoldContacts[numContacts].mLocalPointB = V3SetZ(q0, t);
+							manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+						}
+					}
+				}
+			}
+		}
+
+
+		   
+		const Vec3V ext = V3Merge(extentX, extentY, max);
+		const Vec3V negExt = V3Merge(nExtentX, nExtentY, FNeg(FAdd(contactDist, FEps())));
+
+		for (PxU32 rStart = 0, rEnd = 3; rStart < 4; rEnd = rStart++)
+		{
+			const Vec3V p0 = pts[rStart];	
+			const Vec3V p1 = pts[rEnd];
+
+			if(!pPenetration[rStart] &&  !pPenetration[rEnd])
+				continue;
+
+			const bool con0 = pPenetration[rStart] && pArea[rStart];
+			const bool con1 = pPenetration[rEnd] && pArea[rEnd];
+			if(con0 && con1)
+				continue;
+
+			//intersect t value with x plane
+			const Vec3V p0p1 = V3Sub(p1, p0);
+
+			FloatV tmin, tmax;
+			if(Gu::intersectSegmentAABB(p0, p0p1, ext, negExt, tmin, tmax))
+			{
+				if(!con0)
+				{
+					const Vec3V intersectP = V3ScaleAdd(p0p1, tmin, p0);
+					manifoldContacts[numContacts].mLocalPointA = V3SetZ(intersectP, zero);
+					manifoldContacts[numContacts].mLocalPointB = intersectP;
+					manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), FNeg(V3GetZ(intersectP)));
+				}
+				if(!con1)
+				{
+					const Vec3V intersectP = V3ScaleAdd(p0p1, tmax, p0);
+					manifoldContacts[numContacts].mLocalPointA = V3SetZ(intersectP, zero);
+					manifoldContacts[numContacts].mLocalPointB =  intersectP;
+					manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), FNeg(V3GetZ(intersectP)));
+				}
+			}
+		}
+}
+
+
+static PxU32 doBoxBoxGenerateContacts(const Ps::aos::Vec3VArg box0Extent, const Ps::aos::Vec3VArg box1Extent, const Ps::aos::PsMatTransformV& transform0, const Ps::aos::PsMatTransformV& transform1, const Ps::aos::FloatVArg contactDist, Gu::PersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	using namespace Ps::aos;
+
+	const FloatV ea0 = V3GetX(box0Extent);
+	const FloatV ea1 = V3GetY(box0Extent);
+	const FloatV ea2 = V3GetZ(box0Extent);
+
+	const FloatV eb0 = V3GetX(box1Extent);
+	const FloatV eb1 = V3GetY(box1Extent);
+	const FloatV eb2 = V3GetZ(box1Extent);
+
+
+	const PsMatTransformV transform1To0 = transform0.transformInv(transform1);
+	const Mat33V rot0To1 =M33Trnsps(transform1To0.rot);
+
+	const Vec3V uEps = V3Load(1e-6f); 
+
+	const FloatV zero = FZero();
+
+	const FloatV tx = V3GetX(transform1To0.p);
+	const FloatV ty = V3GetY(transform1To0.p);
+	const FloatV tz = V3GetZ(transform1To0.p);
+	const Vec3V col0 = transform1To0.getCol0();
+	const Vec3V col1 = transform1To0.getCol1();
+	const Vec3V col2 = transform1To0.getCol2();
+
+	const Vec3V abs1To0Col0 = V3Add(V3Abs(col0), uEps);
+	const Vec3V abs1To0Col1 = V3Add(V3Abs(col1), uEps);
+	const Vec3V abs1To0Col2 = V3Add(V3Abs(col2), uEps);
+
+	const Vec3V abs0To1Col0 = V3Add(V3Abs(rot0To1.col0), uEps);
+	const Vec3V abs0To1Col1 = V3Add(V3Abs(rot0To1.col1), uEps);
+	const Vec3V abs0To1Col2 = V3Add(V3Abs(rot0To1.col2), uEps);
+
+	
+	FloatV sign[6];
+	FloatV overlap[6];
+
+	FloatV ra, rb, radiusSum;
+	//ua0
+	{
+		
+		sign[0] = tx;
+
+		const Vec3V vtemp3 = V3Mul(abs0To1Col0, box1Extent);
+		rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+
+		radiusSum = FAdd(ea0, rb);
+		overlap[0] =  FAdd(FSub(radiusSum, FAbs(sign[0])), contactDist);
+		if(FAllGrtr(zero, overlap[0]))
+			return false;
+	}
+
+	//ua1
+	{
+		sign[1] = ty;
+
+		const Vec3V vtemp3 = V3Mul(abs0To1Col1, box1Extent);
+		rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+
+		radiusSum = FAdd(ea1, rb);
+		overlap[1] =  FAdd(FSub(radiusSum, FAbs(sign[1])), contactDist);
+		if(FAllGrtr(zero, overlap[1]))
+			return false;
+
+	}
+
+
+	//ua2	
+	{
+		sign[2] = tz;
+		ra = ea2;
+
+		const Vec3V vtemp3 = V3Mul(abs0To1Col2, box1Extent);
+		rb = FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+
+		radiusSum = FAdd(ea2, rb);
+		overlap[2] =  FAdd(FSub(radiusSum, FAbs(sign[2])), contactDist);
+		if(FAllGrtr(zero, overlap[2]))
+			return false;
+		
+	}
+   
+	//ub0 
+	{
+		sign[3] = V3Dot(transform1To0.p, col0);
+
+		const Vec3V vtemp3 = V3Mul(abs1To0Col0, box0Extent);
+		ra =  FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+
+		radiusSum = FAdd(ra, eb0);
+		overlap[3] =  FAdd(FSub(radiusSum, FAbs(sign[3])), contactDist);
+		if(FAllGrtr(zero, overlap[3]))
+			return false;
+	
+	}
+
+	//ub1
+	{
+		sign[4] = V3Dot(transform1To0.p, col1);
+
+		const Vec3V vtemp3 = V3Mul(abs1To0Col1, box0Extent);
+		ra =  FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+
+		radiusSum = FAdd(ra, eb1);
+		overlap[4] =  FAdd(FSub(radiusSum, FAbs(sign[4])), contactDist);
+		if(FAllGrtr(zero, overlap[4]))
+			return false;
+	}
+
+	//ub2
+	{
+		sign[5] = V3Dot(transform1To0.p, col2);
+
+		const Vec3V vtemp3 = V3Mul(abs1To0Col2, box0Extent);
+		ra =  FAdd(V3GetX(vtemp3), FAdd(V3GetY(vtemp3), V3GetZ(vtemp3)));
+		
+		radiusSum = FAdd(ra, eb2);
+		overlap[5] =  FAdd(FSub(radiusSum, FAbs(sign[5])), contactDist);
+		if(FAllGrtr(zero, overlap[5]))
+			return false;
+	}
+
+
+	//ua0 X ub0
+	{
+		//B into A's space, ua0Xub0[0,-b3, b2]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetY(col0), tz), FMul(V3GetZ(col0), ty)));
+
+		//B into A's space, ua0Xub0[0,-b3, b2]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col0), ea1);
+		const FloatV vtemp1 = FMul(V3GetY(abs1To0Col0), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub0[0, a3, -a2]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col0), eb1);
+		const FloatV vtemp02 = FMul(V3GetY(abs0To1Col0), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum)) return false;
+	}
+
+	//ua0 X ub1
+	{
+		//B into A's space, ua0Xub0[0, -b3, b2]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetY(col1), tz), FMul(V3GetZ(col1), ty)));
+
+		//B into A's space, ua0Xub0[0, -b3, b2]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col1), ea1);
+		const FloatV vtemp1 = FMul(V3GetY(abs1To0Col1), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[-a3, 0, a1]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col0), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col0), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+
+		if(FAllGrtr(absSign, radiusSum)) return false;
+
+	}
+
+	//ua0 X ub2
+	{
+		//B into A's space, ua0Xub0[0, -b3, b2]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetY(col2), tz), FMul(V3GetZ(col2), ty)));
+
+
+		//B into A's space, ua0Xub0[0, -b3, b2]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col2), ea1);
+		const FloatV vtemp1 = FMul(V3GetY(abs1To0Col2), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[a2, -a1, 0]
+		const FloatV vtemp01 = FMul(V3GetY(abs0To1Col0), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col0), eb1);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum)) return false;
+
+	}
+
+	//ua1 X ub0
+	{
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetZ(col0), tx), FMul(V3GetX(col0), tz)));
+
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col0), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col0), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[0, a3, -a2]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col1), eb1);
+		const FloatV vtemp02 = FMul(V3GetY(abs0To1Col1), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+
+	}
+
+	//ua1 X ub1
+	{
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetZ(col1), tx), FMul(V3GetX(col1), tz)));
+
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col1), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col1), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[-a3, 0, -a1]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col1), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col1), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+	}
+
+	//ua1 X ub2
+	{
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV absSign=FAbs(FSub(FMul(V3GetZ(col2), tx), FMul(V3GetX(col2), tz)));
+
+		//B into A's space, ua0Xub0[b3, 0, -b1]
+		const FloatV vtemp0 = FMul(V3GetZ(abs1To0Col2), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col2), ea2);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[a2, -a1, 0]
+		const FloatV vtemp01 = FMul(V3GetY(abs0To1Col1), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col1), eb1);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+	}
+
+	//ua2 X ub0
+	{
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetX(col0), ty), FMul(V3GetY(col0), tx)));
+
+
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV vtemp0 = FMul(V3GetY(abs1To0Col0), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col0), ea1);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[0, a3, -a2]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col2), eb1);
+		const FloatV vtemp02 = FMul(V3GetY(abs0To1Col2), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+	}
+
+	//ua2 X ub1
+	{
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV absSign = FAbs(FSub(FMul(V3GetX(col1), ty), FMul(V3GetY(col1), tx)));
+
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV vtemp0 = FMul(V3GetY(abs1To0Col1), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col1), ea1);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[-a3, 0, a1]
+		const FloatV vtemp01 = FMul(V3GetZ(abs0To1Col2), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col2), eb2);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+	}
+
+	//ua2 X ub2
+	{
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV absSign=FAbs(FSub(FMul(V3GetX(col2), ty), FMul(V3GetY(col2), tx)));
+
+		//B into A's space, ua2Xub0[-b2, b1, 0]
+		const FloatV vtemp0 = FMul(V3GetY(abs1To0Col2), ea0);
+		const FloatV vtemp1 = FMul(V3GetX(abs1To0Col2), ea1);
+		ra = FAdd(vtemp0, vtemp1);
+
+		//A into B's space, ua0Xub1[a2, -a1, 0]
+		const FloatV vtemp01 = FMul(V3GetY(abs0To1Col2), eb0);
+		const FloatV vtemp02 = FMul(V3GetX(abs0To1Col2), eb1);
+		rb = FAdd(vtemp01, vtemp02);
+		
+		radiusSum = FAdd(FAdd(ra, rb), contactDist);
+		if(FAllGrtr(absSign, radiusSum))return false;
+	}
+
+	Vec3V mtd;
+
+	PxU32 feature = 0;
+	FloatV minOverlap = overlap[0];
+
+	for(PxU32 i=1; i<6; ++i)
+	{
+		if(FAllGrtr(minOverlap, overlap[i]))
+		{
+			minOverlap = overlap[i];
+			feature = i;
+		}
+	}
+
+
+	PsMatTransformV newTransformV;
+	const Vec3V axis00 = transform0.getCol0();
+	const Vec3V axis01 = transform0.getCol1();
+	const Vec3V axis02 = transform0.getCol2();
+	const Vec3V axis10 = transform1.getCol0();
+	const Vec3V axis11 = transform1.getCol1();
+	const Vec3V axis12 = transform1.getCol2();
+
+	Vec3V incidentFaceNormalInNew;
+	Vec3V pts[4];
+	bool flip = false;
+	switch(feature)
+	{
+	case 0: //ua0
+		{
+
+			
+			if(FAllGrtrOrEq(zero, sign[0]))	
+			{
+				mtd = axis00;
+				newTransformV.rot.col0 = V3Neg(axis02);
+				newTransformV.rot.col1 = axis01;
+				newTransformV.rot.col2 = axis00;
+				newTransformV.p = V3NegScaleSub(axis00, ea0, transform0.p);
+			}
+			else		
+			{
+
+				const Vec3V nAxis00 = V3Neg(axis00);
+				mtd = nAxis00;
+				newTransformV.rot.col0 = axis02;
+				newTransformV.rot.col1 = axis01;
+				newTransformV.rot.col2 = nAxis00;
+				newTransformV.p = V3ScaleAdd(axis00, ea0, transform0.p);
+				
+			}
+	
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
+		
+			calculateContacts(ea2, ea1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+			
+
+			break;
+		};
+	case 1: //ua1
+		{
+			
+			if(FAllGrtrOrEq(zero, sign[1]))	
+			{
+				mtd = axis01;
+				newTransformV.rot.col0 = axis00;
+				newTransformV.rot.col1 = V3Neg(axis02);
+				newTransformV.rot.col2 = axis01;	
+				newTransformV.p = V3NegScaleSub(axis01, ea1, transform0.p); 
+
+			}
+			else		
+			{
+
+
+				const Vec3V nAxis01 = V3Neg(axis01);
+				mtd = nAxis01;
+				newTransformV.rot.col0 = axis00;
+				newTransformV.rot.col1 = axis02;
+				newTransformV.rot.col2 = nAxis01;	
+				newTransformV.p = V3ScaleAdd(axis01, ea1, transform0.p);
+			}
+
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
+
+			calculateContacts(ea0, ea2, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+
+			break;
+
+		};
+	case 2: //ua2
+		{
+
+			if(FAllGrtrOrEq(zero, sign[2]))	
+			{
+				mtd = axis02;
+				newTransformV.rot.col0 = axis00;
+				newTransformV.rot.col1 = axis01;
+				newTransformV.rot.col2 = axis02;	
+				
+				newTransformV.p = V3NegScaleSub(axis02, ea2, transform0.p); 
+			}
+			else		
+			{
+				const Vec3V nAxis02 = V3Neg(axis02);
+				mtd = nAxis02;
+				newTransformV.rot.col0 = axis00;
+				newTransformV.rot.col1 = V3Neg(axis01);
+				newTransformV.rot.col2 = nAxis02;
+				newTransformV.p = V3ScaleAdd(axis02, ea2, transform0.p);
+				
+			}
+
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform1);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, V3Neg(localNormal), transform1ToNew, box1Extent);
+
+			calculateContacts(ea0, ea1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+
+			
+			break;
+		};
+	case 3: //ub0
+		{
+		
+			flip = true;
+			if(FAllGrtrOrEq(zero, sign[3]))	
+			{
+				mtd = axis10;
+				newTransformV.rot.col0 = axis12;
+				newTransformV.rot.col1 = axis11;
+				newTransformV.rot.col2 = V3Neg(axis10);
+				newTransformV.p = V3ScaleAdd(axis10, eb0, transform1.p); //transform0.p - extents0.x*axis00;
+			}
+			else		
+			{
+				mtd = V3Neg(axis10);
+				newTransformV.rot.col0 = V3Neg(axis12);
+				newTransformV.rot.col1 = axis11;
+				newTransformV.rot.col2 = axis10;
+				newTransformV.p =V3NegScaleSub(axis10, eb0, transform1.p);//transform0.p + extents0.x*axis00;
+			}
+
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
+
+			calculateContacts(eb2, eb1, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+			
+			break;
+		};
+	case 4: //ub1;
+		{
+			flip = true;
+			if(FAllGrtrOrEq(zero, sign[4]))	
+			{
+				mtd = axis11;
+				newTransformV.rot.col0 = axis10;
+				newTransformV.rot.col1 = axis12;
+				newTransformV.rot.col2 = V3Neg(axis11);
+			
+				newTransformV.p = V3ScaleAdd(axis11, eb1, transform1.p);
+			
+			}
+			else		
+			{
+				mtd = V3Neg(axis11);
+
+				newTransformV.rot.col0 = axis10;
+				newTransformV.rot.col1 = V3Neg(axis12);
+				newTransformV.rot.col2 = axis11;
+				newTransformV.p = V3NegScaleSub(axis11, eb1, transform1.p); //transform0.p + extents0.x*axis00;
+				
+			}
+
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
+			calculateContacts(eb0, eb2, pts, incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+			break;
+		}
+	case 5: //ub2;
+		{
+
+			flip = true;
+			if(FAllGrtrOrEq(zero, sign[5]))	
+			{
+				mtd = axis12;
+				newTransformV.rot.col0 = axis10;
+				newTransformV.rot.col1 = V3Neg(axis11);
+				newTransformV.rot.col2 = V3Neg(axis12);
+				newTransformV.p = V3ScaleAdd(axis12, eb2, transform1.p);
+			}
+			else		
+			{
+				mtd = V3Neg(axis12);
+				
+				newTransformV.rot.col0 = axis10;
+				newTransformV.rot.col1 = axis11;
+				newTransformV.rot.col2 = axis12;
+				newTransformV.p = V3NegScaleSub(axis12, eb2, transform1.p);
+			}
+
+			const Ps::aos::PsMatTransformV transform1ToNew = newTransformV.transformInv(transform0);
+			const Vec3V localNormal =newTransformV.rotateInv(mtd);
+			getIncidentPolygon(pts, incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
+		
+			calculateContacts(eb0, eb1, pts,  incidentFaceNormalInNew, localNormal, manifoldContacts, numContacts, contactDist);
+			break;
+		};
+	default:
+		return false;
+	}
+
+	if(flip)
+	{
+		for(PxU32 i=0; i<numContacts; ++i)
+		{
+			const Vec3V localB = manifoldContacts[i].mLocalPointB;
+			manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
+			manifoldContacts[i].mLocalPointA = localB;
+		}
+	}
+	const Ps::aos::PsMatTransformV transformNewTo1 = transform1.transformInv(newTransformV);
+	const Ps::aos::PsMatTransformV transformNewTo0 = transform0.transformInv(newTransformV);
+	//transform points to the local space of transform0 and transform1
+	const Ps::aos::Vec3V localNormalInB = transformNewTo1.rotate(Vec3V_From_Vec4V(manifoldContacts[0].mLocalNormalPen));
+
+	for(PxU32 i=0; i<numContacts; ++i)
+	{
+		manifoldContacts[i].mLocalPointA = transformNewTo0.transform(manifoldContacts[i].mLocalPointA);
+		manifoldContacts[i].mLocalPointB = transformNewTo1.transform(manifoldContacts[i].mLocalPointB);
+		manifoldContacts[i].mLocalNormalPen = V4SetW(localNormalInB, V4GetW(manifoldContacts[i].mLocalNormalPen));
+	}
+
+	return true;
+}  
+
+
+
+bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+	// Get actual shape data
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(&manifold, 256);
+	const PxBoxGeometry& shapeBox0 = shape0.get<const PxBoxGeometry>();
+	const PxBoxGeometry& shapeBox1 = shape1.get<const PxBoxGeometry>();
+
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+
+
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	const Vec3V boxExtents0 = V3LoadU(shapeBox0.halfExtents);
+	const Vec3V boxExtents1 = V3LoadU(shapeBox1.halfExtents);
+	
+	//Transfer A into the local space of B
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+	const PsTransformV curRTrans(transf1.transformInv(transf0));
+	const PsMatTransformV aToB(curRTrans);
+
+	const FloatV boxMargin0 = Gu::CalculatePCMBoxMargin(boxExtents0);
+	const FloatV boxMargin1 = Gu::CalculatePCMBoxMargin(boxExtents1);
+	const FloatV minMargin = FMin(boxMargin0, boxMargin1);
+
+	const PxU32 initialContacts = manifold.mNumContacts;
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
+	
+	manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
+
+	const PxU32 newContacts = manifold.mNumContacts;
+
+	const bool bLostContacts = (newContacts != initialContacts);
+  
+	PX_UNUSED(bLostContacts);
+
+	if(bLostContacts || manifold.invalidate_BoxConvex(curRTrans, minMargin))	
+	{
+		
+		manifold.setRelativeTransform(curRTrans);
+		
+		const PsMatTransformV transfV0(transf0);
+		const PsMatTransformV transfV1(transf1);
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		PxU32 numContacts = 0;
+	
+		if(doBoxBoxGenerateContacts(boxExtents0, boxExtents1, transfV0, transfV1, contactDist, manifoldContacts, numContacts)) 
+		{
+			if(numContacts > 0)
+			{
+			
+				manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+				const Vec3V worldNormal = transfV1.rotate(Vec3V_From_Vec4V(manifold.mContactPoints[0].mLocalNormalPen));
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transfV1);
+#if	PCM_LOW_LEVEL_DEBUG
+				manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+				return true;
+			}
+			else
+			{
+				const Vec3V zeroV = V3Zero();
+				ShrunkBoxV box0(zeroV, boxExtents0);
+				ShrunkBoxV 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,
+					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
+
+				if(status == EPA_CONTACT)
+				{
+				
+					RelativeConvex<BoxV> convexA1(box0, aToB);
+					LocalConvex<BoxV> convexB1(box1);
+
+					status=  epaPenetration(convexA1, convexB1, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, 
+						closestA, closestB, normal, penDep);
+				}
+
+				if(status == GJK_CONTACT || status == EPA_CONTACT)
+				{
+					const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+					const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+					const Vec3V localPointB = closestB;
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+					numContacts += manifold.addManifoldPoint(localPointA, localPointB, localNormalPen, replaceBreakingThreshold);
+
+					//transform the normal back to world space
+					normal = transf1.rotate(normal);
+
+					manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf1, contactDist);
+
+#if	PCM_LOW_LEVEL_DEBUG
+					manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+
+					return true;
+				}
+
+			}
+		}
+	}
+	else if(manifold.getNumContacts() > 0)
+	{
+		const Vec3V worldNormal =  manifold.getWorldNormal(transf1);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return true;
+	}
+	
+	return false;
+}
+
+}
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
new file mode 100644
index 00000000..8378e2ce
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactBoxConvex.cpp
@@ -0,0 +1,281 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+#include "GuVecBox.h"
+#include "GuVecShrunkBox.h"
+#include "GuVecShrunkConvexHull.h"
+#include "GuVecShrunkConvexHullNoScale.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+#include "GuContactBuffer.h"
+
+namespace physx
+{
+
+using namespace Ps::aos;
+
+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)
+{
+	Gu::PolygonalData polyData0;
+	PCMPolygonalBox polyBox0(halfExtents);
+	polyBox0.getPolygonalData(&polyData0);
+	polyData0.mPolygonVertexRefs = gPCMBoxPolygonData;
+	
+	Gu::PolygonalData polyData1;
+	getPCMConvexData(convexHull, idtScale, polyData1);
+	
+	Mat33V identity =  M33Identity();
+	SupportLocalImpl<BoxV> map0(box, transf0, identity, identity, true);
+
+	PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)];
+
+	SupportLocal* map1 = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) : 
+		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(), 
+		doOverlapTest, renderOutput, toleranceScale))
+	{
+		if (numContacts > 0)
+		{
+			//reduce contacts
+			manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+
+#if	PCM_LOW_LEVEL_DEBUG
+			manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+
+			const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+
+			manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+		}
+		else
+		{
+			//if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
+			if (!doOverlapTest)
+			{
+				const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+			}
+		}
+		
+		return true;
+	}
+
+	return false;
+
+}
+
+static bool addGJKEPAContacts(Gu::ShrunkConvexHullV& convexHull, Gu::ShrunkBoxV& box, 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;
+	if (status == GJK_CONTACT)
+	{
+		const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+		const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+
+		//Add contact to contact stream
+		manifoldContacts[0].mLocalPointA = localPointA;
+		manifoldContacts[0].mLocalPointB = closestB;
+		manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+		//Add contact to manifold
+		manifold.addManifoldPoint(aToB.transformInv(closestA), closestB, localNormalPen, replaceBreakingThreshold);
+
+	}
+	else
+	{
+		PX_ASSERT(status == EPA_CONTACT);
+
+		RelativeConvex<BoxV> epaConvexA(box, aToB);
+		LocalConvex<ConvexHullV> epaConvexB(convexHull);
+
+		status = epaPenetration(epaConvexA, epaConvexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+			closestA, closestB, normal, penDep);
+		if (status == EPA_CONTACT)
+		{
+
+			const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+
+			//Add contact to contact stream
+			manifoldContacts[0].mLocalPointA = localPointA;
+			manifoldContacts[0].mLocalPointB = closestB;
+			manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+			//Add contact to manifold
+			manifold.addManifoldPoint(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+		}
+		else
+		{
+			doOverlapTest = true;
+		}
+	}
+
+	return doOverlapTest;
+}
+
+bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	
+
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+	const PxBoxGeometry& shapeBox = shape0.get<const PxBoxGeometry>();
+	
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(shapeConvex.hullData);
+	
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+
+	const Vec3V zeroV = V3Zero();
+
+
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
+
+	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	
+	//Transfer A into the local space of B
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+	const PsTransformV curRTrans(transf1.transformInv(transf0));
+	const PsMatTransformV aToB(curRTrans);
+
+	const Gu::ConvexHullData* hullData = shapeConvex.hullData;
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
+	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
+
+	const FloatV minMargin = FMin(convexMargin, boxMargin);//FMin(boxMargin, convexMargin);
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
+	const PxU32 initialContacts = manifold.mNumContacts;
+
+	manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);  
+	
+	//After the refresh contact points, the numcontacts in the manifold will be changed
+
+	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;
+
+		Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
+		FloatV penDep = FZero(); 
+		
+		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);
+		
+		const RelativeConvex<ShrunkBoxV> 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,
+				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,
+				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
+
+		} 
+
+		manifold.setRelativeTransform(curRTrans); 
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+	
+		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));
+		}
+		else if(status == GJK_NON_INTERSECT)
+		{
+			return false;
+		}
+		else
+		{
+			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)
+			{
+				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);
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+				return true;
+			}
+		} 
+	}
+	else if(manifold.getNumContacts()>0)
+	{
+		const Vec3V worldNormal =  manifold.getWorldNormal(transf1);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return true;
+	}
+
+	return false;
+
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp
new file mode 100644
index 00000000..c0c4c968
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleBox.cpp
@@ -0,0 +1,230 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuVecBox.h"
+#include "GuVecCapsule.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+
+
+namespace physx
+{
+	using namespace Ps::aos;
+
+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)
+{
+
+	PolygonalData polyData;
+	PCMPolygonalBox polyBox(halfExtents);
+	polyBox.getPolygonalData(&polyData);
+
+	Mat33V identity = M33Identity();
+	SupportLocalImpl<BoxV> map(box, transf1, identity, identity);
+
+	PxU32 origContacts = numContacts;
+	if (generateCapsuleBoxFullContactManifold(capsule, polyData, &map, aToB, manifoldContacts, numContacts, contactDist, normal, closest, boxMargin, doOverlapTest, toleranceScale))
+	{
+		//EPA has contacts and we have new contacts, we discard the EPA contacts
+		if(origContacts != 0 && numContacts != origContacts)
+		{
+			numContacts--;
+			manifoldContacts++;
+		}
+
+		manifold.addBatchManifoldContacts2(manifoldContacts, numContacts);
+		
+		normal = transf1.rotate(normal);
+		
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsule.radius, contactDist);
+	
+		return true;
+		
+	}
+
+	return false;
+
+}
+
+
+bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS)
+{
+	
+	PX_UNUSED(renderOutput);
+
+	PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(&manifold, 256);
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxBoxGeometry& shapeBox = shape1.get<const PxBoxGeometry>();
+
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());  
+
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
+
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+
+	const PsTransformV curRTrans = transf1.transformInv(transf0);
+	const PsMatTransformV aToB_(curRTrans);
+
+	const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
+	const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
+
+	const PxU32 initialContacts = manifold.mNumContacts;
+
+	const FloatV boxMargin = Gu::CalculatePCMBoxMargin(boxExtents);
+	
+	const FloatV minMargin = FMin(boxMargin, capsuleRadius);
+
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
+	
+	const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
+	//refreshContactPoints remove invalid contacts from the manifold and update the number correspondingly
+	manifold.refreshContactPoints(aToB_, projectBreakingThreshold, refreshDist);
+
+	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
+
+	PX_UNUSED(bLostContacts);
+	if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))	
+	{
+
+		GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
+
+		Vec3V closestA(zeroV), closestB(zeroV);
+		Vec3V normal(zeroV); // from a to b
+		const FloatV zero = FZero();
+		FloatV penDep = zero;
+
+		manifold.setRelativeTransform(curRTrans);
+		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);
+		LocalConvex<BoxV> convexB(box);
+		const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), box.getCenter());
+		status =  gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, 
+			manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
+
+		PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		PxU32 numContacts = 0;
+		bool doOverlapTest = false;
+		if(status == GJK_NON_INTERSECT)
+		{
+			return false;
+		}
+		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));
+		}
+		else 
+		{
+			if(status == GJK_CONTACT)
+			{
+				const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+				//Add contact to contact stream
+				manifoldContacts[numContacts].mLocalPointA = localPointA;
+				manifoldContacts[numContacts].mLocalPointB = closestB;
+				manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+			}
+			else
+			{
+				PX_ASSERT(status == EPA_CONTACT);
+	
+				status= epaPenetration(convexA, convexB,  manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, 
+					closestA, closestB, normal, penDep, true);
+				if(status == EPA_CONTACT)
+				{
+					const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+					//Add contact to contact stream
+					manifoldContacts[numContacts].mLocalPointA = localPointA;
+					manifoldContacts[numContacts].mLocalPointB = closestB;
+					manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+
+				}
+				else
+				{
+					doOverlapTest = true;
+				}
+
+			}
+			
+
+			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));
+			}
+			else
+			{
+				
+				//The contacts is either come from GJK or EPA
+				const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.1f));
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+				manifold.addManifoldPoint2(curRTrans.transformInv(closestA), closestB, localNormalPen, replaceBreakingThreshold);
+				
+				normal = transf1.rotate(normal);
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
+			
+				return true;
+			}
+		}	
+	}
+	else if(manifold.getNumContacts() > 0)
+	{
+		const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf0, capsuleRadius, contactDist);
+		return true;
+	}
+
+	return false;
+
+}
+}
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleCapsule.cpp
new file mode 100644
index 00000000..4b99ae76
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleCapsule.cpp
@@ -0,0 +1,297 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.     
+
+#include "GuVecCapsule.h"
+#include "GuGeometryUnion.h"
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuDistanceSegmentSegmentSIMD.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps;
+using namespace aos;
+
+static Vec4V pcmDistancePointSegmentTValue22(	const Vec3VArg a0, const Vec3VArg b0, 
+												const Vec3VArg a1, const Vec3VArg b1,
+												const Vec3VArg p0, const Vec3VArg p1,
+												const Vec3VArg p2, const Vec3VArg p3)
+{
+	const Vec4V zero = V4Zero();
+	const Vec3V ap00 = V3Sub(p0, a0);
+	const Vec3V ap10 = V3Sub(p1, a0);
+	const Vec3V ap01 = V3Sub(p2, a1);
+	const Vec3V ap11 = V3Sub(p3, a1);
+
+	const Vec3V ab0 = V3Sub(b0, a0);
+	const Vec3V ab1 = V3Sub(b1, a1);
+
+/*	const FloatV nom00 = V3Dot(ap00, ab0);
+	const FloatV nom10 = V3Dot(ap10, ab0);
+	const FloatV nom01 = V3Dot(ap01, ab1);
+	const FloatV nom11 = V3Dot(ap11, ab1);*/
+
+	const Vec4V combinedDot = V3Dot4(ap00, ab0, ap10, ab0, ap01, ab1, ap11, ab1);
+	const FloatV nom00 = V4GetX(combinedDot);
+	const FloatV nom10 = V4GetY(combinedDot);
+	const FloatV nom01 = V4GetZ(combinedDot);
+	const FloatV nom11 = V4GetW(combinedDot);
+	
+	const FloatV denom0 = V3Dot(ab0, ab0);
+	const FloatV denom1 = V3Dot(ab1, ab1);
+
+	const Vec4V nom = V4Merge(nom00, nom10, nom01, nom11);
+	const Vec4V denom = V4Merge(denom0, denom0, denom1, denom1);
+
+	const Vec4V tValue = V4Div(nom, denom);
+	return V4Sel(V4IsEq(denom, zero), zero, tValue);
+}
+
+namespace physx
+{
+namespace Gu
+{
+
+	static void storeContact(const Vec3VArg contact, const Vec3VArg normal, const FloatVArg separation, Gu::ContactBuffer& buffer)
+	{
+		Gu::ContactPoint& point = buffer.contacts[buffer.count++];
+
+		const Vec4V normalSep = Ps::aos::V4SetW(Vec4V_From_Vec3V(normal), separation);
+
+		V4StoreA(normalSep, &point.normal.x);
+		V3StoreA(contact, point.point);
+		point.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+
+	}
+
+bool pcmContactCapsuleCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	// Get actual shape data
+	const PxCapsuleGeometry& shapeCapsule0 = shape0.get<const PxCapsuleGeometry>();
+	const PxCapsuleGeometry& shapeCapsule1 = shape1.get<const PxCapsuleGeometry>();
+
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+
+	const Vec3V _p0 = V3LoadA(&transform0.p.x);
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+
+	const Vec3V _p1 = V3LoadA(&transform1.p.x);
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+
+	/*PsTransformV transf0(p0, q0);
+	PsTransformV transf1(p1, q1);*/
+
+	
+	const FloatV r0 = FLoad(shapeCapsule0.radius);
+	const FloatV halfHeight0 = FLoad(shapeCapsule0.halfHeight);
+
+	const FloatV r1 = FLoad(shapeCapsule1.radius);
+	const FloatV halfHeight1 = FLoad(shapeCapsule1.halfHeight);
+
+	const FloatV cDist = FLoad(params.mContactDistance);
+
+	const Vec3V positionOffset = V3Scale(V3Add(_p0, _p1),  FHalf());
+	const Vec3V p0 = V3Sub(_p0, positionOffset);
+	const Vec3V p1 = V3Sub(_p1, positionOffset);
+
+	const FloatV zero = FZero();
+	//const FloatV one = FOne();
+	const Vec3V zeroV = V3Zero();
+	
+
+	/*const Vec3V positionOffset = V3Scale(V3Add(transf0.p, transf1.p), FloatV_From_F32(0.5f));
+	transf0.p = V3Sub(transf0.p, positionOffset);
+	transf1.p = V3Sub(transf1.p, positionOffset);*/
+
+	const Vec3V basisVector0 = QuatGetBasisVector0(q0);
+	const Vec3V tmp0 = V3Scale(basisVector0, halfHeight0);
+	const Vec3V s0 = V3Add(p0, tmp0);
+	const Vec3V e0 = V3Sub(p0, tmp0);
+	const Vec3V d0 = V3Sub(e0, s0);
+
+	const Vec3V basisVector1 = QuatGetBasisVector0(q1);
+	const Vec3V tmp1 = V3Scale(basisVector1, halfHeight1);
+	const Vec3V s1 = V3Add(p1, tmp1);
+	const Vec3V e1 = V3Sub(p1, tmp1); 
+
+	const Vec3V d1 = V3Sub(e1, s1);
+
+	const FloatV sumRadius = FAdd(r0, r1);
+	const FloatV inflatedSum = FAdd(sumRadius, cDist);
+	const FloatV inflatedSumSquared = FMul(inflatedSum, inflatedSum);
+	const FloatV a = V3Dot(d0, d0);//squared length of segment1
+	const FloatV e = V3Dot(d1, d1);//squared length of segment2
+	const FloatV eps = FLoad(1e-6);//FEps();
+
+	FloatV t0, t1;
+	const FloatV sqDist0 = distanceSegmentSegmentSquared(s0, d0, s1, d1, t0, t1);
+
+	if(FAllGrtrOrEq(inflatedSumSquared, sqDist0))
+	{
+		const Vec4V zeroV4 = V4Zero();
+		const Vec4V oneV4 = V4One();
+		//check to see whether these two capsule are paralle
+		const FloatV parallelTolerance  = FLoad(0.9998f);
+		
+		
+		const BoolV con0 = FIsGrtr(eps, a);
+		const BoolV con1 = FIsGrtr(eps, e);
+		const Vec3V dir0 = V3Sel(con0, zeroV, V3ScaleInv(d0, FSqrt(a)));
+		const Vec3V dir1 = V3Sel(con1, zeroV, V3ScaleInv(d1, FSqrt(e)));
+
+		const FloatV cos = FAbs(V3Dot(dir0, dir1));
+		if(FAllGrtr(cos, parallelTolerance))//paralle
+		{
+			//project s, e into s1e1
+			const Vec4V t= pcmDistancePointSegmentTValue22(s0, e0, s1, e1,
+															s1, e1, s0, e0);
+
+			const BoolV con = BAnd(V4IsGrtrOrEq(t, zeroV4), V4IsGrtrOrEq(oneV4, t));
+			const BoolV con00 = BGetX(con);
+			const BoolV con01 = BGetY(con);
+			const BoolV con10 = BGetZ(con);
+			const BoolV con11 = BGetW(con);
+
+		/*	PX_ALIGN(16, PxU32 conditions[4]);
+			F32Array_Aligned_From_Vec4V(con, (PxF32*)conditions);*/
+
+			
+			PxU32 numContact=0;
+
+			if(BAllEqTTTT(con00))
+			{
+				const Vec3V projS1 = V3ScaleAdd(d0, V4GetX(t), s0);
+				const Vec3V v = V3Sub(projS1, s1);
+				const FloatV sqDist = V3Dot(v, v);
+				const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
+				
+				if(BAllEqTTTT(bCon))
+				{
+					const FloatV dist = FSqrt(sqDist);
+					const FloatV pen =  FSub(dist, sumRadius);
+					const Vec3V normal = V3ScaleInv(v, dist);
+					PX_ASSERT(isFiniteVec3V(normal));
+					const Vec3V _p = V3NegScaleSub(normal, r0, projS1);
+					const Vec3V p = V3Add(_p, positionOffset);
+					
+					storeContact(p, normal, pen, contactBuffer);
+					numContact++;
+				}
+			}
+			if(BAllEqTTTT(con01))
+			{
+				const Vec3V projE1 = V3ScaleAdd(d0, V4GetY(t), s0);
+				const Vec3V v = V3Sub(projE1, e1);
+				const FloatV sqDist = V3Dot(v, v);
+				const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
+				
+				if(BAllEqTTTT(bCon))
+				{
+					const FloatV dist = FSqrt(sqDist);
+					const FloatV pen =  FSub(dist, sumRadius);
+					const Vec3V normal =  V3ScaleInv(v, dist);
+					PX_ASSERT(isFiniteVec3V(normal));
+					const Vec3V _p = V3NegScaleSub(normal, r0, projE1);
+					const Vec3V p = V3Add(_p, positionOffset);
+					storeContact(p, normal, pen, contactBuffer);
+					numContact++;
+				}
+			}
+
+			if(BAllEqTTTT(con10))
+			{
+				const Vec3V projS0 = V3ScaleAdd(d1, V4GetZ(t), s1);
+				const Vec3V v = V3Sub(s0, projS0);
+				const FloatV sqDist = V3Dot(v, v);
+				const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
+				
+				if(BAllEqTTTT(bCon))
+				{
+					const FloatV dist = FSqrt(sqDist);
+					const FloatV pen =  FSub(dist, sumRadius);
+					const Vec3V normal = V3ScaleInv(v, dist);
+				 	PX_ASSERT(isFiniteVec3V(normal));
+					const Vec3V _p = V3NegScaleSub(normal, r0, s0);
+					const Vec3V p = V3Add(_p, positionOffset);
+					//const Vec3V p = V3ScaleAdd(normal, r0, s0);
+					storeContact(p, normal, pen, contactBuffer);
+					numContact++;
+				}
+			}
+
+			if(BAllEqTTTT(con11))
+			{
+				const Vec3V projE0 = V3ScaleAdd(d1, V4GetW(t), s1);
+				const Vec3V v = V3Sub(e0, projE0);
+				const FloatV sqDist = V3Dot(v, v);
+				const BoolV bCon = BAnd(FIsGrtr(sqDist, eps), FIsGrtr(inflatedSumSquared, sqDist));
+				
+				if(BAllEqTTTT(bCon))
+				{
+					const FloatV dist = FSqrt(sqDist);
+					const FloatV pen =  FSub(dist, sumRadius);
+					const Vec3V normal = V3ScaleInv(v, dist);
+					PX_ASSERT(isFiniteVec3V(normal));
+					const Vec3V _p = V3NegScaleSub(normal, r0, e0);
+					const Vec3V p = V3Add(_p, positionOffset);
+					//const Vec3V p = V3ScaleAdd(normal, r0, e0);
+					storeContact(p, normal, pen, contactBuffer);
+					numContact++;
+				}
+			}
+
+			if(numContact)
+				return true;
+
+		}  
+
+		const Vec3V closestA = V3ScaleAdd(d0, t0, s0);
+		const Vec3V closestB = V3ScaleAdd(d1, t1, s1);
+		
+		const BoolV con = FIsGrtr(eps, sqDist0);
+		//const Vec3V normal = V3Sel(FIsEq(dist, zero), V3Sel(FIsGrtr(a, eps), V3Normalise(d0), V3Scale(V3Sub(closestA, closestB), FRecip(dist)));
+		const Vec3V _normal = V3Sel(con, V3Sel(FIsGrtr(a, eps), d0, V3UnitX()), V3Sub(closestA, closestB));
+		const Vec3V normal = V3Normalize(_normal);
+		PX_ASSERT(isFiniteVec3V(normal));
+		const Vec3V _point = V3NegScaleSub(normal, r0, closestA);
+		const Vec3V p = V3Add(_point, positionOffset);
+		const FloatV dist = FSel(con, zero, FSqrt(sqDist0));
+		const FloatV pen = FSub(dist, sumRadius);
+		//PX_ASSERT(FAllGrtrOrEq(zero, pen));
+		storeContact(p, normal, pen, contactBuffer);
+		return true;
+	}
+	return false;
+}
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
new file mode 100644
index 00000000..e80e39bd
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleConvex.cpp
@@ -0,0 +1,272 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+#include "GuVecCapsule.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+
+namespace physx
+{
+
+using namespace Ps::aos;
+
+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)
+{
+
+	PX_UNUSED(renderOutput);
+	Gu::PolygonalData polyData;
+	getPCMConvexData(convexHull,idtScale, polyData);
+
+	PxU8 buff[sizeof(SupportLocalImpl<ConvexHullV>)];
+	SupportLocal* map = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) : 
+	static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
+
+	PxU32 numContacts = 0;
+	if (generateFullContactManifold(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, normal, closest, tolerance, doOverlapTest, toleranceScale))
+	{
+
+		if (numContacts > 0)
+		{
+			manifold.addBatchManifoldContacts2(manifoldContacts, numContacts);
+			//transform normal into the world space
+			normal = transf1.rotate(normal);
+			manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsule.radius, contactDist);
+		}
+		else
+		{
+			if (!doOverlapTest)
+			{
+				normal = transf1.rotate(normal);
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsule.radius, contactDist);
+			}
+		}
+
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return true;
+		
+	}
+	return false;
+
+}
+
+bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+
+	PersistentContactManifold& manifold = cache.getManifold();
+
+	Ps::prefetchLine(shapeConvex.hullData);
+
+		
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+
+	const Vec3V zeroV = V3Zero();
+
+	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
+	const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
+	const ConvexHullData* hullData =shapeConvex.hullData;
+	
+	//Transfer A into the local space of B
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+	const PsTransformV curRTrans(transf1.transformInv(transf0));
+	const PsMatTransformV aToB(curRTrans);
+	
+
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
+	const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius);
+	const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
+	
+	const PxU32 initialContacts = manifold.mNumContacts;
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f));
+	const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
+
+	manifold.refreshContactPoints(aToB,  projectBreakingThreshold, refreshDist);
+
+	//ML: after refreshContactPoints, we might lose some contacts
+	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
+
+	GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
+
+	Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
+	const FloatV zero = FZero();
+	FloatV penDep = zero;
+
+	PX_UNUSED(bLostContacts);
+	if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
+	{
+		const bool idtScale = shapeConvex.scale.isIdentity();
+
+		manifold.setRelativeTransform(curRTrans);
+		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);  
+		ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
+		convexHull.setMargin(zero);
+	
+		//transform capsule(a) into the local space of convexHull(b)
+		CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
+	
+		LocalConvex<CapsuleV> convexA(capsule);
+		const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+		if(idtScale)
+		{
+			LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
+
+			status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, 
+				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
+		}
+		else
+		{
+			LocalConvex<ConvexHullV> convexB(convexHull);
+			status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, 
+				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
+
+		}     
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		bool doOverlapTest = false;
+		if(status == GJK_NON_INTERSECT)
+		{
+			return false;
+		}
+		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));
+		}
+		else 
+		{
+			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+
+			if(status == GJK_CONTACT)
+			{
+				const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+				//Add contact to contact stream
+				manifoldContacts[0].mLocalPointA = localPointA;
+				manifoldContacts[0].mLocalPointB = closestB;
+				manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+				//Add contact to manifold
+				manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+			}
+			else
+			{
+				PX_ASSERT(status == EPA_CONTACT);
+				
+				if(idtScale)
+				{
+					LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
+
+					status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+					closestA, closestB, normal, penDep, true);
+				}
+				else
+				{
+					LocalConvex<ConvexHullV> convexB(convexHull);
+					status= Gu::epaPenetration(convexA, convexB,  manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+					closestA, closestB, normal, penDep, true);
+				}
+				
+				
+				if(status == EPA_CONTACT)
+				{
+					const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+					//Add contact to contact stream
+					manifoldContacts[0].mLocalPointA = localPointA;
+					manifoldContacts[0].mLocalPointB = closestB;
+					manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+					//Add contact to manifold
+					manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+					
+
+				}
+				else
+				{
+					doOverlapTest = true;   
+				}
+			}
+
+		
+			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));
+			}
+			else
+			{
+				//This contact is either come from GJK or EPA
+				normal = transf1.rotate(normal);
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+				manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+				return true;
+			}
+		}	
+	}
+	else if (manifold.getNumContacts() > 0)
+	{
+		normal = manifold.getWorldNormal(transf1);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return true;
+	}
+	return false;
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleHeightField.cpp
new file mode 100644
index 00000000..8f187307
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleHeightField.cpp
@@ -0,0 +1,158 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "PsVecMath.h"
+#include "PsVecTransform.h"
+#include "GuVecTriangle.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuHeightField.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuSegment.h"
+#include "GuInternal.h"
+#include "GuPCMContactMeshCallback.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+namespace physx
+{
+
+struct PCMCapsuleVsHeightfieldContactGenerationCallback :  PCMHeightfieldContactGenerationCallback<PCMCapsuleVsHeightfieldContactGenerationCallback>
+{
+	PCMCapsuleVsHeightfieldContactGenerationCallback& operator=(const PCMCapsuleVsHeightfieldContactGenerationCallback&);
+
+public:
+	PCMCapsuleVsMeshContactGeneration		mGeneration;
+
+	PCMCapsuleVsHeightfieldContactGenerationCallback(
+		const Gu::CapsuleV&			capsule,
+		const Ps::aos::FloatVArg	contactDistance,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+	
+		const PsTransformV& capsuleTransform, 
+		const PsTransformV& heightfieldTransform,
+		const PxTransform&	heightfieldTransform1,
+		Gu::MultiplePersistentContactManifold& multiManifold,
+		Gu::ContactBuffer& contactBuffer,
+		Gu::HeightFieldUtil& hfUtil 
+		
+		
+	) :
+		PCMHeightfieldContactGenerationCallback<PCMCapsuleVsHeightfieldContactGenerationCallback>(hfUtil, heightfieldTransform1),
+		mGeneration(capsule, contactDistance, replaceBreakingThreshold, capsuleTransform, heightfieldTransform, multiManifold, contactBuffer)
+	{
+	}
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(Gu::TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMCapsuleVsMeshContactGeneration>(cache);
+	}
+	
+};
+
+bool Gu::pcmContactCapsuleHeightField(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxHeightFieldGeometryLL& shapeHeight = shape1.get<const PxHeightFieldGeometryLL>();
+
+	Gu::MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const PsTransformV capsuleTransform = loadTransformA(transform0);//capsule transform
+	const PsTransformV heightfieldTransform = loadTransformA(transform1);//height feild
+
+	const PsTransformV curTransform = heightfieldTransform.transformInv(capsuleTransform);
+	
+	const FloatV replaceBreakingThreshold = FMul(capsuleRadius, FLoad(0.001f));
+
+	if(multiManifold.invalidate(curTransform, capsuleRadius, FLoad(0.02f)))
+	{
+
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform); 
+
+		const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(shapeHeight.heightField);
+
+		Gu::HeightFieldUtil hfUtil(shapeHeight, hf);
+
+		const PxVec3 tmp = getCapsuleHalfHeightVector(transform0, shapeCapsule);
+
+		const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
+
+		const PxVec3 capsuleCenterInMesh = transform1.transformInv(transform0.p);
+		const PxVec3 capsuleDirInMesh = transform1.rotateInv(tmp);
+		const Gu::CapsuleV capsule(V3LoadU(capsuleCenterInMesh), V3LoadU(capsuleDirInMesh), capsuleRadius);
+
+
+		PCMCapsuleVsHeightfieldContactGenerationCallback callback(
+			capsule,
+			contactDist,
+			replaceBreakingThreshold,
+			capsuleTransform,
+			heightfieldTransform,
+			transform1,
+			multiManifold,
+			contactBuffer,
+			hfUtil
+		);
+
+		PxBounds3 bounds;
+		bounds.maximum = PxVec3(shapeCapsule.halfHeight + inflatedRadius, inflatedRadius, inflatedRadius);
+		bounds.minimum = -bounds.maximum;
+
+		bounds = PxBounds3::transformFast(transform1.transformInv(transform0), bounds);
+
+		hfUtil.overlapAABBTriangles(transform1, bounds, 0, &callback);
+
+		callback.mGeneration.processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE, false);
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+		// We must be in local space to use the cache
+		const FloatV projectBreakingThreshold = FMul(capsuleRadius, FLoad(0.05f));
+		const FloatV refereshDistance = FAdd(capsuleRadius, contactDist);
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
+
+	}
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, capsuleTransform, heightfieldTransform, capsuleRadius);
+}
+
+
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleMesh.cpp
new file mode 100644
index 00000000..d2a085fb
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactCapsuleMesh.cpp
@@ -0,0 +1,182 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecTriangle.h"
+#include "GuVecCapsule.h"
+#include "GuGeometryUnion.h"
+#include "PsVecMath.h"
+#include "PsVecTransform.h"
+
+#include "GuContactMethodImpl.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuSegment.h"
+#include "GuVecCapsule.h"
+#include "GuInternal.h"
+#include "GuPCMContactMeshCallback.h"
+#include "GuConvexEdgeFlags.h"
+#include "GuBox.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace physx::shdfnd::aos;
+
+namespace physx
+{
+
+struct PCMCapsuleVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback< PCMCapsuleVsMeshContactGenerationCallback >
+{
+	PCMCapsuleVsMeshContactGenerationCallback& operator=(const PCMCapsuleVsMeshContactGenerationCallback&);
+public:
+	PCMCapsuleVsMeshContactGeneration		mGeneration;
+
+	PCMCapsuleVsMeshContactGenerationCallback(
+		const CapsuleV&				capsule,
+		const Ps::aos::FloatVArg	contactDist,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+		const PsTransformV&			sphereTransform,
+		const PsTransformV&			meshTransform,
+		MultiplePersistentContactManifold& multiManifold,
+		ContactBuffer&				contactBuffer,
+		const PxU8*					extraTriData,
+		const Cm::FastVertex2ShapeScaling& meshScaling,
+		bool						idtMeshScale,
+		Cm::RenderOutput*			renderOutput = NULL
+	) :
+		PCMMeshContactGenerationCallback<PCMCapsuleVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale),
+		mGeneration(capsule, contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer, renderOutput)
+	{
+	}
+
+	PX_FORCE_INLINE bool doTest(const PxVec3&, const PxVec3&, const PxVec3&) { return true; }
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMCapsuleVsMeshContactGeneration>(cache);
+	}
+	
+};
+
+bool Gu::pcmContactCapsuleMesh(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+	const PxCapsuleGeometry& shapeCapsule= shape0.get<const PxCapsuleGeometry>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	//gRenderOutPut = cache.mRenderOutput;
+	const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const PsTransformV capsuleTransform = loadTransformA(transform0);//capsule transform
+	const PsTransformV meshTransform = loadTransformA(transform1);//triangleMesh  
+
+	const PsTransformV curTransform = meshTransform.transformInv(capsuleTransform);
+	
+	// We must be in local space to use the cache
+	if(multiManifold.invalidate(curTransform, capsuleRadius, FLoad(0.02f)))
+	{
+		const FloatV replaceBreakingThreshold = FMul(capsuleRadius, FLoad(0.001f));
+		//const FloatV capsuleHalfHeight = FloatV_From_F32(shapeCapsule.halfHeight);
+		Cm::FastVertex2ShapeScaling meshScaling;
+		const bool idtMeshScale = shapeMesh.scale.isIdentity();
+		if(!idtMeshScale)
+			meshScaling.init(shapeMesh.scale);
+
+		// Capsule data
+		const PxVec3 tmp = getCapsuleHalfHeightVector(transform0, shapeCapsule);
+		Segment worldCapsule;
+		worldCapsule.p0 = transform0.p + tmp;
+		worldCapsule.p1 = transform0.p - tmp;
+
+		
+		const Segment meshCapsule(	// Capsule in mesh space
+			transform1.transformInv(worldCapsule.p0),
+			transform1.transformInv(worldCapsule.p1));
+
+		const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
+
+		const PxVec3 capsuleCenterInMesh = transform1.transformInv(transform0.p);
+		const PxVec3 capsuleDirInMesh = transform1.rotateInv(tmp);
+		const CapsuleV capsule(V3LoadU(capsuleCenterInMesh), V3LoadU(capsuleDirInMesh), capsuleRadius);
+
+		// We must be in local space to use the cache
+		const Capsule queryCapsule(meshCapsule, inflatedRadius);
+
+		const TriangleMesh* meshData = shapeMesh.meshData;
+
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform); 
+
+		const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
+		// mesh scale is not baked into cached verts
+		PCMCapsuleVsMeshContactGenerationCallback callback(
+			capsule,
+			contactDist,
+			replaceBreakingThreshold,
+			capsuleTransform,
+			meshTransform,
+			multiManifold,
+			contactBuffer,
+			extraData,
+			meshScaling,
+			idtMeshScale,
+			renderOutput);
+
+		//bound the capsule in shape space by an OBB:
+		Box queryBox;
+		queryBox.create(queryCapsule);
+
+		//apply the skew transform to the box:
+		if(!idtMeshScale)
+			meshScaling.transformQueryBounds(queryBox.center, queryBox.extents, queryBox.rot);
+
+		Midphase::intersectOBB(meshData, queryBox, callback, true);
+
+		callback.flushCache();
+
+		callback.mGeneration.processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE, false);
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+		const FloatV projectBreakingThreshold = FMul(capsuleRadius, FLoad(0.05f));
+		const FloatV refereshDistance = FAdd(capsuleRadius, contactDist);
+		//multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
+	}
+
+	//multiManifold.drawManifold(*gRenderOutPut, capsuleTransform, meshTransform);
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, capsuleTransform, meshTransform, capsuleRadius);
+}
+
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.cpp
new file mode 100644
index 00000000..1934caa5
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.cpp
@@ -0,0 +1,1266 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuVecTriangle.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuConvexEdgeFlags.h"
+#include "GuBarycentricCoordinates.h"
+
+namespace physx
+{
+
+namespace Gu
+{
+/*
+	This function adds the newly created manifold contacts to a new patch or existing patches 
+*/
+void PCMConvexVsMeshContactGeneration::addContactsToPatch(const Ps::aos::Vec3VArg patchNormal, const PxU32 previousNumContacts)
+{
+	using namespace Ps::aos;
+	
+	const Vec3V patchNormalInTriangle = mMeshToConvex.rotateInv(patchNormal);
+	
+
+	const PxU32 newContacts = mNumContacts - previousNumContacts;
+
+	if(newContacts > GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE)
+	{
+		//if the current created manifold contacts are more than GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points, we will reduce the total numContacts 
+		//to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE. However, after we add these points into a patch, the patch contacts will be variable. Then we will 
+		//do contact reduction for that patch in the processContacts. After the contact reduction, there will be no more than GU_SINGLE_MANIFOLD_CACHE_SIZE(6) 
+		//contacts inside a signlePersistentContactManifold
+		Gu::SinglePersistentContactManifold::reduceContacts(&mManifoldContacts[previousNumContacts], newContacts);
+		mNumContacts = previousNumContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;  
+	}   
+
+	//get rid of duplicate manifold contacts for the newly created contacts
+	for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
+	{
+		for(PxU32 j=i+1; j<mNumContacts; ++j)
+		{
+			Vec3V dif = V3Sub(mManifoldContacts[j].mLocalPointB, mManifoldContacts[i].mLocalPointB);
+			FloatV d = V3Dot(dif, dif);
+			if(FAllGrtr(mSqReplaceBreakingThreshold, d))
+			{
+				mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
+				mNumContacts--;
+				j--;
+			}
+		}
+	}
+
+	//calculate the maxPen and transform the patch normal and localPointB into mesh's local space
+	FloatV maxPen = FMax();
+	for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
+	{
+		const FloatV pen = V4GetW(mManifoldContacts[i].mLocalNormalPen);
+		mManifoldContacts[i].mLocalNormalPen = V4SetW(patchNormalInTriangle, pen);
+		mManifoldContacts[i].mLocalPointB = mMeshToConvex.transformInv(mManifoldContacts[i].mLocalPointB);
+		maxPen = FMin(maxPen, pen);
+	}
+
+	//Based on the patch normal and add the newly avaiable manifold points to the corresponding patch
+	addManifoldPointToPatch(patchNormalInTriangle, maxPen, previousNumContacts);
+	
+	PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
+	if(mNumContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
+	{
+		PX_ASSERT(mNumContacts <= ContactBuffer::MAX_CONTACTS);
+		processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE);
+	}
+}
+
+
+void PCMConvexVsMeshContactGeneration::generateLastContacts()
+{
+	using namespace Ps::aos;
+	// Process delayed contacts
+	PxU32 nbEntries = mDeferredContacts.size();
+
+	if(nbEntries)
+	{
+		nbEntries /= sizeof(PCMDeferredPolyData)/sizeof(PxU32);
+
+		const PCMDeferredPolyData* PX_RESTRICT cd = reinterpret_cast<const PCMDeferredPolyData*>(mDeferredContacts.begin());
+		for(PxU32 i=0;i<nbEntries;i++)
+		{
+			const PCMDeferredPolyData& currentContact = cd[i];  
+
+			const PxU32 ref0 = currentContact.mInds[0];
+			const PxU32 ref1 = currentContact.mInds[1];  
+			const PxU32 ref2 = currentContact.mInds[2];
+
+			PxU8 triFlags = currentContact.triFlags;
+
+			
+			bool needsProcessing =  (((triFlags & ETD_CONVEX_EDGE_01) != 0 || mEdgeCache.get(CachedEdge(ref0, ref1)) == NULL)) && 
+									(((triFlags & ETD_CONVEX_EDGE_12) != 0 || mEdgeCache.get(CachedEdge(ref1, ref2)) == NULL)) && 
+									(((triFlags & ETD_CONVEX_EDGE_20) != 0 || mEdgeCache.get(CachedEdge(ref2, ref0)) == NULL));
+			
+
+			if(needsProcessing)
+			{
+
+				Gu::TriangleV localTriangle(currentContact.mVerts);
+				Vec3V patchNormal;
+				const PxU32 previousNumContacts = mNumContacts;
+				//the localTriangle is in the convex space
+				//Generate contacts - we didn't generate contacts with any neighbours
+				generatePolyDataContactManifold(localTriangle, currentContact.mFeatureIndex, currentContact.mTriangleIndex, triFlags, mManifoldContacts, mNumContacts, mContactDist, patchNormal);
+
+				FloatV v, w;
+				const FloatV upperBound = FLoad(0.97f);
+				const FloatV lowerBound = FSub(FOne(), upperBound);
+				PxU32 currentContacts = mNumContacts;
+				for(PxU32 j=currentContacts; j>previousNumContacts; --j)
+				{
+					PxU32 ind = j-1;
+					//calculate the barycentric coordinate of the contacts in localTriangle, p = a + v(b-a) + w(c-a)., p=ua+vb+wc
+					barycentricCoordinates(mManifoldContacts[ind].mLocalPointB, localTriangle.verts[0], localTriangle.verts[1], localTriangle.verts[2], v, w);
+					//const FloatV u = FSub(one, FAdd(v, w)); 
+
+					bool keepContact = true;
+					if(FAllGrtr(v, upperBound))//v > upperBound
+					{
+						//vertex1
+						keepContact = !mVertexCache.contains(Gu::CachedVertex(ref1));
+					}
+					else if(FAllGrtr(w, upperBound))// w > upperBound
+					{
+						//vertex2
+						keepContact = !mVertexCache.contains(Gu::CachedVertex(ref2));
+					}
+					else if(FAllGrtrOrEq(lowerBound, FAdd(v, w))) // u(1-(v+w)) > upperBound
+					{
+						//vertex0
+						keepContact = !mVertexCache.contains(Gu::CachedVertex(ref0));
+					}
+					
+					if(!keepContact)
+					{
+						//ML: if feature code is any of the vertex in this triangle and we have generated contacts with any other triangles which contains this vertex, we should drop it
+						currentContacts--;
+
+						for(PxU32 k = ind; k < currentContacts; ++k)
+						{
+							mManifoldContacts[k] = mManifoldContacts[k+1];
+						}
+					}	
+				}
+
+				mNumContacts = currentContacts;
+
+				if(currentContacts > previousNumContacts)
+				{
+					addContactsToPatch(patchNormal, previousNumContacts);
+				}
+				
+			}
+		}
+	}
+
+}
+
+bool PCMConvexVsMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
+{
+	using namespace Ps::aos;
+
+
+	const Mat33V identity =  M33Identity();
+	const FloatV zero = FZero();
+
+	const Vec3V v0 = V3LoadU(verts[0]);
+	const Vec3V v1 = V3LoadU(verts[1]);
+	const Vec3V v2 = V3LoadU(verts[2]);
+
+	
+
+	const Vec3V v10 = V3Sub(v1, v0);
+	const Vec3V v20 = V3Sub(v2, v0);
+
+	const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
+	const FloatV d = V3Dot(v0, n);//d = -p0.dot(n);
+
+	const FloatV dist = FSub(V3Dot(mHullCenterMesh, n), d);//p.dot(n) + d;
+	
+	// Backface culling
+	if(FAllGrtr(zero, dist))
+		return false;
+
+
+	//tranform verts into the box local space
+	const Vec3V locV0 = mMeshToConvex.transform(v0);
+	const Vec3V locV1 = mMeshToConvex.transform(v1);
+	const Vec3V locV2 = mMeshToConvex.transform(v2);
+
+	Gu::TriangleV localTriangle(locV0, locV1, locV2);
+
+	{
+
+		SupportLocalImpl<Gu::TriangleV> localTriMap(localTriangle, mConvexTransform, identity, identity, true);
+
+		const PxU32 previousNumContacts = mNumContacts;
+		Vec3V patchNormal;
+
+		generateTriangleFullContactManifold(localTriangle, triangleIndex, vertInds, triFlags, mPolyData, &localTriMap, mPolyMap, mManifoldContacts, mNumContacts, mContactDist, patchNormal);
+		
+		if(mNumContacts > previousNumContacts)
+		{
+#if PCM_LOW_LEVEL_DEBUG
+			Gu::PersistentContactManifold::drawTriangle(*mRenderOutput, mMeshTransform.transform(v0), mMeshTransform.transform(v1), mMeshTransform.transform(v2), 0x00ff00);
+#endif
+
+			bool inActiveEdge0 = (triFlags & ETD_CONVEX_EDGE_01) == 0;
+			bool inActiveEdge1 = (triFlags & ETD_CONVEX_EDGE_12) == 0;
+			bool inActiveEdge2 = (triFlags & ETD_CONVEX_EDGE_20) == 0;
+
+			if(inActiveEdge0)
+				mEdgeCache.addData(CachedEdge(vertInds[0], vertInds[1]));
+			if(inActiveEdge1)
+				mEdgeCache.addData(CachedEdge(vertInds[1], vertInds[2]));
+			if(inActiveEdge2)
+				mEdgeCache.addData(CachedEdge(vertInds[2], vertInds[0]));
+
+			mVertexCache.addData(CachedVertex(vertInds[0]));
+			mVertexCache.addData(CachedVertex(vertInds[1]));
+			mVertexCache.addData(CachedVertex(vertInds[2]));
+
+			addContactsToPatch(patchNormal, previousNumContacts);
+		}
+	}
+
+	
+
+	return true;
+}
+
+
+bool PCMConvexVsMeshContactGeneration::processTriangle(const Gu::PolygonalData& polyData, SupportLocal* polyMap, const PxVec3* verts, const PxU32 triangleIndex, PxU8 triFlags,const Ps::aos::FloatVArg inflation, const bool isDoubleSided, 
+													   const Ps::aos::PsTransformV& convexTransform, const Ps::aos::PsMatTransformV& meshToConvex, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	using namespace Ps::aos;
+
+
+	const Mat33V identity =  M33Identity();
+	const FloatV zero = FZero();
+
+	const Vec3V v0 = V3LoadU(verts[0]);
+	const Vec3V v1 = V3LoadU(verts[1]);
+	const Vec3V v2 = V3LoadU(verts[2]);
+
+	//tranform verts into the box local space
+	const Vec3V locV0 = meshToConvex.transform(v0);
+	const Vec3V locV1 = meshToConvex.transform(v1);
+	const Vec3V locV2 = meshToConvex.transform(v2);
+
+	const Vec3V v10 = V3Sub(locV1, locV0);
+	const Vec3V v20 = V3Sub(locV2, locV0);
+
+	const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
+	const FloatV d = V3Dot(locV0, n);//d = -p0.dot(n);
+
+	const FloatV dist = FSub(V3Dot(polyMap->shapeSpaceCenterOfMass, n), d);//p.dot(n) + d;
+	
+	// Backface culling
+	const bool culled = !isDoubleSided && (FAllGrtr(zero, dist));
+	if(culled)
+		return false;
+
+
+	Gu::TriangleV localTriangle(locV0, locV1, locV2);
+
+	SupportLocalImpl<Gu::TriangleV> localTriMap(localTriangle, convexTransform, identity, identity, true);
+
+	Vec3V patchNormal;
+
+	generateTriangleFullContactManifold(localTriangle, triangleIndex, triFlags, polyData, &localTriMap, polyMap, manifoldContacts, numContacts, inflation, patchNormal);
+	
+	return true;
+}
+
+PX_FORCE_INLINE Ps::aos::Vec4V pcmDistanceSegmentSegmentSquared4(		const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg d0, 
+														const Ps::aos::Vec3VArg p02, const Ps::aos::Vec3VArg d02, 
+                                                        const Ps::aos::Vec3VArg p12, const Ps::aos::Vec3VArg d12, 
+														const Ps::aos::Vec3VArg p22, const Ps::aos::Vec3VArg d22,
+                                                        const Ps::aos::Vec3VArg p32, const Ps::aos::Vec3VArg d32,
+                                                        Ps::aos::Vec4V& s, Ps::aos::Vec4V& t)
+{
+	using namespace Ps::aos;
+	const Vec4V zero = V4Zero();
+	const Vec4V one = V4One();
+	const Vec4V eps = V4Eps();
+	const Vec4V half = V4Splat(FHalf());
+ 
+	const Vec4V d0X = V4Splat(V3GetX(d0));
+	const Vec4V d0Y = V4Splat(V3GetY(d0));
+	const Vec4V d0Z = V4Splat(V3GetZ(d0));
+	const Vec4V pX  = V4Splat(V3GetX(p));
+	const Vec4V pY  = V4Splat(V3GetY(p));
+	const Vec4V pZ  = V4Splat(V3GetZ(p));
+
+	Vec4V d024 = Vec4V_From_Vec3V(d02);
+	Vec4V d124 = Vec4V_From_Vec3V(d12);
+	Vec4V d224 = Vec4V_From_Vec3V(d22);
+	Vec4V d324 = Vec4V_From_Vec3V(d32);
+
+	Vec4V p024 = Vec4V_From_Vec3V(p02);
+	Vec4V p124 = Vec4V_From_Vec3V(p12);
+	Vec4V p224 = Vec4V_From_Vec3V(p22);
+	Vec4V p324 = Vec4V_From_Vec3V(p32);
+
+	Vec4V d0123X, d0123Y, d0123Z;
+	Vec4V p0123X, p0123Y, p0123Z;
+
+	PX_TRANSPOSE_44_34(d024, d124, d224, d324, d0123X, d0123Y, d0123Z);
+	PX_TRANSPOSE_44_34(p024, p124, p224, p324, p0123X, p0123Y, p0123Z);
+
+	const Vec4V rX = V4Sub(pX, p0123X);
+	const Vec4V rY = V4Sub(pY, p0123Y);
+	const Vec4V rZ = V4Sub(pZ, p0123Z);
+
+	//TODO - store this in a transposed state and avoid so many dot products?
+
+	const FloatV dd = V3Dot(d0, d0);
+
+	const Vec4V e = V4MulAdd(d0123Z, d0123Z, V4MulAdd(d0123X, d0123X, V4Mul(d0123Y, d0123Y)));
+	const Vec4V b = V4MulAdd(d0Z, d0123Z, V4MulAdd(d0X, d0123X, V4Mul(d0Y, d0123Y)));
+	const Vec4V c = V4MulAdd(d0Z, rZ, V4MulAdd(d0X, rX, V4Mul(d0Y, rY)));
+	const Vec4V f = V4MulAdd(d0123Z, rZ, V4MulAdd(d0123X, rX, V4Mul(d0123Y, rY))); 
+
+	const Vec4V a(V4Splat(dd));
+
+	const Vec4V aRecip(V4Recip(a));
+	const Vec4V eRecip(V4Recip(e));
+
+	//if segments not parallell, compute closest point on two segments and clamp to segment1
+	const Vec4V denom = V4Sub(V4Mul(a, e), V4Mul(b, b));
+	const Vec4V temp = V4Sub(V4Mul(b, f), V4Mul(c, e));
+	//const Vec4V s0 = V4Clamp(V4Div(temp, denom), zero, one);
+	//In PS3, 0(temp)/0(denom) will produce QNaN and V4Clamp can't clamp the value to zero and one. In PC, 0/0 will produce inf and V4Clamp clamp the value to be one.
+	//Therefore, we need to add the select code to protect against this case
+	const Vec4V value = V4Sel(V4IsEq(denom, zero), one, V4Div(temp, denom));
+	const Vec4V s0 = V4Clamp(value, zero, one);
+  
+	//test whether segments are parallel
+	const BoolV con2 = V4IsGrtrOrEq(eps, denom);     
+	const Vec4V sTmp = V4Sel(con2, half, s0);
+      
+	//compute point on segment2 closest to segment1
+	//const Vec4V tTmp = V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip);
+	const Vec4V tTmp = V4Sel(V4IsEq(e, zero), one, V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip));
+
+	//if t is in [zero, one], done. otherwise clamp t
+	const Vec4V t2 = V4Clamp(tTmp, zero, one);
+
+	//recompute s for the new value
+	//const Vec4V comp = V4Mul(V4Sub(V4Mul(b,t2), c), aRecip);
+	const Vec4V comp = V4Sel(V4IsEq(a, zero), one, V4Mul(V4Sub(V4Mul(b,t2), c), aRecip));
+	const Vec4V s2 = V4Clamp(comp, zero, one);
+
+	s = s2;
+	t = t2;
+
+	const Vec4V closest1X = V4MulAdd(d0X, s2, pX);
+	const Vec4V closest1Y = V4MulAdd(d0Y, s2, pY);
+	const Vec4V closest1Z = V4MulAdd(d0Z, s2, pZ);
+
+	const Vec4V closest2X = V4MulAdd(d0123X, t2, p0123X);
+	const Vec4V closest2Y = V4MulAdd(d0123Y, t2, p0123Y);
+	const Vec4V closest2Z = V4MulAdd(d0123Z, t2, p0123Z);
+
+	const Vec4V vvX = V4Sub(closest1X, closest2X);
+	const Vec4V vvY = V4Sub(closest1Y, closest2Y);
+	const Vec4V vvZ = V4Sub(closest1Z, closest2Z);
+
+	const Vec4V vd = V4MulAdd(vvX, vvX, V4MulAdd(vvY, vvY, V4Mul(vvZ, vvZ)));
+
+	return vd;
+}
+
+static Ps::aos::FloatV pcmDistancePointTriangleSquared(	const Ps::aos::Vec3VArg p, 
+														const Ps::aos::Vec3VArg a, 
+														const Ps::aos::Vec3VArg b, 
+														const Ps::aos::Vec3VArg c,
+														const PxU8 triFlags,
+														Ps::aos::Vec3V& closestP,
+														bool& generateContact)
+{
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+	const Vec3V bc = V3Sub(c, b);
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V bp = V3Sub(p, b);
+	const Vec3V cp = V3Sub(p, c);
+
+	const FloatV d1 = V3Dot(ab, ap); //  snom
+	const FloatV d2 = V3Dot(ac, ap); //  tnom
+	const FloatV d3 = V3Dot(ab, bp); // -sdenom
+	const FloatV d4 = V3Dot(ac, bp); //  unom = d4 - d3
+	const FloatV d5 = V3Dot(ab, cp); //  udenom = d5 - d6
+	const FloatV d6 = V3Dot(ac, cp); // -tdenom
+	const FloatV unom = FSub(d4, d3);
+	const FloatV udenom = FSub(d5, d6);
+
+	const Vec3V n = V3Cross(ab, ac);
+	const VecCrossV crossA = V3PrepareCross(ap);
+	const VecCrossV crossB = V3PrepareCross(bp);
+	const VecCrossV crossC = V3PrepareCross(cp);
+	const Vec3V bCrossC = V3Cross(crossB, crossC);
+	const Vec3V cCrossA = V3Cross(crossC, crossA);
+	const Vec3V aCrossB = V3Cross(crossA, crossB);
+
+	//const FloatV va = V3Dot(n, bCrossC);//edge region of BC, signed area rbc, u = S(rbc)/S(abc) for a
+	//const FloatV vb = V3Dot(n, cCrossA);//edge region of AC, signed area rac, v = S(rca)/S(abc) for b
+	//const FloatV vc = V3Dot(n, aCrossB);//edge region of AB, signed area rab, w = S(rab)/S(abc) for c
+	
+	//check if p in vertex region outside a
+	const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
+	const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
+	const BoolV con0 = BAnd(con00, con01); // vertex region a
+
+	if(BAllEqTTTT(con0))
+	{
+		//Vertex 0
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_01) || (triFlags & Gu::ETD_CONVEX_EDGE_20);
+		closestP = a;
+		return V3Dot(ap, ap);
+	}
+
+	//check if p in vertex region outside b
+	const BoolV con10 = FIsGrtrOrEq(d3, zero);
+	const BoolV con11 = FIsGrtrOrEq(d3, d4);
+	const BoolV con1 = BAnd(con10, con11); // vertex region b
+	if(BAllEqTTTT(con1))
+	{
+		//Vertex 1
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_01) || (triFlags & Gu::ETD_CONVEX_EDGE_12);
+		closestP = b;
+		return V3Dot(bp, bp);
+	}
+
+	//check if p in vertex region outside c
+	const BoolV con20 = FIsGrtrOrEq(d6, zero);
+	const BoolV con21 = FIsGrtrOrEq(d6, d5); 
+	const BoolV con2 = BAnd(con20, con21); // vertex region c
+	if(BAllEqTTTT(con2))
+	{
+		//Vertex 2
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_12) || (triFlags & Gu::ETD_CONVEX_EDGE_20);
+		closestP = c;
+		return V3Dot(cp, cp);
+	}
+
+	//check if p in edge region of AB
+	//const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
+	const FloatV vc = V3Dot(n, aCrossB);//edge region of AB, signed area rab, w = S(rab)/S(abc) for c
+	const BoolV con30 = FIsGrtr(zero, vc);
+	const BoolV con31 = FIsGrtrOrEq(d1, zero);
+	const BoolV con32 = FIsGrtr(zero, d3);
+	const BoolV con3 = BAnd(con30, BAnd(con31, con32));
+	if(BAllEqTTTT(con3))
+	{
+		// Edge 01
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_01) != 0;
+		const FloatV sScale = FDiv(d1, FSub(d1, d3));
+		const Vec3V closest3 = V3ScaleAdd(ab, sScale, a);//V3Add(a, V3Scale(ab, sScale));
+		const Vec3V vv = V3Sub(p, closest3);
+		closestP = closest3;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of BC
+	//const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
+	const FloatV va = V3Dot(n, bCrossC);//edge region of BC, signed area rbc, u = S(rbc)/S(abc) for a
+	
+	const BoolV con40 = FIsGrtr(zero, va);
+	const BoolV con41 = FIsGrtrOrEq(d4, d3);
+	const BoolV con42 = FIsGrtrOrEq(d5, d6);
+	const BoolV con4 = BAnd(con40, BAnd(con41, con42)); 
+	if(BAllEqTTTT(con4))
+	{
+		// Edge 12
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_12) != 0;
+		const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
+		const Vec3V closest4 = V3ScaleAdd(bc, uScale, b);//V3Add(b, V3Scale(bc, uScale));
+		const Vec3V vv = V3Sub(p, closest4);
+		closestP = closest4;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of AC
+	//const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
+	const FloatV vb = V3Dot(n, cCrossA);//edge region of AC, signed area rac, v = S(rca)/S(abc) for b
+	const BoolV con50 = FIsGrtr(zero, vb);
+	const BoolV con51 = FIsGrtrOrEq(d2, zero);
+	const BoolV con52 = FIsGrtr(zero, d6);
+	const BoolV con5 = BAnd(con50, BAnd(con51, con52));
+	if(BAllEqTTTT(con5))
+	{
+		//Edge 20
+		generateContact = (triFlags & Gu::ETD_CONVEX_EDGE_20) != 0;
+		const FloatV tScale = FDiv(d2, FSub(d2, d6));
+		const Vec3V closest5 = V3ScaleAdd(ac, tScale, a);//V3Add(a, V3Scale(ac, tScale));
+		const Vec3V vv = V3Sub(p, closest5);
+		closestP = closest5;
+		return V3Dot(vv, vv);
+	}
+
+	generateContact = true;
+
+	//P must project inside face region. Compute Q using Barycentric coordinates
+	const FloatV nn = V3Dot(n, n);
+	const FloatV t = FDiv(V3Dot(n, V3Sub(a, p)), nn); 
+	const Vec3V vv = V3Scale(n, t);
+	closestP = V3Add(p, vv);
+	return V3Dot(vv, vv);	
+}
+
+bool Gu::PCMSphereVsMeshContactGeneration::processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
+{
+	PX_UNUSED(triangleIndex);
+	PX_UNUSED(vertInds);
+
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+
+	const Vec3V v0 = V3LoadU(verts[0]);
+	const Vec3V v1 = V3LoadU(verts[1]);
+	const Vec3V v2 = V3LoadU(verts[2]);
+
+	const Vec3V v10 = V3Sub(v1, v0);
+	const Vec3V v20 = V3Sub(v2, v0);
+
+	const Vec3V n = V3Normalize(V3Cross(v10, v20));//(p1 - p0).cross(p2 - p0).getNormalized();
+	const FloatV d = V3Dot(v0, n);//d = -p0.dot(n);
+
+	const FloatV dist0 = FSub(V3Dot(mSphereCenter, n), d);//p.dot(n) + d;
+	
+	// Backface culling
+	if(FAllGrtr(zero, dist0))
+		return false;
+
+
+	const FloatV tolerance = FLoad(0.996);//around 5 degree
+	//FloatV u, v;
+	Vec3V closestP;
+	//mShereCenter will be in the local space of the triangle mesh
+	//const FloatV sqDist = Gu::distancePointTriangleSquared(mSphereCenter, v0, v1, v2, u, v, closestP);
+	bool generateContact = false;
+	const FloatV sqDist = pcmDistancePointTriangleSquared(mSphereCenter, v0, v1, v2, triFlags, closestP, generateContact);
+
+	//sphere center is on the triangle surface, we take triangle normal as the patchNormal. Otherwise, we need to calculate the patchNormal
+	Vec3V patchNormal = n;
+	if(FAllGrtr(sqDist, FEps() ))
+		patchNormal = V3Normalize(V3Sub(mSphereCenter, closestP));
+
+	const FloatV cosTheta = V3Dot(patchNormal, n);
+	
+	//sphere overlap with triangles
+	if(FAllGrtr(mSqInflatedSphereRadius, sqDist) && (generateContact || FAllGrtr(cosTheta, tolerance)))
+	{
+		const FloatV dist = FSqrt(sqDist);
+	
+		PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
+
+		bool foundPatch = false;
+		if(mNumContactPatch > 0)
+		{
+			if(FAllGrtr(V3Dot(mContactPatch[mNumContactPatch-1].mPatchNormal, patchNormal), mAcceptanceEpsilon))
+			{
+				PCMContactPatch& patch = mContactPatch[mNumContactPatch-1];
+
+				PX_ASSERT((patch.mEndIndex - patch.mStartIndex) == 1);
+				
+				if(FAllGrtr(patch.mPatchMaxPen, dist))
+				{
+					//overwrite the old contact
+					mManifoldContacts[patch.mStartIndex].mLocalPointA = zeroV;//in sphere's space
+					mManifoldContacts[patch.mStartIndex].mLocalPointB = closestP;
+					mManifoldContacts[patch.mStartIndex].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(patchNormal), dist);
+					mManifoldContacts[patch.mStartIndex].mFaceIndex = triangleIndex;
+					patch.mPatchMaxPen = dist;
+				}
+				
+				foundPatch = true;
+			}
+		}
+		if(!foundPatch)
+		{
+			mManifoldContacts[mNumContacts].mLocalPointA = zeroV;//in sphere's space
+			mManifoldContacts[mNumContacts].mLocalPointB = closestP;
+			mManifoldContacts[mNumContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(patchNormal), dist);
+			mManifoldContacts[mNumContacts++].mFaceIndex = triangleIndex;
+			
+			mContactPatch[mNumContactPatch].mStartIndex = mNumContacts - 1;
+			mContactPatch[mNumContactPatch].mEndIndex = mNumContacts;
+			mContactPatch[mNumContactPatch].mPatchMaxPen = dist;
+			mContactPatch[mNumContactPatch++].mPatchNormal = patchNormal;
+		}
+
+		PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
+
+		if(mNumContacts >= 16)
+		{
+			PX_ASSERT(mNumContacts <= 64);
+			processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE);
+		}
+		
+	}
+	return true;
+}
+
+
+void Gu::PCMCapsuleVsMeshContactGeneration::generateEE(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg sqInflatedRadius,
+													   const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex, const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,
+														Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	using namespace Ps::aos;
+	const FloatV zero = FZero();
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V n = V3Cross(ab, normal);
+	const FloatV d = V3Dot(n, a);
+	const FloatV np = V3Dot(n, p);
+	const FloatV nq = V3Dot(n,q);
+	const FloatV signP = FSub(np, d);
+	const FloatV signQ = FSub(nq, d);
+	const FloatV temp = FMul(signP, signQ);
+	if(FAllGrtr(temp, zero)) return;//If both points in the same side as the plane, no intersect points
+	
+	// if colliding edge (p3,p4) and plane are parallel return no collision
+	const Vec3V pq = V3Sub(q, p);
+	const FloatV npq= V3Dot(n, pq); 
+	if(FAllEq(npq, zero))	return;
+
+	const FloatV one = FOne();
+	//calculate the intersect point in the segment pq
+	const FloatV segTValue = FDiv(FSub(d, np), npq);
+	const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
+
+	//calculate a normal perpendicular to ray localPointA + normal, 2D segment segment intersection
+	const Vec3V perNormal = V3Cross(normal, pq);
+	const Vec3V ap = V3Sub(localPointA, a);
+	const FloatV nom = V3Dot(perNormal, ap);
+	const FloatV denom = V3Dot(perNormal, ab);
+
+	//const FloatV tValue = FClamp(FDiv(nom, denom), zero, FOne());
+	const FloatV tValue = FDiv(nom, denom);
+	const BoolV con = BAnd(FIsGrtrOrEq(one, tValue), FIsGrtrOrEq(tValue, zero));
+	if(BAllEqFFFF(con))
+		return;
+
+	//const Vec3V localPointB = V3ScaleAdd(ab, tValue, a); v = V3Sub(localPointA, localPointB); v =  V3NegScaleSub(ab, tValue, tap)
+	const Vec3V v = V3NegScaleSub(ab, tValue, ap);
+	const FloatV sqDist = V3Dot(v, v);
+	
+	if(FAllGrtr(sqInflatedRadius, sqDist))
+	{
+		
+		const Vec3V localPointB = V3Sub(localPointA, v);
+		const FloatV signedDist = V3Dot(v, normal);
+	
+		manifoldContacts[numContacts].mLocalPointA = localPointA;
+		manifoldContacts[numContacts].mLocalPointB = localPointB;
+		manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
+		manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
+	}
+}
+
+
+
+void Gu::PCMCapsuleVsMeshContactGeneration::generateEEContacts(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,
+															   const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg normal,
+															   const PxU32 triangleIndex, const Ps::aos::Vec3VArg p, 
+															   const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg sqInflatedRadius,
+															   Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	
+	using namespace Ps::aos;
+	generateEE(p, q, sqInflatedRadius, normal, triangleIndex, a, b, manifoldContacts, numContacts);
+	generateEE(p, q, sqInflatedRadius, normal, triangleIndex, b, c, manifoldContacts, numContacts);
+	generateEE(p, q, sqInflatedRadius, normal, triangleIndex, a, c, manifoldContacts, numContacts);
+
+}
+
+
+void Gu::PCMCapsuleVsMeshContactGeneration::generateEEMTD(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg inflatedRadius,
+																const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex, const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,
+																Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	using namespace Ps::aos;
+	const FloatV zero = FZero();
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V n = V3Cross(ab, normal);
+	const FloatV d = V3Dot(n, a);
+	const FloatV np = V3Dot(n, p);
+	const FloatV nq = V3Dot(n,q);
+	const FloatV signP = FSub(np, d);
+	const FloatV signQ = FSub(nq, d);
+	const FloatV temp = FMul(signP, signQ);
+	if(FAllGrtr(temp, zero)) return;//If both points in the same side as the plane, no intersect points
+	
+	// if colliding edge (p3,p4) and plane are parallel return no collision
+	const Vec3V pq = V3Sub(q, p);
+	const FloatV npq= V3Dot(n, pq); 
+	if(FAllEq(npq, zero))	return;
+
+	//calculate the intersect point in the segment pq
+	const FloatV segTValue = FDiv(FSub(d, np), npq);
+	const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
+
+	//calculate a normal perpendicular to ray localPointA + normal, 2D segment segment intersection
+	const Vec3V perNormal = V3Cross(normal, pq);
+	const Vec3V ap = V3Sub(localPointA, a);
+	const FloatV nom = V3Dot(perNormal, ap);
+	const FloatV denom = V3Dot(perNormal, ab);
+
+	const FloatV tValue = FClamp(FDiv(nom, denom), zero, FOne());
+
+	const Vec3V v = V3NegScaleSub(ab, tValue, ap);
+	const FloatV signedDist = V3Dot(v, normal);
+
+	if(FAllGrtr(inflatedRadius, signedDist))
+	{
+		
+		const Vec3V localPointB = V3Sub(localPointA, v);
+		manifoldContacts[numContacts].mLocalPointA = localPointA;
+		manifoldContacts[numContacts].mLocalPointB = localPointB;
+		manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
+		manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
+	}
+}
+
+
+void Gu::PCMCapsuleVsMeshContactGeneration::generateEEContactsMTD(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,
+															   const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg normal,
+															   const PxU32 triangleIndex, const Ps::aos::Vec3VArg p, 
+															   const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg inflatedRadius,
+															   Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	
+	using namespace Ps::aos;
+	generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, a, b, manifoldContacts, numContacts);
+	generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, b, c, manifoldContacts, numContacts);
+	generateEEMTD(p, q, inflatedRadius, normal, triangleIndex, a, c, manifoldContacts, numContacts);
+
+}
+
+bool Gu::PCMCapsuleVsMeshContactGeneration::generateContacts(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,
+															 const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg planeNormal, 
+															 const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex,
+															 const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q,
+															 const Ps::aos::FloatVArg inflatedRadius,
+															 Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+
+	using namespace Ps::aos;
+	
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V aq = V3Sub(q, a);
+
+	//This is used to calculate the barycentric coordinate
+	const FloatV d00 = V3Dot(ab, ab);
+	const FloatV d01 = V3Dot(ab, ac);
+	const FloatV d11 = V3Dot(ac, ac);
+	const FloatV bdenom = FRecip(FSub(FMul(d00,d11), FMul(d01, d01)));
+
+	//compute the intersect point of p and triangle plane abc
+	const FloatV inomp = V3Dot(planeNormal, V3Neg(ap));
+	const FloatV ideom = V3Dot(planeNormal, normal);
+
+	const FloatV ipt = FSel(FIsGrtr(ideom, FZero()), FDiv(inomp, ideom), FZero());
+	//compute the distance from triangle plane abc
+	const FloatV dist3 = V3Dot(ap, planeNormal);
+	
+	const Vec3V closestP31 = V3ScaleAdd(normal, ipt, p);
+	const Vec3V closestP30 = p;
+
+
+	//Compute the barycentric coordinate for project point of q
+	const Vec3V pV20 = V3Sub(closestP31, a);
+	const FloatV pD20 = V3Dot(pV20, ab);
+	const FloatV pD21 = V3Dot(pV20, ac);
+	const FloatV v0 = FMul(FSub(FMul(d11, pD20), FMul(d01, pD21)), bdenom);
+	const FloatV w0 = FMul(FSub(FMul(d00, pD21), FMul(d01, pD20)), bdenom);
+
+	//check closestP3 is inside the triangle
+	const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
+
+	
+	const BoolV tempCon0 = BAnd(con0, FIsGrtr(inflatedRadius, dist3));
+	if(BAllEqTTTT(tempCon0))
+	{
+		manifoldContacts[numContacts].mLocalPointA = closestP30;//transform to B space, it will get convert to A space later
+		manifoldContacts[numContacts].mLocalPointB = closestP31;
+		manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), FNeg(ipt));
+		manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
+	}
+	
+	const FloatV inomq = V3Dot(planeNormal, V3Neg(aq));
+
+	//compute the distance of q and triangle plane abc
+	const FloatV dist4 = V3Dot(aq, planeNormal);
+
+	const FloatV iqt = FSel(FIsGrtr(ideom, FZero()), FDiv(inomq, ideom), FZero());
+
+	const Vec3V closestP41 = V3ScaleAdd(normal, iqt, q);
+	const Vec3V closestP40 = q;
+
+	//Compute the barycentric coordinate for project point of q
+	const Vec3V qV20 = V3Sub(closestP41, a);
+	const FloatV qD20 = V3Dot(qV20, ab);
+	const FloatV qD21 = V3Dot(qV20, ac);
+	const FloatV v1 = FMul(FSub(FMul(d11, qD20), FMul(d01, qD21)), bdenom);
+	const FloatV w1 = FMul(FSub(FMul(d00, qD21), FMul(d01, qD20)), bdenom);
+
+
+	const BoolV con1 = isValidTriangleBarycentricCoord(v1, w1);
+	
+	const BoolV tempCon1 = BAnd(con1, FIsGrtr(inflatedRadius, dist4));
+	if(BAllEqTTTT(tempCon1))
+	{
+		manifoldContacts[numContacts].mLocalPointA = closestP40;
+		manifoldContacts[numContacts].mLocalPointB = closestP41;
+		manifoldContacts[numContacts].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal),FNeg(iqt));
+		manifoldContacts[numContacts++].mFaceIndex = triangleIndex;
+
+	}
+	
+	return false;
+}
+
+
+/*
+	t is the barycenteric coordinate of a segment
+	u is the barycenteric coordinate of a triangle
+	v is the barycenteric coordinate of a triangle
+*/
+Ps::aos::FloatV pcmDistanceSegmentTriangleSquared(	const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q,
+													const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, const Ps::aos::Vec3VArg c,
+													Ps::aos::FloatV& t, Ps::aos::FloatV& u, Ps::aos::FloatV& v)
+{
+	using namespace Ps::aos;
+	
+	const FloatV one = FOne();
+	const FloatV zero = FZero();
+
+	const Vec3V pq = V3Sub(q, p);
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+	const Vec3V bc = V3Sub(c, b);
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V aq = V3Sub(q, a);
+
+	const Vec3V n =V3Normalize(V3Cross(ab, ac)); // normalize vector
+
+	const Vec4V combinedDot = V3Dot4(ab, ab, ab, ac, ac, ac, ap, n);
+	const FloatV d00 = V4GetX(combinedDot);
+	const FloatV d01 = V4GetY(combinedDot);
+	const FloatV d11 = V4GetZ(combinedDot);
+	const FloatV dist3 = V4GetW(combinedDot);
+
+	const FloatV bdenom = FRecip(FSub(FMul(d00,d11), FMul(d01, d01)));
+	
+
+	const FloatV sqDist3 = FMul(dist3, dist3);
+
+
+	//compute the closest point of q and triangle plane abc
+	const FloatV dist4 = V3Dot(aq, n);
+	const FloatV sqDist4 = FMul(dist4, dist4);
+	const FloatV dMul = FMul(dist3, dist4);
+	const BoolV con = FIsGrtr(zero, dMul);
+
+	if(BAllEqTTTT(con))
+	{
+		//compute the intersect point
+		const FloatV nom = FNeg(V3Dot(n, ap));
+		const FloatV denom = FRecip(V3Dot(n, pq));
+		const FloatV t0 = FMul(nom, denom);
+		const Vec3V ip = V3ScaleAdd(pq, t0, p);//V3Add(p, V3Scale(pq, t));
+		const Vec3V v2 = V3Sub(ip, a);
+		const FloatV d20 = V3Dot(v2, ab);
+		const FloatV d21 = V3Dot(v2, ac);
+
+		const FloatV v0 = FMul(FNegScaleSub(d01, d21, FMul(d11, d20)), bdenom);
+		const FloatV w0 = FMul(FNegScaleSub(d01, d20, FMul(d00, d21)), bdenom);
+
+		const BoolV con0 = isValidTriangleBarycentricCoord(v0, w0);
+		if(BAllEqTTTT(con0))
+		{
+			t = t0;
+			u = v0;
+			v = w0;
+			return zero;
+		}
+	}
+
+	const Vec3V closestP31 = V3NegScaleSub(n, dist3, p);//V3Sub(p, V3Scale(n, dist3));
+	const Vec3V closestP41 = V3NegScaleSub(n, dist4, q);// V3Sub(q, V3Scale(n, dist4));
+
+	//Compute the barycentric coordinate for project point of q
+	const Vec3V pV20 = V3Sub(closestP31, a);
+	const Vec3V qV20 = V3Sub(closestP41, a);
+
+	const Vec4V pD2 = V3Dot4(pV20, ab, pV20, ac, qV20, ab, qV20, ac);
+
+	const Vec4V pD2Swizzle = V4PermYXWZ(pD2);
+
+	const Vec4V d11d00 = V4UnpackXY(V4Splat(d11), V4Splat(d00));
+
+	const Vec4V v0w0v1w1 = V4Scale(V4NegMulSub(V4Splat(d01), pD2Swizzle, V4Mul(d11d00, pD2)), bdenom);
+
+	const FloatV v0 = V4GetX(v0w0v1w1);
+	const FloatV w0 = V4GetY(v0w0v1w1);
+	const FloatV v1 = V4GetZ(v0w0v1w1);
+	const FloatV w1 = V4GetW(v0w0v1w1);
+
+	const BoolV _con = isValidTriangleBarycentricCoord2(v0w0v1w1);
+
+	const BoolV con0 = BGetX(_con);
+	const BoolV con1 = BGetY(_con);
+
+
+	const BoolV cond2 = BAnd(con0, con1);	
+
+	if(BAllEqTTTT(cond2))
+	{
+		/*
+			both p and q project points are interior point 
+		*/
+		const BoolV d2 = FIsGrtr(sqDist4, sqDist3);
+		t = FSel(d2, zero, one);
+		u = FSel(d2, v0, v1);
+		v = FSel(d2, w0, w1);
+		return FSel(d2, sqDist3, sqDist4);
+	}
+	else
+	{
+		Vec4V t40, t41;
+		const Vec4V sqDist44 = pcmDistanceSegmentSegmentSquared4(p,pq,a,ab, b,bc, a,ac, a,ab, t40, t41); 
+
+		const FloatV t00 = V4GetX(t40);
+		const FloatV t10 = V4GetY(t40);
+		const FloatV t20 = V4GetZ(t40);
+
+		const FloatV t01 = V4GetX(t41);
+		const FloatV t11 = V4GetY(t41);
+		const FloatV t21 = V4GetZ(t41);
+
+		//edge ab
+		const FloatV u01 = t01;
+		const FloatV v01 = zero;
+
+		//edge bc
+		const FloatV u11 = FSub(one, t11);
+		const FloatV v11 = t11;
+
+		//edge ac
+		const FloatV u21 = zero;
+		const FloatV v21 = t21;
+
+		const FloatV sqDist0(V4GetX(sqDist44));
+		const FloatV sqDist1(V4GetY(sqDist44));
+		const FloatV sqDist2(V4GetZ(sqDist44));
+
+		const BoolV con2 = BAnd(FIsGrtr(sqDist1, sqDist0), FIsGrtr(sqDist2, sqDist0));
+		const BoolV con3 = FIsGrtr(sqDist2, sqDist1);
+		const FloatV sqDistPE = FSel(con2, sqDist0, FSel(con3, sqDist1, sqDist2));
+		const FloatV uEdge = FSel(con2, u01, FSel(con3, u11, u21));
+		const FloatV vEdge = FSel(con2, v01, FSel(con3, v11, v21));
+		const FloatV tSeg = FSel(con2,  t00, FSel(con3, t10, t20));
+
+
+		if(BAllEqTTTT(con0))
+		{
+			//p's project point is an interior point
+			const BoolV d2 = FIsGrtr(sqDistPE, sqDist3);
+			t = FSel(d2, zero, tSeg);
+			u = FSel(d2, v0, uEdge);
+			v = FSel(d2, w0, vEdge);
+			return FSel(d2, sqDist3, sqDistPE);
+		}
+		else if(BAllEqTTTT(con1))
+		{
+			//q's project point is an interior point
+			const BoolV d2 = FIsGrtr(sqDistPE, sqDist4);
+			t = FSel(d2, one, tSeg);
+			u = FSel(d2, v1, uEdge);
+			v = FSel(d2, w1, vEdge);
+			return FSel(d2, sqDist4, sqDistPE);
+		}
+		else
+		{
+			t = tSeg;
+			u = uEdge;
+			v = vEdge;
+			return sqDistPE;
+		}
+
+	}
+
+}
+
+
+static bool selectNormal(const Ps::aos::FloatVArg u, Ps::aos::FloatVArg v, PxU8 data)
+{
+	using namespace Ps::aos;
+	const FloatV zero = FZero();
+	const FloatV one = FOne();
+	// Analysis
+	if(FAllEq(u, zero))
+	{
+		if(FAllEq(v,zero))
+		{
+			// Vertex 0
+			if(!(data & (Gu::ETD_CONVEX_EDGE_01|Gu::ETD_CONVEX_EDGE_20)))
+				return true;
+		}
+		else if(FAllEq(v,one))
+		{
+			// Vertex 2
+			if(!(data & (Gu::ETD_CONVEX_EDGE_12|Gu::ETD_CONVEX_EDGE_20)))
+				return true;
+		}
+		else
+		{
+			// Edge 0-2
+			if(!(data & Gu::ETD_CONVEX_EDGE_20))
+				return true;
+		}
+	}
+	else if(FAllEq(u,one))
+	{
+		if(FAllEq(v,zero))
+		{
+			// Vertex 1
+			if(!(data & (Gu::ETD_CONVEX_EDGE_01|Gu::ETD_CONVEX_EDGE_12)))
+				return true;
+
+		}
+	}
+	else
+	{
+		if(FAllEq(v,zero))
+		{
+			// Edge 0-1
+			if(!(data & Gu::ETD_CONVEX_EDGE_01))
+				return true;
+		}
+		else
+		{
+			const FloatV threshold = FLoad(0.9999f);
+			const FloatV temp = FAdd(u, v);
+			if(FAllGrtrOrEq(temp, threshold))
+			{
+				// Edge 1-2
+				if(!(data & Gu::ETD_CONVEX_EDGE_12))
+					return true;
+			}
+			else
+			{
+				// Face
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+
+bool Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(const PxVec3* verts, const PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds)
+{
+	PX_UNUSED(triangleIndex);
+	PX_UNUSED(vertInds);
+
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+
+	const Vec3V p0 = V3LoadU(verts[0]);
+	const Vec3V p1 = V3LoadU(verts[1]);
+	const Vec3V p2 = V3LoadU(verts[2]);
+
+	const Vec3V p10 = V3Sub(p1, p0);
+	const Vec3V p20 = V3Sub(p2, p0);
+
+	const Vec3V n = V3Normalize(V3Cross(p10, p20));//(p1 - p0).cross(p2 - p0).getNormalized();
+	const FloatV d = V3Dot(p0, n);//d = -p0.dot(n);
+
+	const FloatV dist = FSub(V3Dot(mCapsule.getCenter(), n), d);//p.dot(n) + d;
+	
+	// Backface culling
+	if(FAllGrtr(zero, dist))
+		return false;
+
+
+	FloatV t, u, v;
+	const FloatV sqDist = pcmDistanceSegmentTriangleSquared(mCapsule.p0, mCapsule.p1, p0, p1, p2, t, u, v);
+	
+	if(FAllGrtr(mSqInflatedRadius, sqDist))
+	{
+
+		Vec3V patchNormalInTriangle;
+		if(selectNormal(u, v, triFlags))
+		{
+			patchNormalInTriangle = n;
+		}
+		else
+		{
+			if(FAllEq(sqDist, zero))
+			{
+				//segment intersect with the triangle
+				patchNormalInTriangle = n;
+			}
+			else
+			{
+				const Vec3V pq = V3Sub(mCapsule.p1, mCapsule.p0);
+				const Vec3V pointOnSegment = V3ScaleAdd(pq, t, mCapsule.p0);
+				const FloatV w = FSub(FOne(), FAdd(u, v));
+				const Vec3V pointOnTriangle = V3ScaleAdd(p0, w, V3ScaleAdd(p1, u, V3Scale(p2, v)));
+				patchNormalInTriangle = V3Normalize(V3Sub(pointOnSegment, pointOnTriangle));
+				
+			}
+		}
+
+		const PxU32 previousNumContacts = mNumContacts;
+
+		generateContacts(p0, p1, p2, n, patchNormalInTriangle, triangleIndex, mCapsule.p0, mCapsule.p1, mInflatedRadius, mManifoldContacts, mNumContacts);
+		//ML: this need to use the sqInflatedRadius to avoid some bad contacts
+		generateEEContacts(p0, p1, p2,patchNormalInTriangle,  triangleIndex, mCapsule.p0, mCapsule.p1, mSqInflatedRadius, mManifoldContacts, mNumContacts);
+		
+
+		PxU32 numContacts = mNumContacts - previousNumContacts;
+
+		if(numContacts > 0)
+		{ 
+			FloatV maxPen = FMax();
+			for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
+			{
+				const FloatV pen = V4GetW(mManifoldContacts[i].mLocalNormalPen);
+				mManifoldContacts[i].mLocalPointA = mMeshToConvex.transform(mManifoldContacts[i].mLocalPointA);
+				maxPen = FMin(maxPen, pen);
+			}
+
+			for(PxU32 i = previousNumContacts; i<mNumContacts; ++i)
+			{
+				Vec3V contact0 = mManifoldContacts[i].mLocalPointB;
+				for(PxU32 j=i+1; j<mNumContacts; ++j)
+				{
+					Vec3V contact1 = mManifoldContacts[j].mLocalPointB;
+					Vec3V dif = V3Sub(contact1, contact0);
+					FloatV d1 = V3Dot(dif, dif);
+					if(FAllGrtr(mSqReplaceBreakingThreshold, d1))
+					{
+						mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
+						mNumContacts--;
+						j--;
+					}
+				}
+			}
+
+			PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
+
+			addManifoldPointToPatch(patchNormalInTriangle, maxPen, previousNumContacts);
+			
+			PX_ASSERT(mNumContactPatch <PCM_MAX_CONTACTPATCH_SIZE);
+			if(mNumContacts >= 16)
+			{
+				PX_ASSERT(mNumContacts <= 64);
+				processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE);
+			}
+		}
+	}
+
+	return true;
+}
+
+bool Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(const TriangleV& triangleV, const PxU32 triangleIndex, const CapsuleV& capsule, const Ps::aos::FloatVArg inflatedRadius, const PxU8 trigFlag,
+															Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts)
+{
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+
+	const Vec3V p0 = triangleV.verts[0];
+	const Vec3V p1 = triangleV.verts[1];
+	const Vec3V p2 = triangleV.verts[2];
+
+	const Vec3V n = triangleV.normal();
+
+	const FloatV sqInflatedRadius = FMul(inflatedRadius, inflatedRadius);
+	
+
+	FloatV t, u, v;
+	const FloatV sqDist = pcmDistanceSegmentTriangleSquared(capsule.p0, capsule.p1, p0, p1, p2, t, u, v);
+	
+	if(FAllGrtr(sqInflatedRadius, sqDist))
+	{
+
+		Vec3V patchNormalInTriangle;
+		if(selectNormal(u, v, trigFlag))
+		{
+			patchNormalInTriangle = n;
+		}
+		else
+		{
+			if(FAllEq(sqDist, zero))
+			{
+				//segment intersect with the triangle
+				patchNormalInTriangle = n;
+			}
+			else
+			{
+				const Vec3V pq = V3Sub(capsule.p1, capsule.p0);
+				const Vec3V pointOnSegment = V3ScaleAdd(pq, t, capsule.p0);
+				const FloatV w = FSub(FOne(), FAdd(u, v));
+				const Vec3V pointOnTriangle = V3ScaleAdd(p0, w, V3ScaleAdd(p1, u, V3Scale(p2, v)));
+				patchNormalInTriangle = V3Normalize(V3Sub(pointOnSegment, pointOnTriangle));
+			}
+		}
+
+		generateContacts(p0, p1, p2, n, patchNormalInTriangle, triangleIndex, capsule.p0, capsule.p1, inflatedRadius, manifoldContacts, numContacts);
+	
+		generateEEContactsMTD(p0, p1, p2, patchNormalInTriangle, triangleIndex, capsule.p0, capsule.p1, inflatedRadius, manifoldContacts, numContacts);
+		
+		
+	}
+
+	return true;
+
+}
+
+}
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.h
new file mode 100644
index 00000000..c2af8d6f
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexCommon.h
@@ -0,0 +1,419 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_CONTACT_CONVEX_COMMON_H
+#define GU_PCM_CONTACT_CONVEX_COMMON_H
+
+#define	PCM_MAX_CONTACTPATCH_SIZE	32
+
+#include "GuContactBuffer.h"
+#include "GuVecCapsule.h"
+#include "GuPCMTriangleContactGen.h"
+#include "PsInlineArray.h"
+#include "GuTriangleCache.h"
+
+namespace physx
+{
+
+namespace Gu
+{
+
+class PCMMeshContactGeneration
+{
+	PX_NOCOPY(PCMMeshContactGeneration)
+public:
+	PCMContactPatch							mContactPatch[PCM_MAX_CONTACTPATCH_SIZE];
+	PCMContactPatch*						mContactPatchPtr[PCM_MAX_CONTACTPATCH_SIZE];
+	const Ps::aos::FloatV					mContactDist;
+	const Ps::aos::FloatV					mReplaceBreakingThreshold;
+	const Ps::aos::PsTransformV&			mConvexTransform;
+	const Ps::aos::PsTransformV&			mMeshTransform;
+	Gu::MultiplePersistentContactManifold&	mMultiManifold;
+	Gu::ContactBuffer&						mContactBuffer;
+
+	Ps::aos::FloatV							mAcceptanceEpsilon;
+	Ps::aos::FloatV							mSqReplaceBreakingThreshold;
+	Ps::aos::PsMatTransformV				mMeshToConvex;
+	Gu::MeshPersistentContact*				mManifoldContacts;
+	PxU32									mNumContacts;
+	PxU32									mNumContactPatch;
+	PxU32									mNumCalls;
+	Cm::RenderOutput*						mRenderOutput;
+
+	PCMMeshContactGeneration(
+		const Ps::aos::FloatVArg	contactDist,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+		const Ps::aos::PsTransformV& convexTransform,
+		const Ps::aos::PsTransformV& meshTransform,
+		Gu::MultiplePersistentContactManifold& multiManifold,
+		Gu::ContactBuffer& contactBuffer,
+		Cm::RenderOutput*  renderOutput
+
+	) :
+		mContactDist(contactDist),
+		mReplaceBreakingThreshold(replaceBreakingThreshold),
+		mConvexTransform(convexTransform),
+		mMeshTransform(meshTransform),
+		mMultiManifold(multiManifold),
+		mContactBuffer(contactBuffer),
+		mRenderOutput(renderOutput)
+		
+	{
+		using namespace Ps::aos;
+		mNumContactPatch = 0;
+		mNumContacts = 0;
+		mNumCalls = 0;
+
+		mMeshToConvex = mConvexTransform.transformInv(mMeshTransform);
+
+		//Assign the PCMContactPatch to the PCMContactPathPtr
+		for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
+		{
+			mContactPatchPtr[i] = &mContactPatch[i];
+		}
+		mManifoldContacts = PX_CP_TO_MPCP(contactBuffer.contacts);
+
+		mSqReplaceBreakingThreshold = FMul(replaceBreakingThreshold, replaceBreakingThreshold);
+
+		mAcceptanceEpsilon = FLoad(0.996);//5 degree
+		//mAcceptanceEpsilon = FloatV_From_F32(0.9999);//5 degree
+	}
+
+	template <PxU32 TriangleCount, typename Derived>
+	bool processTriangleCache(Gu::TriangleCache<TriangleCount>& cache)
+	{
+		PxU32 count = cache.mNumTriangles;
+		PxVec3* verts = cache.mVertices;
+		PxU32* vertInds = cache.mIndices;
+		PxU32* triInds = cache.mTriangleIndex;
+		PxU8* edgeFlags = cache.mEdgeFlags;
+		while(count--)
+		{
+			(static_cast<Derived*>(this))->processTriangle(verts, *triInds, *edgeFlags, vertInds);
+			verts += 3;
+			vertInds += 3;
+			triInds++;
+			edgeFlags++;
+		}
+		return true;
+	}
+	void prioritizeContactPatches();
+	void addManifoldPointToPatch(const Ps::aos::Vec3VArg currentPatchNormal, const Ps::aos::FloatVArg maxPen, const PxU32 previousNumContacts);
+	void processContacts(const PxU8 maxContactPerManifold, const bool isNotLastPatch = true);
+};
+
+/*
+	This function is based on the current patch normal to either create a new patch or merge the manifold contacts in this patch with the manifold contacts in the last existing
+	patch. This means there might be more than GU_SINGLE_MANIFOLD_CACHE_SIZE in a SinglePersistentContactManifold.
+*/
+PX_FORCE_INLINE void PCMMeshContactGeneration::addManifoldPointToPatch(const Ps::aos::Vec3VArg currentPatchNormal, const Ps::aos::FloatVArg maxPen, const PxU32 previousNumContacts)
+{
+	using namespace Ps::aos;
+
+	bool foundPatch = false;
+	//we have existing patch
+	if(mNumContactPatch > 0)
+	{
+		//if the direction between the last existing patch normal and the current patch normal are within acceptance epsilon, which means we will be
+		//able to merge the last patch's contacts with the current patch's contacts. This is just to avoid to create an extra patch. We have some logic
+		//later to refine the patch again
+		if(FAllGrtr(V3Dot(mContactPatch[mNumContactPatch-1].mPatchNormal, currentPatchNormal), mAcceptanceEpsilon))
+		{
+			//get the last patch
+			PCMContactPatch& patch = mContactPatch[mNumContactPatch-1];
+
+			//remove duplicate contacts
+			for(PxU32 i = patch.mStartIndex; i<patch.mEndIndex; ++i)
+			{
+				for(PxU32 j = previousNumContacts; j<mNumContacts; ++j)
+				{
+					Vec3V dif = V3Sub(mManifoldContacts[j].mLocalPointB, mManifoldContacts[i].mLocalPointB);
+					FloatV d = V3Dot(dif, dif);
+					if(FAllGrtr(mSqReplaceBreakingThreshold, d))
+					{
+						if(FAllGrtr(V4GetW(mManifoldContacts[i].mLocalNormalPen), V4GetW(mManifoldContacts[j].mLocalNormalPen)))
+						{
+							//The new contact is deeper than the old contact so we keep the deeper contact
+							mManifoldContacts[i] = mManifoldContacts[j];
+						}
+						mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
+						mNumContacts--;
+						j--;
+					}
+				}
+			}
+			patch.mEndIndex = mNumContacts;
+			patch.mPatchMaxPen = FMin(patch.mPatchMaxPen, maxPen);
+			foundPatch = true;
+		}
+	}
+
+	//If there are no existing patch which match the currentPatchNormal, we will create a new patch
+	if(!foundPatch)
+	{
+		mContactPatch[mNumContactPatch].mStartIndex = previousNumContacts;
+		mContactPatch[mNumContactPatch].mEndIndex = mNumContacts;
+		mContactPatch[mNumContactPatch].mPatchMaxPen = maxPen;
+		mContactPatch[mNumContactPatch++].mPatchNormal = currentPatchNormal;
+	}
+}
+
+/*
+	This function sort the contact patch based on the max penetration so that deepest penetration contact patch will be in front of the less penetration contact
+	patch
+*/
+PX_FORCE_INLINE  void PCMMeshContactGeneration::prioritizeContactPatches()
+{
+	//we are using insertion sort to prioritize contact patchs
+	using namespace Ps::aos;
+	//sort the contact patch based on the max penetration
+	for(PxU32 i=1; i<mNumContactPatch; ++i)
+	{
+		const PxU32 indexi = i-1;
+		if(FAllGrtr(mContactPatchPtr[indexi]->mPatchMaxPen, mContactPatchPtr[i]->mPatchMaxPen))
+		{
+			//swap
+			PCMContactPatch* tmp = mContactPatchPtr[indexi];
+			mContactPatchPtr[indexi] = mContactPatchPtr[i];
+			mContactPatchPtr[i] = tmp;
+
+			for(PxI32 j=PxI32(i-2); j>=0; j--)
+			{
+				const PxU32 indexj = PxU32(j+1);
+				if(FAllGrtrOrEq(mContactPatchPtr[indexj]->mPatchMaxPen, mContactPatchPtr[j]->mPatchMaxPen))
+					break;
+				//swap
+				PCMContactPatch* temp = mContactPatchPtr[indexj];
+				mContactPatchPtr[indexj] = mContactPatchPtr[j];
+				mContactPatchPtr[j] = temp;
+			}
+		}
+	}
+}
+
+
+PX_FORCE_INLINE void PCMMeshContactGeneration::processContacts(const PxU8 maxContactPerManifold, bool isNotLastPatch)
+{
+	using namespace Ps::aos;
+	
+	if(mNumContacts != 0)
+	{
+		//reorder the contact patches based on the max penetration
+		prioritizeContactPatches();
+		//connect the patches which's angle between patch normals are within 5 degree
+		mMultiManifold.refineContactPatchConnective(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mAcceptanceEpsilon);
+		//get rid of duplicate manifold contacts in connected contact patches
+		mMultiManifold.reduceManifoldContactsInDifferentPatches(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mNumContacts, mSqReplaceBreakingThreshold);
+		//add the manifold contact to the corresponding manifold
+		mMultiManifold.addManifoldContactPoints(mManifoldContacts, mNumContacts, mContactPatchPtr, mNumContactPatch, mSqReplaceBreakingThreshold, mAcceptanceEpsilon, maxContactPerManifold);
+		
+		mNumContacts = 0;
+		mNumContactPatch = 0;
+
+		if(isNotLastPatch)
+		{
+			//remap the contact patch pointer to contact patch
+			for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
+			{
+				mContactPatchPtr[i] = &mContactPatch[i];
+			}
+		}
+	}
+}
+
+struct PCMDeferredPolyData
+{
+public:
+	PxVec3	mVerts[3];			//36
+	PxU32	mInds[3];			//48
+	PxU32	mTriangleIndex;		//52
+	PxU32	mFeatureIndex;		//56
+	PxU8	triFlags;			//57
+};
+
+#define MAX_CACHE_SIZE	128
+
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
+#endif
+
+class PCMConvexVsMeshContactGeneration : public PCMMeshContactGeneration
+{
+	PCMConvexVsMeshContactGeneration &operator=(PCMConvexVsMeshContactGeneration  &);
+
+public:
+
+	Gu::CacheMap<Gu::CachedEdge, MAX_CACHE_SIZE>	mEdgeCache;
+	Gu::CacheMap<Gu::CachedVertex, MAX_CACHE_SIZE>	mVertexCache;
+	Ps::aos::Vec3V									mHullCenterMesh;
+
+	Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE>&		mDeferredContacts;
+	const Gu::PolygonalData&						mPolyData;
+	SupportLocal*									mPolyMap;
+	const Cm::FastVertex2ShapeScaling&				mConvexScaling;  
+	bool											mIdtConvexScale;
+	Cm::RenderOutput*								mRenderOutput;
+	
+				
+	PCMConvexVsMeshContactGeneration(
+		const Ps::aos::FloatVArg				contactDistance,
+		const Ps::aos::FloatVArg				replaceBreakingThreshold,
+		const Ps::aos::PsTransformV&			convexTransform, 
+		const Ps::aos::PsTransformV&			meshTransform,
+		Gu::MultiplePersistentContactManifold&	multiManifold,
+		Gu::ContactBuffer&						contactBuffer,
+
+		const Gu::PolygonalData&				polyData,
+		SupportLocal*							polyMap,
+		Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE>&	 delayedContacts,
+		const Cm::FastVertex2ShapeScaling&		convexScaling,
+		bool									idtConvexScale,
+		Cm::RenderOutput*						renderOutput
+		
+	) : PCMMeshContactGeneration(contactDistance, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer, renderOutput),
+		mDeferredContacts(delayedContacts),
+		mPolyData(polyData),
+		mPolyMap(polyMap),
+		mConvexScaling(convexScaling),
+		mIdtConvexScale(idtConvexScale),
+		mRenderOutput(renderOutput)
+	{
+		using namespace Ps::aos;
+	
+		// Hull center in local space
+		const Vec3V hullCenterLocal = V3LoadU(mPolyData.mCenter);
+		// Hull center in mesh space
+		mHullCenterMesh = mMeshToConvex.transformInv(hullCenterLocal);
+
+	}
+
+	bool generateTriangleFullContactManifold(Gu::TriangleV& localTriangle, const PxU32 triangleIndex, const PxU32* triIndices, const PxU8 triFlags, const Gu::PolygonalData& polyData,  Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal);
+
+	bool generatePolyDataContactManifold(Gu::TriangleV& localTriangle, const PxU32 featureIndex, const PxU32 triangleIndex, const PxU8 triFlags, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal);
+	void generateLastContacts();
+	void addContactsToPatch(const Ps::aos::Vec3VArg patchNormal, const PxU32 previousNumContacts);
+
+	bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds); 
+
+	static bool generateTriangleFullContactManifold(Gu::TriangleV& localTriangle, const PxU32 triangleIndex, const PxU8 triFlags, const Gu::PolygonalData& polyData,  Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal, Cm::RenderOutput* renderOutput = NULL);
+
+	static bool processTriangle(const Gu::PolygonalData& polyData, SupportLocal* polyMap, const PxVec3* verts, const PxU32 triangleIndex, PxU8 triFlags, const Ps::aos::FloatVArg inflation, const bool isDoubleSided, 
+		const Ps::aos::PsTransformV& convexTransform, const Ps::aos::PsMatTransformV& meshToConvex, Gu::MeshPersistentContact* manifoldContact, PxU32& numContacts); 
+};
+
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+class PCMSphereVsMeshContactGeneration : public PCMMeshContactGeneration
+{
+public:
+	Ps::aos::Vec3V	mSphereCenter;
+	Ps::aos::FloatV mSphereRadius;
+	Ps::aos::FloatV	mSqInflatedSphereRadius;
+
+				
+	PCMSphereVsMeshContactGeneration(
+		const Ps::aos::Vec3VArg		sphereCenter,
+		const Ps::aos::FloatVArg	sphereRadius,
+		const Ps::aos::FloatVArg	contactDist,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+		const Ps::aos::PsTransformV& sphereTransform,
+		const Ps::aos::PsTransformV& meshTransform,
+		Gu::MultiplePersistentContactManifold& multiManifold,
+		Gu::ContactBuffer&			contactBuffer,
+		Cm::RenderOutput*			renderOutput = NULL
+
+	) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer, renderOutput),
+		mSphereCenter(sphereCenter),
+		mSphereRadius(sphereRadius)
+	{
+		using namespace Ps::aos;
+		const FloatV inflatedSphereRadius = FAdd(sphereRadius, contactDist);
+		mSqInflatedSphereRadius = FMul(inflatedSphereRadius, inflatedSphereRadius);
+	}
+
+
+	bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
+};
+
+class PCMCapsuleVsMeshContactGeneration : public PCMMeshContactGeneration
+{
+	PCMCapsuleVsMeshContactGeneration &operator=(PCMCapsuleVsMeshContactGeneration &);
+public:
+	Ps::aos::FloatV mInflatedRadius;
+	Ps::aos::FloatV	mSqInflatedRadius;
+	const CapsuleV&	mCapsule;
+
+				
+	PCMCapsuleVsMeshContactGeneration( 
+		const CapsuleV&				capsule,
+		const Ps::aos::FloatVArg	contactDist,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+		const Ps::aos::PsTransformV& sphereTransform,
+		const Ps::aos::PsTransformV& meshTransform,
+		Gu::MultiplePersistentContactManifold& multiManifold,
+		Gu::ContactBuffer& contactBuffer,
+		Cm::RenderOutput*			renderOutput = NULL
+
+	) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer, renderOutput),
+		mCapsule(capsule)
+	{
+		using namespace Ps::aos;
+		mInflatedRadius = FAdd(capsule.radius, contactDist);
+		mSqInflatedRadius = FMul(mInflatedRadius, mInflatedRadius);
+	}
+
+	void generateEEContacts(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex, 
+		const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg sqInflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+
+	void generateEE(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q,  const Ps::aos::FloatVArg sqInflatedRadius, const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex,
+		const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+	
+	static bool generateContacts(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg planeNormal, const Ps::aos::Vec3VArg normal,  
+		const PxU32 triangleIndex, const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+
+	static void generateEEContactsMTD(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b,const Ps::aos::Vec3VArg c, const Ps::aos::Vec3VArg normal, const PxU32 triangleIndex,
+		const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q, const Ps::aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+
+	static void generateEEMTD(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg q,  const Ps::aos::FloatVArg inflatedRadius, const Ps::aos::Vec3VArg normal, const PxU32 trianlgeIndex, 
+		const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+
+	bool processTriangle(const PxVec3* verts, const PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
+
+	static bool processTriangle(const TriangleV& triangle, const PxU32 triangleIndex, const CapsuleV& capsule, const Ps::aos::FloatVArg inflatedRadius, const PxU8 triFlag, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
+};
+
+}
+}
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
new file mode 100644
index 00000000..4d0fd351
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexConvex.cpp
@@ -0,0 +1,310 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+#include "GuVecConvexHull.h"
+#include "GuVecShrunkConvexHull.h"
+#include "GuVecShrunkConvexHullNoScale.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuGeometryUnion.h"   
+
+#include "GuContactMethodImpl.h"
+#include "GuPCMShapeConvex.h"
+#include "GuPCMContactGen.h"
+#include "GuContactBuffer.h"
+
+namespace physx
+{
+
+	using namespace Ps::aos;
+
+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)
+{
+	Gu::PolygonalData polyData0, polyData1;
+	getPCMConvexData(convexHull0, idtScale0, polyData0);
+	getPCMConvexData(convexHull1, idtScale1, polyData1);
+
+	PxU8 buff0[sizeof(SupportLocalImpl<ConvexHullV>)];
+	PxU8 buff1[sizeof(SupportLocalImpl<ConvexHullV>)];
+
+	SupportLocal* map0 = (idtScale0 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull0), transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)) : 
+		static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff0, SupportLocalImpl<ConvexHullV>)(convexHull0, transf0, convexHull0.vertex2Shape, convexHull0.shape2Vertex, idtScale0)));
+
+	SupportLocal* map1 = (idtScale1 ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull1), transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)) : 
+		static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff1, SupportLocalImpl<ConvexHullV>)(convexHull1, transf1, convexHull1.vertex2Shape, convexHull1.shape2Vertex, idtScale1)));
+
+
+	PxU32 numContacts = 0;
+
+	if(generateFullContactManifold(polyData0, polyData1, map0, map1, manifoldContacts, numContacts, contactDist, normal, closestA, closestB, convexHull0.getMargin(), 
+		convexHull1.getMargin(), doOverlapTest, renderOutput, toleranceScale))
+	{
+		
+		if (numContacts > 0)
+		{
+			//reduce contacts
+			manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+
+			const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+
+			//add the manifold contacts;
+			manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+
+
+#if	PCM_LOW_LEVEL_DEBUG
+			manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		}
+		else
+		{
+			//if doOverlapTest is true, which means GJK/EPA degenerate so we won't have any contact in the manifoldContacts array
+			if (!doOverlapTest)
+			{
+				const Vec3V worldNormal = manifold.getWorldNormal(transf1);
+
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+			}
+
+		}
+		return true;
+		
+	}
+
+	return false;
+
+}
+
+static GjkStatus convexHullNoScale0(const ShrunkConvexHullV& convexHull0, const ShrunkConvexHullV& convexHull1, const bool idtScale1, const PsMatTransformV& aToB, const FloatVArg contactDist,
+									 Vec3V& closestA, Vec3V& closestB, Vec3V& normal, FloatV& penDep, PersistentContactManifold& manifold)
+{
+	const RelativeConvex<ShrunkConvexHullNoScaleV> convexA(static_cast<const ShrunkConvexHullNoScaleV&>(convexHull0), aToB);
+	if(idtScale1)
+	{
+		const LocalConvex<ShrunkConvexHullNoScaleV> convexB(static_cast<const ShrunkConvexHullNoScaleV&>(convexHull1));
+		return gjkPenetration<RelativeConvex<ShrunkConvexHullNoScaleV>, LocalConvex<ShrunkConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+						manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
+	}
+	else
+	{
+		const LocalConvex<ShrunkConvexHullV> convexB(convexHull1);
+		return gjkPenetration<RelativeConvex<ShrunkConvexHullNoScaleV>, LocalConvex<ShrunkConvexHullV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,false);
+	}
+}
+
+static GjkStatus convexHullHasScale0(ShrunkConvexHullV& convexHull0, ShrunkConvexHullV& convexHull1, const bool idtScale1, const PsMatTransformV& aToB, 
+									   const FloatVArg contactDist, Vec3V& closestA, Vec3V& closestB, Vec3V& normal, FloatV& penDep, PersistentContactManifold& manifold)
+{
+	RelativeConvex<ShrunkConvexHullV> convexA(convexHull0, aToB);
+	if(idtScale1)
+	{
+		LocalConvex<ShrunkConvexHullNoScaleV> convexB(static_cast<ShrunkConvexHullNoScaleV&>(convexHull1));
+		return gjkPenetration< RelativeConvex<ShrunkConvexHullV>, LocalConvex<ShrunkConvexHullNoScaleV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+						manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,false);
+	}
+	else
+	{
+		LocalConvex<ShrunkConvexHullV> convexB(convexHull1);
+		return gjkPenetration<RelativeConvex<ShrunkConvexHullV>, LocalConvex<ShrunkConvexHullV> >(convexA, convexB, aToB.p, contactDist, closestA, closestB, normal, penDep,
+					manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, false);
+	}
+}
+
+
+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;
+	if (status == GJK_CONTACT)
+	{
+		const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+		const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+
+		//Add contact to contact stream
+		manifoldContacts[0].mLocalPointA = localPointA;
+		manifoldContacts[0].mLocalPointB = closestB;
+		manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+		//Add contact to manifold
+		manifold.addManifoldPoint(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+	}
+	else
+	{
+		PX_ASSERT(status == EPA_CONTACT);
+
+		RelativeConvex<ConvexHullV> convexA1(convexHull0, aToB);
+		LocalConvex<ConvexHullV> convexB1(convexHull1);
+
+		status = epaPenetration(convexA1, convexB1, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+			closestA, closestB, normal, penDep);
+
+		if (status == EPA_CONTACT)
+		{
+			const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
+			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+
+			//Add contact to contact stream
+			manifoldContacts[0].mLocalPointA = localPointA;
+			manifoldContacts[0].mLocalPointB = closestB;
+			manifoldContacts[0].mLocalNormalPen = localNormalPen;
+
+			//Add contact to manifold
+			manifold.addManifoldPoint(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
+		}
+		else
+		{
+			doOverlapTest = true;
+		}
+	}
+
+	return doOverlapTest;
+}
+
+bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS)
+{
+	const PxConvexMeshGeometryLL& shapeConvex0 = shape0.get<const PxConvexMeshGeometryLL>();
+	const PxConvexMeshGeometryLL& shapeConvex1 = shape1.get<const PxConvexMeshGeometryLL>();
+	PersistentContactManifold& manifold = cache.getManifold();
+
+	Ps::prefetchLine(shapeConvex0.hullData);
+	Ps::prefetchLine(shapeConvex1.hullData);
+
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+
+	const Vec3V vScale0 = V3LoadU_SafeReadW(shapeConvex0.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const Vec3V vScale1 = V3LoadU_SafeReadW(shapeConvex1.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	//Transfer A into the local space of B
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+	const PsTransformV curRTrans(transf1.transformInv(transf0));
+	const PsMatTransformV aToB(curRTrans);
+	
+	const Gu::ConvexHullData* hullData0 = shapeConvex0.hullData;
+	const Gu::ConvexHullData* hullData1 = shapeConvex1.hullData;
+
+	const FloatV convexMargin0 = Gu::CalculatePCMConvexMargin(hullData0, vScale0);
+	const FloatV convexMargin1 = Gu::CalculatePCMConvexMargin(hullData1, vScale1);
+	
+	const PxU32 initialContacts = manifold.mNumContacts;
+	
+	const FloatV minMargin = FMin(convexMargin0, convexMargin1);
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
+	
+	manifold.refreshContactPoints(aToB, projectBreakingThreshold, contactDist);
+
+	//ML: after refreshContactPoints, we might lose some contacts
+	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
+
+	PX_UNUSED(bLostContacts);
+	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 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);
+
+		Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
+		FloatV penDep =  FZero();
+
+		if(idtScale0)
+		{
+			status = convexHullNoScale0(convexHull0, convexHull1, idtScale1, aToB, contactDist, closestA, closestB, normal, penDep, manifold);
+		}
+		else
+		{
+			status = convexHullHasScale0(convexHull0, convexHull1, idtScale1, aToB, contactDist, closestA, closestB, normal, penDep, manifold);
+		}
+
+		manifold.setRelativeTransform(curRTrans);
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		
+		if(status == GJK_DEGENERATE)
+		{
+			return fullContactsGenerationConvexConvex(convexHull0, convexHull1, transf0, transf1, idtScale0, idtScale1, manifoldContacts, contactBuffer, 
+				manifold, normal, closestA, closestB, contactDist, true, renderOutput, FLoad(params.mToleranceLength));
+		}
+		else if(status == GJK_NON_INTERSECT)
+		{
+			return false;
+		}
+		else
+		{
+			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+			
+			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)
+			{
+				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);
+				manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+				return true;
+			}
+			
+		}
+	}
+	else if(manifold.getNumContacts()> 0)
+	{
+		const Vec3V worldNormal =  manifold.getWorldNormal(transf1);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, worldNormal, transf1, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return true;
+	}
+
+	return false;
+	
+}
+
+}
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
new file mode 100644
index 00000000..029a60d9
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexHeightField.cpp
@@ -0,0 +1,274 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuVecBox.h"
+#include "GuVecShrunkBox.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuVecShrunkConvexHullNoScale.h"
+#include "GuVecTriangle.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuPCMShapeConvex.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuHeightField.h"
+#include "GuHeightFieldUtil.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuPCMContactMeshCallback.h"
+
+#include "PsVecMath.h"
+
+
+using namespace physx;
+using namespace Gu;
+using namespace physx::shdfnd::aos;
+
+namespace physx
+{
+
+struct PCMConvexVsHeightfieldContactGenerationCallback
+	: PCMHeightfieldContactGenerationCallback< PCMConvexVsHeightfieldContactGenerationCallback >
+{
+	PCMConvexVsHeightfieldContactGenerationCallback& operator=(const PCMConvexVsHeightfieldContactGenerationCallback&);
+public:
+	PCMConvexVsMeshContactGeneration		mGeneration;
+
+	PCMConvexVsHeightfieldContactGenerationCallback(
+		const Ps::aos::FloatVArg				contactDistance,
+		const Ps::aos::FloatVArg				replaceBreakingThreshold,
+		const Gu::PolygonalData&				polyData,
+		SupportLocal*							polyMap,
+		const Cm::FastVertex2ShapeScaling&		convexScaling,
+		bool									idtConvexScale,
+		const PsTransformV&						convexTransform, 
+		const PsTransformV&						heightfieldTransform,
+		const PxTransform&						heightfieldTransform1,
+		Gu::MultiplePersistentContactManifold&	multiManifold,
+		Gu::ContactBuffer&						contactBuffer,
+		Gu::HeightFieldUtil&					hfUtil,
+		Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE>&	delayedContacts,
+		Cm::RenderOutput*						renderOutput = NULL
+		
+	) :
+		PCMHeightfieldContactGenerationCallback< PCMConvexVsHeightfieldContactGenerationCallback >(hfUtil, heightfieldTransform1),
+		mGeneration(contactDistance, replaceBreakingThreshold, convexTransform, heightfieldTransform,  multiManifold,
+			contactBuffer, polyData, polyMap, delayedContacts, convexScaling, idtConvexScale, renderOutput)
+	{
+	}
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(Gu::TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMConvexVsMeshContactGeneration>(cache);
+	}
+	
+};
+
+bool Gu::PCMContactConvexHeightfield(
+	const Gu::PolygonalData& polyData, Gu::SupportLocal* polyMap, const Ps::aos::FloatVArg minMargin,
+	const PxBounds3& hullAABB, const PxHeightFieldGeometry& shapeHeightfield,
+	const PxTransform& transform0, const PxTransform& transform1,
+	PxReal contactDistance, Gu::ContactBuffer& contactBuffer,
+	const Cm::FastVertex2ShapeScaling& convexScaling, bool idtConvexScale,
+	Gu::MultiplePersistentContactManifold& multiManifold, Cm::RenderOutput* renderOutput)
+
+{
+
+	using namespace Ps::aos;
+	using namespace Gu;
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&transform0.p.x);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV contactDist = FLoad(contactDistance);
+	//Transfer A into the local space of B
+	const PsTransformV convexTransform(p0, q0);//box
+	const PsTransformV heightfieldTransform(p1, q1);//heightfield  
+	const PsTransformV curTransform = heightfieldTransform.transformInv(convexTransform);
+	
+
+	if(multiManifold.invalidate(curTransform, minMargin))
+	{
+		const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform); 
+
+		////////////////////
+
+		const PxTransform t0to1 = transform1.transformInv(transform0);
+		
+		const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(shapeHeightfield.heightField);
+		Gu::HeightFieldUtil hfUtil(shapeHeightfield, hf);
+
+		//Gu::HeightFieldUtil hfUtil(shapeHeightfield);
+
+		////////////////////
+
+		/*const Cm::Matrix34 world0(transform0);
+		const Cm::Matrix34 world1(transform1);
+
+		const PxU8* PX_RESTRICT extraData = meshData->mExtraTrigData;*/
+
+	    Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE> delayedContacts;
+			
+		PCMConvexVsHeightfieldContactGenerationCallback blockCallback(
+			contactDist,
+			replaceBreakingThreshold,
+			polyData,
+			polyMap, 
+			convexScaling, 
+			idtConvexScale,
+			convexTransform, 
+			heightfieldTransform,
+			transform1,
+			multiManifold,
+			contactBuffer,
+			hfUtil,
+			delayedContacts,
+			renderOutput
+		);
+
+		hfUtil.overlapAABBTriangles(transform1, PxBounds3::transformFast(t0to1, hullAABB), 0, &blockCallback);
+
+		PX_ASSERT(multiManifold.mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
+		blockCallback.mGeneration.generateLastContacts();
+		blockCallback.mGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+
+		const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.6f));
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
+	}
+
+#if PCM_LOW_LEVEL_DEBUG
+	 multiManifold.drawManifold(*renderOutput, convexTransform, heightfieldTransform);
+#endif
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, heightfieldTransform);
+
+}
+
+
+bool Gu::pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	
+	const PxConvexMeshGeometryLL& shapeConvex = shape0.get<const PxConvexMeshGeometryLL>();
+	const physx::PxHeightFieldGeometryLL& shapHeightField = shape1.get<const PxHeightFieldGeometryLL>();
+
+	const Gu::ConvexHullData* hullData = shapeConvex.hullData;
+	Gu::MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&transform0.p.x);
+
+	const PsTransformV convexTransform(p0, q0);
+
+	//const bool idtScaleMesh = shapeMesh.scale.isIdentity();
+
+	//Cm::FastVertex2ShapeScaling meshScaling;
+	//if(!idtScaleMesh)
+	//	meshScaling.init(shapeMesh.scale);
+
+	Cm::FastVertex2ShapeScaling convexScaling;
+	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 = 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 QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
+	Gu::ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, shapeConvex.scale.isIdentity());
+
+	if(idtScaleConvex)
+	{
+		SupportLocalShrunkImpl<Gu::ConvexHullNoScaleV, Gu::ShrunkConvexHullNoScaleV> 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);
+		return Gu::PCMContactConvexHeightfield(polyData, &convexMap, minMargin, hullAABB, shapHeightField, transform0, transform1, params.mContactDistance, contactBuffer, convexScaling, 
+			idtScaleConvex, multiManifold, renderOutput);
+	}
+}  
+
+bool Gu::pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+
+	MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const PxBoxGeometry& shapeBox = shape0.get<const PxBoxGeometry>();
+	const physx::PxHeightFieldGeometryLL& shapHeightField = shape1.get<const PxHeightFieldGeometryLL>();
+
+	const PxVec3 ext = shapeBox.halfExtents + PxVec3(params.mContactDistance);
+	const PxBounds3 hullAABB(-ext, ext);
+
+	Cm::FastVertex2ShapeScaling idtScaling;
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	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);
+
+	Gu::BoxV boxV(V3Zero(), boxExtents);
+
+	const PsTransformV boxTransform(p0, q0);//box
+
+	Gu::PolygonalData polyData;
+	Gu::PCMPolygonalBox polyBox(shapeBox.halfExtents);
+	polyBox.getPolygonalData(&polyData);
+
+	Mat33V identity =  M33Identity();
+	//SupportLocalImpl<Gu::BoxV> boxMap(boxV, boxTransform, identity, identity);
+	SupportLocalShrunkImpl<Gu::BoxV, Gu::ShrunkBoxV> 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/GuPCMContactConvexMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp
new file mode 100644
index 00000000..d739b6dc
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactConvexMesh.cpp
@@ -0,0 +1,264 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecBox.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuVecTriangle.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuPCMShapeConvex.h"
+#include "GuConvexUtilsInternal.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuPCMContactMeshCallback.h"
+#include "GuIntersectionTriangleBox.h"
+#include "GuBox.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace physx::shdfnd::aos;
+
+namespace physx
+{
+
+struct PCMConvexVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback<PCMConvexVsMeshContactGenerationCallback>
+{
+	PCMConvexVsMeshContactGenerationCallback& operator=(const PCMConvexVsMeshContactGenerationCallback&);
+public:
+	PCMConvexVsMeshContactGeneration	mGeneration;
+	const BoxPadded&					mBox;
+
+	PCMConvexVsMeshContactGenerationCallback(
+		const Ps::aos::FloatVArg				contactDistance,
+		const Ps::aos::FloatVArg				replaceBreakingThreshold,
+		const PsTransformV&						convexTransform, 
+		const PsTransformV&						meshTransform,
+		MultiplePersistentContactManifold&		multiManifold,
+		ContactBuffer&							contactBuffer,
+		const PolygonalData&					polyData,
+		SupportLocal*							polyMap,
+		Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE>&	delayedContacts,
+		const Cm::FastVertex2ShapeScaling&		convexScaling,
+		bool									idtConvexScale,
+		const Cm::FastVertex2ShapeScaling&		meshScaling,
+		const PxU8*								extraTriData,
+		bool									idtMeshScale,
+		const BoxPadded&						box,
+		Cm::RenderOutput*						renderOutput = NULL
+		
+	) :
+		PCMMeshContactGenerationCallback<PCMConvexVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale),
+		mGeneration(contactDistance, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer, polyData, polyMap, delayedContacts, convexScaling, idtConvexScale, renderOutput),
+		mBox(box)
+	{
+	}
+
+	PX_FORCE_INLINE Ps::IntBool doTest(const PxVec3& v0, const PxVec3& v1, const PxVec3& v2)
+	{
+		// PT: this one is safe because midphase vertices are directly passed to the function
+		return intersectTriangleBox(mBox, v0, v1, v2);
+	}
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMConvexVsMeshContactGeneration>(cache);
+	}
+	
+};
+
+
+bool Gu::PCMContactConvexMesh(const PolygonalData& polyData, SupportLocal* polyMap, const Ps::aos::FloatVArg minMargin, const PxBounds3& hullAABB, const PxTriangleMeshGeometryLL& shapeMesh,
+						const PxTransform& transform0, const PxTransform& transform1,
+						PxReal contactDistance, ContactBuffer& contactBuffer,
+						const Cm::FastVertex2ShapeScaling& convexScaling, const Cm::FastVertex2ShapeScaling& meshScaling,
+						bool idtConvexScale, bool idtMeshScale, Gu::MultiplePersistentContactManifold& multiManifold,
+						Cm::RenderOutput* renderOutput)
+
+{
+	using namespace Ps::aos;
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&transform0.p.x);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV contactDist = FLoad(contactDistance);
+	//Transfer A into the local space of B
+	const PsTransformV convexTransform(p0, q0);//box
+	const PsTransformV meshTransform(p1, q1);//triangleMesh  
+	const PsTransformV curTransform = meshTransform.transformInv(convexTransform);
+	
+	
+	if(multiManifold.invalidate(curTransform, minMargin))
+	{
+		const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform); 
+
+	
+		////////////////////
+		const TriangleMesh* PX_RESTRICT meshData = shapeMesh.meshData;
+
+		const Cm::Matrix34 world0(transform0);
+		const Cm::Matrix34 world1(transform1);
+		BoxPadded hullOBB;
+		computeHullOBB(hullOBB, hullAABB, contactDistance, world0, world1, meshScaling, idtMeshScale);
+
+		// Setup the collider
+
+		Ps::InlineArray<PxU32,LOCAL_CONTACTS_SIZE> delayedContacts;
+		
+		const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
+		PCMConvexVsMeshContactGenerationCallback blockCallback(
+			contactDist, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer,
+			polyData, polyMap, delayedContacts, convexScaling, idtConvexScale, meshScaling, extraData, idtMeshScale, 
+			hullOBB, renderOutput);
+
+		Midphase::intersectOBB(meshData, hullOBB, blockCallback, true);
+
+		PX_ASSERT(multiManifold.mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
+
+		blockCallback.flushCache();
+		//This is very important
+		blockCallback.mGeneration.generateLastContacts();
+		blockCallback.mGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);
+
+#if PCM_LOW_LEVEL_DEBUG
+		multiManifold.drawManifold(*renderOutput, transform0, transform1);
+#endif
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+		const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.8f));
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, contactDist);
+	}
+
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, meshTransform);
+}
+
+bool Gu::pcmContactConvexMesh(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	PX_UNUSED(renderOutput);
+
+	const PxConvexMeshGeometryLL& shapeConvex = shape0.get<const PxConvexMeshGeometryLL>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	const ConvexHullData* hullData = shapeConvex.hullData;
+	MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const PsTransformV convexTransform = loadTransformA(transform0);
+
+	const bool idtScaleMesh = shapeMesh.scale.isIdentity();
+
+	Cm::FastVertex2ShapeScaling meshScaling;
+	if(!idtScaleMesh)
+		meshScaling.init(shapeMesh.scale);
+
+	Cm::FastVertex2ShapeScaling convexScaling;
+	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);
+	
+	ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtScaleConvex);
+
+
+	if(idtScaleConvex)
+	{
+		SupportLocalImpl<Gu::ConvexHullNoScaleV> convexMap(static_cast<ConvexHullNoScaleV&>(convexHull), convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, true);
+		return Gu::PCMContactConvexMesh(polyData, &convexMap, minMargin, hullAABB, shapeMesh,transform0,transform1, params.mContactDistance, contactBuffer, convexScaling,  
+			meshScaling, idtScaleConvex, idtScaleMesh, multiManifold, renderOutput);
+	}
+	else
+	{
+		SupportLocalImpl<Gu::ConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, false);
+		return Gu::PCMContactConvexMesh(polyData, &convexMap, minMargin, hullAABB, shapeMesh,transform0,transform1, params.mContactDistance, contactBuffer, convexScaling,  
+			meshScaling, idtScaleConvex, idtScaleMesh, multiManifold, renderOutput);
+	}
+}
+
+bool Gu::pcmContactBoxMesh(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	PX_UNUSED(renderOutput);
+
+	MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const PxBoxGeometry& shapeBox = shape0.get<const PxBoxGeometry>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	const PxBounds3 hullAABB(-shapeBox.halfExtents, shapeBox.halfExtents);  
+
+	const bool idtMeshScale = shapeMesh.scale.isIdentity();
+
+	Cm::FastVertex2ShapeScaling meshScaling;
+	if(!idtMeshScale)
+		meshScaling.init(shapeMesh.scale);
+
+	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);
+
+	BoxV boxV(V3Zero(), boxExtents);
+
+	const PsTransformV boxTransform = loadTransformA(transform0);//box
+
+	PolygonalData polyData;
+	PCMPolygonalBox polyBox(shapeBox.halfExtents);
+	polyBox.getPolygonalData(&polyData);
+
+	Mat33V identity =  M33Identity();
+	SupportLocalImpl<BoxV> boxMap(boxV, boxTransform, identity, identity, true);
+
+	return Gu::PCMContactConvexMesh(polyData, &boxMap, minMargin, hullAABB, shapeMesh,transform0,transform1, params.mContactDistance, contactBuffer, idtScaling,  meshScaling, 
+		true, idtMeshScale, multiManifold, renderOutput);
+}
+
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h
new file mode 100644
index 00000000..6747b49b
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGen.h
@@ -0,0 +1,74 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_CONTACT_GEN_H
+#define GU_PCM_CONTACT_GEN_H
+
+
+#include "GuConvexSupportTable.h"
+#include "GuPersistentContactManifold.h"
+#include "GuShapeConvex.h"
+#include "GuSeparatingAxes.h"
+
+namespace physx
+{
+
+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);
+
+	//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);
+
+	//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);
+
+	//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);
+	
+	//MTD code for capsule vs box/convexhull
+	bool computeMTD(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, Ps::aos::FloatV& penDepth, Ps::aos::Vec3V& normal);
+
+	void buildPartialHull(const Gu::PolygonalData& polyData, SupportLocal* map, Gu::SeparatingAxes& validAxes, const Ps::aos::Vec3VArg v, const Ps::aos::Vec3VArg _dir);
+
+	//full contact gen code for sphere vs convexhull
+	bool generateSphereFullContactManifold(const Gu::CapsuleV& capsule, Gu::PolygonalData& polyData, Gu::SupportLocal* map, Gu::PersistentContact* manifoldContacts, PxU32& numContacts,
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& normal, const bool doOverlapTest);
+
+}  
+}
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
new file mode 100644
index 00000000..68354425
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
@@ -0,0 +1,725 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGeometryUnion.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+#include "CmRenderOutput.h"
+#include "GuPCMContactGenUtil.h"
+#include "PsVecMath.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+
+#define PCM_USE_INTERNAL_OBJECT 1
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+
+	//Precompute the convex data
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+namespace physx
+{
+
+namespace Gu
+{
+	
+	static bool testFaceNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, const PsMatTransformV& transform1To0, const FloatVArg contactDist, 
+		FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, const FeatureStatus faceStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(polyData1);
+		
+		FloatV _minOverlap = FMax();//minOverlap;
+		PxU32  _feature = 0;
+		Vec3V  _faceNormal = faceNormal;
+		FloatV min0, max0;
+		FloatV min1, max1;
+	
+		const Vec3V center1To0 = transform1To0.p;
+		
+#if PCM_USE_INTERNAL_OBJECT
+		const Vec3V zeroV = V3Zero();
+		const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
+		const Vec3V internalCenter1In0 = transform1To0.transform(shapeSpaceCenter1);
+		const FloatV internalRadius1 = FLoad(polyData1.mInternal.mRadius);
+		const Vec3V internalExtents1 = V3LoadU(polyData1.mInternal.mExtents);
+		const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
+
+#endif
+
+		//in the local space of polyData0
+		for(PxU32 i=0; i<polyData0.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData0.mPolygons[i];
+
+			const Vec3V minVert = V3LoadU(polyData0.mVerts[polygon.mMinIndex]);
+			const FloatV planeDist = FLoad(polygon.mPlane.d);
+			const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);
+
+			//transform plane n to shape space
+			const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map0->shape2Vertex, vertexSpacePlaneNormal);
+
+			const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
+			//ML::use this to avoid LHS
+			min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
+			max0 = FMul(FNeg(planeDist), magnitude);
+
+			//normalize shape space normal
+			const Vec3V n0 = V3Scale(shapeSpacePlaneNormal, magnitude);
+
+			//calculate polyData1 projection
+			//rotate polygon's normal into the local space of polyData1
+			const Vec3V n1 = transform0To1.rotate(n0);
+
+
+#if PCM_USE_INTERNAL_OBJECT
+			
+			//test internal object	
+			//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
+			//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
+			//If polyData0 have scale, internalExtens1 will be 0.
+			const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
+			const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
+			const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
+
+			const FloatV _min1 = FSub(internalTrans, radius);
+			const FloatV _max1 = FAdd(internalTrans, radius);
+
+			const FloatV _min = FMax(min0, _min1);
+			const FloatV _max = FMin(max0, _max1);
+
+			const FloatV _tempOverlap = FSub(_max, _min);
+			//const FloatV _tempOverlap = FSub(max0, _min1);	
+
+			//Internal object overlaps more than current min, so can skip it
+			//because (a) it isn't a separating axis and (b) it isn't the smallest axis
+			if(FAllGrtr(_tempOverlap, _minOverlap))
+			{
+				continue;
+			}
+
+#endif
+
+			const FloatV translate = V3Dot(center1To0, n0);
+			map1->doSupport(n1, min1, max1);
+
+			min1 = FAdd(translate, min1);
+			max1 = FAdd(translate, max1);
+
+			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+			if(BAllEqTTTT(con))
+				return false;
+
+			const FloatV tempOverlap = FSub(max0, min1);
+
+			if(FAllGrtr(_minOverlap, tempOverlap))
+			{
+				_minOverlap = tempOverlap;
+				_feature = i;
+				_faceNormal = n0;
+			}
+		}   
+
+		if(FAllGrtr(minOverlap, _minOverlap))
+		{
+			faceNormal = _faceNormal; 
+			minOverlap = _minOverlap;
+			status = faceStatus;
+		}
+
+		
+
+		feature = _feature;
+
+		return true;
+
+	}
+
+
+	//plane is in the shape space of polyData
+	void buildPartialHull(const PolygonalData& polyData, SupportLocal* map, SeparatingAxes& validAxes, const Vec3VArg planeP, const Vec3VArg planeDir)
+	{		
+		const FloatV zero = FZero();
+		const Vec3V dir = V3Normalize(planeDir);
+		for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+			const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
+			
+			Vec3V v0 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[0]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+		
+			FloatV dist0 = V3Dot(dir, V3Sub(v0, planeP));
+
+			for (PxU32 iStart = 0, iEnd = PxU32(polygon.mNbVerts - 1); iStart < polygon.mNbVerts; iEnd = iStart++)
+			{
+				const Vec3V v1 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[iEnd]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				
+				const FloatV dist1 = V3Dot(dir, V3Sub(v1, planeP));
+
+				const BoolV con = BOr(FIsGrtr(dist0, zero), FIsGrtr(dist1, zero));
+
+				//cull edge if either of the vertex will on the positive size of the plane
+				if(BAllEqTTTT(con))
+				{
+					const Vec3V tempV = V3Sub(v0, v1); 
+					PxVec3 temp;
+					V3StoreU(tempV, temp);
+					validAxes.addAxis(temp.getNormalized());
+				}
+
+				v0 = v1;
+				dist0 = dist1;
+			}
+		}
+	}
+
+
+	static bool testEdgeNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, const PsMatTransformV& transform1To0, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& edgeNormalIn0, const FeatureStatus edgeStatus, FeatureStatus& status)
+	{
+		
+		FloatV overlap = minOverlap;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		const FloatV eps = FEps();
+
+		const Vec3V shapeSpaceCenter0 = V3LoadU(polyData0.mCenter);
+		const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
+
+#if PCM_USE_INTERNAL_OBJECT
+		const Vec3V zeroV = V3Zero();
+		const Vec3V internalCenter1In0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
+		const FloatV internalRadius1 = FLoad(polyData1.mInternal.mRadius);
+		const Vec3V internalExtents1 = V3LoadU(polyData1.mInternal.mExtents);
+		const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
+
+		const FloatV internalRadius0 = FLoad(polyData0.mInternal.mRadius);
+		const Vec3V internalExtents0 = V3LoadU(polyData0.mInternal.mExtents);
+		const Vec3V negInternalExtents0 = V3Neg(internalExtents0);
+#endif
+
+		const Vec3V center1To0 = transform1To0.p;
+
+		//in polyData0 shape space
+		const Vec3V dir0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
+		const Vec3V support0 = map0->doSupport(dir0);
+
+		//in polyData1 shape space
+		const Vec3V dir1 = transform0To1.rotate(V3Neg(dir0));
+		const Vec3V support1 = map1->doSupport(dir1);
+		
+		const Vec3V support0In1 = transform0To1.transform(support0);
+		const Vec3V support1In0 = transform1To0.transform(support1);
+
+		SeparatingAxes mSA0;
+		SeparatingAxes mSA1;
+		mSA0.reset();
+		mSA1.reset();
+		
+		buildPartialHull(polyData0, map0, mSA0, support1In0, dir0);
+		buildPartialHull(polyData1, map1, mSA1, support0In1, dir1);
+
+		const PxVec3* axe0 = mSA0.getAxes();
+		const PxVec3* axe1 = mSA1.getAxes();
+		const PxU32 numAxe0 = mSA0.getNumAxes();
+		const PxU32 numAxe1 = mSA1.getNumAxes();
+		for(PxU32 i=0; i < numAxe0; ++i)
+		{
+			//axe0[i] is in the shape space of polyData0
+			const Vec3V v0 = V3LoadU(axe0[i]);
+
+			for(PxU32 j=0; j< numAxe1; ++j)
+			{
+				//axe1[j] is in the shape space of polyData1
+				const Vec3V v1 = V3LoadU(axe1[j]);
+
+				const Vec3V dir = V3Cross(v0, transform1To0.rotate(v1));
+				const FloatV lenSq = V3Dot(dir, dir);
+				if(FAllGrtr(eps, lenSq))
+					continue;
+
+				//n0 is in polyData0's local space
+				const Vec3V n0 = V3Scale(dir, FRsqrt(lenSq));
+				
+				//n1 is in polyData1's local space
+				const Vec3V n1 = transform0To1.rotate(n0);
+
+#if PCM_USE_INTERNAL_OBJECT
+
+				//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
+				//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
+				//If polyData0 have scale, internalExtens1 will be 0.
+				//vertex space n1
+				const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
+				const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
+
+				const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
+
+				const FloatV _min1 = FSub(internalTrans, radius);
+				const FloatV _max1 = FAdd(internalTrans, radius);
+
+				const Vec3V proj0 = V3Sel(V3IsGrtr(n0, zeroV), internalExtents0, negInternalExtents0);
+				const FloatV radius0 = FMax(V3Dot(n0, proj0), internalRadius0);
+
+				const FloatV _max0 = radius0;
+				const FloatV _min0 = FNeg(radius0);
+
+				PX_ASSERT(FAllGrtrOrEq(_max0, _min0));
+				PX_ASSERT(FAllGrtrOrEq(_max1, _min1));
+
+
+				const FloatV _min = FMax(_min0, _min1);
+				const FloatV _max = FMin(_max0, _max1);
+
+				const FloatV _tempOverlap = FSub(_max, _min);
+
+				//Internal object overlaps more than current min, so can skip it
+				//because (a) it isn't a separating axis and (b) it isn't the smallest axis
+				if(FAllGrtr(_tempOverlap, overlap))
+				{
+					continue;
+				}
+
+#endif
+				//get polyData0's projection
+				map0->doSupport(n0, min0, max0);
+
+				const FloatV translate = V3Dot(center1To0, n0);
+
+				//get polyData1's projection
+				map1->doSupport(n1, min1, max1);
+
+				min1 = FAdd(translate, min1);
+				max1 = FAdd(translate, max1);
+
+				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+				if(BAllEqTTTT(con))
+					return false;
+
+				const FloatV tempOverlap = FSub(max0, min1);
+
+#if PCM_USE_INTERNAL_OBJECT
+				PX_ASSERT(FAllGrtrOrEq(tempOverlap, _tempOverlap));
+#endif
+
+				if(FAllGrtr(overlap, tempOverlap))
+				{
+					overlap = tempOverlap;
+					edgeNormalIn0 = n0;
+					status = edgeStatus;
+				}
+			}
+		}
+
+		minOverlap = overlap;
+		
+		return true;
+
+	}
+
+
+	//contactNormal is in the space of polyData0
+	void generatedContacts(PolygonalData& polyData0, PolygonalData& polyData1, const HullPolygonData& referencePolygon, const HullPolygonData& incidentPolygon,  
+		SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, PersistentContact* manifoldContacts, 
+		PxU32& numContacts, const FloatVArg contactDist, Cm::RenderOutput* renderOutput)
+	{
+
+		PX_UNUSED(renderOutput);
+		const FloatV zero = FZero();
+
+		const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;
+
+		//transform the plane normal to shape space
+		const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
+		
+		//this is the matrix transform all points to the 2d plane
+		const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);
+
+		const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;
+
+
+		Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
+		Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
+		bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));
+		FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));
+
+		//Transform all the verts from vertex space to shape space
+		map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);
+		map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);
+
+#if PCM_LOW_LEVEL_DEBUG
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, (PxU32)referencePolygon.mNbVerts, 0x00ff0000); 
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, (PxU32)incidentPolygon.mNbVerts,  0x0000ff00); 
+#endif
+	
+		//This is used to calculate the project point when the 2D reference face points is inside the 2D incident face point
+		const Vec3V sPoint = points1In0[0]; 
+	
+		PX_ASSERT(incidentPolygon.mNbVerts <= 64);
+ 
+		Vec3V eps = Vec3V_From_FloatV(FEps());
+		Vec3V max = Vec3V_From_FloatV(FMax());
+		Vec3V nmax = V3Neg(max); 
+
+		//transform reference polygon to 2d, calculate min and max
+		Vec3V rPolygonMin= max;
+		Vec3V rPolygonMax = nmax;
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			points0In0[i] = M33MulV3(rot, points0In0[i]);
+			rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
+			rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
+		}
+		
+		rPolygonMin = V3Sub(rPolygonMin, eps);
+		rPolygonMax = V3Add(rPolygonMax, eps);
+
+		const FloatV d = V3GetZ(points0In0[0]);
+		const FloatV rd = FAdd(d, contactDist);
+
+		Vec3V iPolygonMin= max; 
+		Vec3V iPolygonMax = nmax;
+
+
+		PxU32 inside = 0;
+		for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
+		{
+			const Vec3V vert1 =points1In0[i]; //this still in polyData1's local space
+			const Vec3V a = transform0To1.transformInv(vert1);
+			points1In0[i] = M33MulV3(rot, a);
+			const FloatV z = V3GetZ(points1In0[i]);
+			points1In0TValue[i] = FSub(z, d);
+			points1In0[i] = V3SetZ(points1In0[i], d);
+			iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
+			iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
+			if(FAllGrtr(rd, z))
+			{
+				points1In0Penetration[i] = true;
+
+				if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
+				{
+					inside++;
+
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), points1In0TValue[i]);
+					manifoldContacts[numContacts].mLocalPointA = vert1;
+					manifoldContacts[numContacts].mLocalPointB = M33TrnspsMulV3(rot, points1In0[i]);
+					manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+					
+				}
+			}
+			else
+			{
+				points1In0Penetration[i] = false;
+			}
+			
+		}
+
+
+		if(inside == incidentPolygon.mNbVerts)
+		{
+			return;
+		}
+
+		inside = 0;
+		iPolygonMin = V3Sub(iPolygonMin, eps);
+		iPolygonMax = V3Add(iPolygonMax, eps);
+
+		const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
+	
+		const Vec3V contactNormalIn1 = transform0To1.rotate(contactNormal);
+
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
+			{
+				//const Vec3V vert0=Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]);
+				const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
+				const Vec3V a = transform0To1.transform(vert0);
+
+				const FloatV nom = V3Dot(incidentNormal, V3Sub(sPoint, a)); 
+				const FloatV denom = V3Dot(incidentNormal, contactNormalIn1);
+				PX_ASSERT(FAllEq(denom, zero)==0);
+				const FloatV t = FDiv(nom, denom);
+
+				if(FAllGrtr(t, contactDist))
+					continue;
+
+
+				inside++;
+
+				const Vec3V projPoint = V3ScaleAdd(contactNormalIn1, t, a);
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t);
+		
+				manifoldContacts[numContacts].mLocalPointA = projPoint;
+				manifoldContacts[numContacts].mLocalPointB = vert0;
+				manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+
+			}
+		}
+
+		if(inside == referencePolygon.mNbVerts)
+			return;
+
+
+		//(2) segment intesection
+		for (PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
+		{
+			if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
+				continue;
+
+	
+			const Vec3V ipA = points1In0[iStart];
+			const Vec3V ipB = points1In0[iEnd];
+
+			Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
+			Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));
+
+			const Vec3V iMin = V3Min(ipA, ipB);
+			const Vec3V iMax = V3Max(ipA, ipB);
+			
+
+			for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
+			{
+	
+				const Vec3V rpA = points0In0[rStart];
+				const Vec3V rpB = points0In0[rEnd];
+
+				const Vec3V rMin = V3Min(rpA, rpB);
+				const Vec3V rMax = V3Max(rpA, rpB);
+				
+				const BoolV tempCon =BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
+				const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+		
+				if(BAllEqTTTT(con))
+					continue;
+			
+					
+				FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
+				FloatV a2 = signed2DTriArea(rpA, rpB, ipB);
+
+
+				if(FAllGrtr(zero, FMul(a1, a2)))
+				{
+					FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
+					FloatV a4 = signed2DTriArea(ipA, ipB, rpB);
+
+					if(FAllGrtr(zero, FMul(a3, a4)))
+					{
+					
+						//these two segment intersect
+
+						const FloatV t = FDiv(a1, FSub(a2, a1));
+
+						const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri); 
+						const Vec3V pAA = V3SetZ(pBB, d);
+						const Vec3V pA = M33TrnspsMulV3(rot, pAA);
+						const Vec3V pB = transform0To1.transform(M33TrnspsMulV3(rot, pBB));
+						const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
+						if(FAllGrtr(pen, contactDist))
+							continue;
+
+
+						const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
+						manifoldContacts[numContacts].mLocalPointA = pB;
+						manifoldContacts[numContacts].mLocalPointB = pA;
+						manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+					}
+				}
+			}
+
+		}
+	}
+
+
+	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 PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
+		const PsMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
+
+	
+		if(doOverlapTest)
+		{
+			//if gjk fail, SAT based yes/no test
+			FeatureStatus status = POLYDATA0;
+			FloatV minOverlap = FMax();
+			Vec3V minNormal = V3Zero();
+	
+			PxU32 feature0;
+			//in the local space of polyData0, minNormal is in polyData0 space
+			if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+				return false;
+			
+			PxU32 feature1;
+			//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
+			if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+				return false;
+
+		
+			bool doEdgeTest = false;
+			
+EdgeTest:
+			if(doEdgeTest)
+			{
+
+				if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
+					return false;
+
+				if(status != EDGE)
+					return true;
+			}
+
+			
+			if(status == POLYDATA0)
+			{
+				//minNormal is in the local space of polydata0
+		
+				const HullPolygonData& referencePolygon = polyData0.mPolygons[feature0];
+				const Vec3V n = transform0To1V.rotate(minNormal);
+				const HullPolygonData& incidentPolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, n)];
+				
+				generatedContacts(polyData0, polyData1, referencePolygon,  incidentPolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);
+				
+				if (numContacts > 0)
+				{
+					const Vec3V nn = V3Neg(n);
+					//flip the contacts
+					for(PxU32 i=0; i<numContacts; ++i)
+					{
+						const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
+						manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
+						manifoldContacts[i].mLocalPointA = localPointB;
+						manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
+					}
+				}
+			}
+			else if(status == POLYDATA1)
+			{
+				//minNormal is in the local space of polydata1
+				const HullPolygonData& referencePolygon = polyData1.mPolygons[feature1];
+				const HullPolygonData& incidentPolygon =  polyData0.mPolygons[getPolygonIndex(polyData0, map0, transform1To0V.rotate(minNormal))];
+				
+				//reference face is polyData1
+				generatedContacts(polyData1, polyData0, referencePolygon,  incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+
+			}
+			else //if(status == EDGE0)
+			{
+				//minNormal is in the local space of polydata0
+			
+				const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, V3Neg(minNormal))];
+				const HullPolygonData& referencePolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, transform0To1V.rotate(minNormal))];
+				generatedContacts(polyData1, polyData0, referencePolygon,  incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+			}
+
+			if(numContacts == 0 && !doEdgeTest)
+			{
+				doEdgeTest = true;
+				goto EdgeTest;
+			}
+
+		}
+		else
+		{
+			const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
+			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_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 HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
+			const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];
+			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+			
+		}
+	
+		return true;
+	}
+
+
+	bool computeMTD(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1,  SupportLocal* map0, SupportLocal* map1, Ps::aos::FloatV& penDepth, Ps::aos::Vec3V& normal)
+	{
+	
+		using namespace Ps::aos;
+
+		const PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
+		const PsMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
+	
+		FeatureStatus status = POLYDATA0;
+		FloatV minOverlap = FMax();
+		Vec3V minNormal = V3Zero();
+		const FloatV contactDist = FZero();
+
+		PxU32 feature0;
+		//in the local space of polyData0, minNormal is in polyData0 space
+		if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+			return false;
+		
+		PxU32 feature1;
+		//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
+		if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+			return false;
+	
+		if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
+			return false;
+
+		penDepth = minOverlap;
+		if(status == POLYDATA1)
+		{
+			//minNormal is in the local space of polydata1
+			normal = map1->transform.rotate(minNormal);
+		}
+		else
+		{
+			PX_ASSERT(status == POLYDATA0 || status == EDGE);
+			//ML: status == POLYDATA0 or status == EDGE, minNormal is in the local space of polydata0
+			normal = V3Neg(map0->transform.rotate(minNormal));
+		}
+
+		return true;
+	}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
new file mode 100644
index 00000000..2ed5aef0
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenSphereCapsule.cpp
@@ -0,0 +1,441 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGeometryUnion.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+#include "CmRenderOutput.h"
+#include "GuPCMContactGenUtil.h"
+#include "PsVecMath.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+
+	//Precompute the convex data
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+namespace physx
+{
+
+namespace Gu
+{
+
+	static bool testSATCapsulePoly(const CapsuleV& capsule, const PolygonalData& polyData, SupportLocal* map,  const FloatVArg contactDist, FloatV& minOverlap, Vec3V& separatingAxis)
+	{
+		using namespace Ps::aos;
+		FloatV _minOverlap = FMax();//minOverlap;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		Vec3V tempAxis = V3UnitY();
+		const FloatV eps = FEps();
+
+		//ML: project capsule to polydata axis 
+		if(!testPolyDataAxis(capsule, polyData, map, contactDist, _minOverlap, tempAxis))
+			return false;
+
+		const Vec3V capsuleAxis = V3Sub(capsule.p1, capsule.p0);
+	
+		for(PxU32 i=0;i<polyData.mNbPolygons;i++)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+			const PxU8* inds1 = polyData.mPolygonVertexRefs + polygon.mVRef8;
+
+			for(PxU32 lStart = 0, lEnd =PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = lStart++)
+			{
+
+				//in the vertex space
+				const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds1[lStart]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds1[lEnd]]);		// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				
+				const Vec3V vertexSpaceV = V3Sub(p11, p10);
+
+				//transform v to shape space
+				const Vec3V shapeSpaceV = M33TrnspsMulV3(map->shape2Vertex, vertexSpaceV);
+			
+				const Vec3V dir = V3Cross(capsuleAxis, shapeSpaceV);
+				const FloatV lenSq = V3Dot(dir, dir);
+				if(FAllGrtr(eps, lenSq))
+					continue;
+				const Vec3V normal = V3ScaleInv(dir, FSqrt(lenSq));
+
+				map->doSupport(normal, min0, max0);
+
+				const FloatV tempMin = V3Dot(capsule.p0, normal);
+				const FloatV tempMax = V3Dot(capsule.p1, normal);
+				min1 = FMin(tempMin, tempMax);
+				max1 = FMax(tempMin, tempMax);
+
+				min1 = FSub(min1, capsule.radius);
+				max1 = FAdd(max1, capsule.radius);
+
+				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+				if(BAllEqTTTT(con))
+					return false;
+
+			
+				const FloatV tempOverlap = FSub(max0, min1);
+
+				if(FAllGrtr(_minOverlap, tempOverlap))
+				{
+					_minOverlap = tempOverlap;
+					tempAxis = normal;
+				}
+			}
+		}
+		
+
+		separatingAxis = tempAxis;
+		minOverlap = _minOverlap;
+
+		return true;
+
+	}
+
+
+	void generatedCapsuleBoxFaceContacts(const CapsuleV& capsule,  PolygonalData& polyData, const HullPolygonData& referencePolygon, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts, 
+		const FloatVArg contactDist, const Vec3VArg normal)
+	{
+
+		const FloatV radius = FAdd(capsule.radius, contactDist);
+
+		//calculate the intersect point of ray to plane
+		const Vec3V planeNormal = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
+		const PxU8* inds = polyData.mPolygonVertexRefs + referencePolygon.mVRef8;
+		const Vec3V a = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[0]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+	
+		const FloatV denom0 = V3Dot(planeNormal, V3Sub(capsule.p0, a)); 
+		const FloatV denom1 = V3Dot(planeNormal, V3Sub(capsule.p1, a)); 
+		const FloatV projPlaneN = V3Dot(planeNormal, normal);
+		const FloatV numer = FSel(FIsGrtr(projPlaneN, FZero()), FRecip(projPlaneN), FZero());
+		const FloatV t0 = FMul(denom0, numer);
+		const FloatV t1 = FMul(denom1, numer);
+
+		const BoolV con0 = FIsGrtrOrEq(radius, t0);
+		const BoolV con1 = FIsGrtrOrEq(radius, t1);
+		if(BAllEqTTTT(BOr(con0, con1)))
+		{
+
+			const Mat33V rot = findRotationMatrixFromZAxis(planeNormal);
+			Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
+			map->populateVerts(inds, referencePolygon.mNbVerts, polyData.mVerts, points0In0);
+
+			Vec3V rPolygonMin = V3Splat(FMax());
+			Vec3V rPolygonMax = V3Neg(rPolygonMin);
+			for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+			{
+				points0In0[i] = M33MulV3(rot, points0In0[i]);
+				rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
+				rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
+			}
+
+			if(BAllEqTTTT(con0))
+			{
+				//add contact
+				const Vec3V proj = V3NegScaleSub(normal, t0, capsule.p0);//V3ScaleAdd(t0, normal, capsule.p0);
+				//transform proj0 to 2D
+				const Vec3V point = M33MulV3(rot, proj);
+
+				if(contains(points0In0, referencePolygon.mNbVerts, point, rPolygonMin, rPolygonMax))
+				{
+					manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p0);
+					manifoldContacts[numContacts].mLocalPointB = proj;
+					manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), t0);
+				}
+
+			}
+
+			if(BAllEqTTTT(con1))
+			{
+				const Vec3V proj = V3NegScaleSub(normal, t1, capsule.p1);//V3ScaleAdd(t1, normal, capsule.p1);
+				//transform proj0 to 2D
+				const Vec3V point = M33MulV3(rot, proj);
+
+				if(contains(points0In0, referencePolygon.mNbVerts, point, rPolygonMin, rPolygonMax))
+				{
+					manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p1);
+					manifoldContacts[numContacts].mLocalPointB = proj;
+					manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal),t1);
+				}
+			}
+		}
+
+	}
+
+
+	static void generatedFaceContacts(const CapsuleV& capsule,  PolygonalData& polyData, SupportLocal* map, const PsMatTransformV& aToB, PersistentContact* manifoldContacts, PxU32& numContacts, 
+		const FloatVArg contactDist, const Vec3VArg normal)
+	{
+		using namespace Ps::aos;
+
+		const FloatV zero = FZero();
+		FloatV tEnter=zero, tExit=zero;
+		const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
+		const Vec3V dir = V3Neg(normal);
+		const Vec3V vertexSpaceDir = M33MulV3(map->shape2Vertex, dir);
+		const Vec3V p0 = M33MulV3(map->shape2Vertex, capsule.p0);
+
+		if(intersectSegmentPolyhedron(p0, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
+		{
+			manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p0);
+			manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p0);
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
+		}
+
+		const Vec3V p1 = M33MulV3(map->shape2Vertex, capsule.p1);
+		if(intersectSegmentPolyhedron(p1, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
+		{
+			manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(capsule.p1);
+			manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p1);
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
+		}
+		
+	}
+
+
+	static void generateEE(const Vec3VArg p, const Vec3VArg q, const Vec3VArg normal, const Vec3VArg a, const Vec3VArg b, const PsMatTransformV& aToB, 
+		PersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg inflatedRadius)
+	{
+
+		const FloatV zero = FZero();
+		const FloatV expandedRatio = FLoad(0.005f);
+		const Vec3V ab = V3Sub(b, a);
+		const Vec3V n = V3Cross(ab, normal);
+		const FloatV d = V3Dot(n, a);
+		const FloatV np = V3Dot(n, p);
+		const FloatV nq = V3Dot(n,q);
+		const FloatV signP = FSub(np, d);
+		const FloatV signQ = FSub(nq, d);
+		const FloatV temp = FMul(signP, signQ);
+		if(FAllGrtr(temp, zero)) return;//If both points in the same side as the plane, no intersect points
+		
+		// if colliding edge (p3,p4) and plane are parallel return no collision
+		const Vec3V pq = V3Sub(q, p);
+		const FloatV npq= V3Dot(n, pq); 
+		if(FAllEq(npq, zero))	return;
+
+
+		//calculate the intersect point in the segment pq with plane n(x - a).
+		const FloatV segTValue = FDiv(FSub(d, np), npq);
+		const Vec3V localPointA = V3ScaleAdd(pq, segTValue, p);
+
+		//ML: ab, localPointA and normal is in the same plane, so that we can do 2D segment segment intersection
+		//calculate a normal perpendicular to ray localPointA + normal*t, then do 2D segment segment intersection
+		const Vec3V perNormal = V3Cross(normal, pq);
+		const Vec3V ap = V3Sub(localPointA, a);
+		const FloatV nom = V3Dot(perNormal, ap);
+		const FloatV denom = V3Dot(perNormal, ab);
+
+		const FloatV tValue = FDiv(nom, denom);
+
+		const FloatV max = FAdd(FOne(), expandedRatio);
+		const FloatV min = FSub(zero, expandedRatio);
+		if(FAllGrtr(tValue, max) || FAllGrtr(min, tValue))
+			return;
+
+		const Vec3V v = V3NegScaleSub(ab, tValue, ap);
+		const FloatV signedDist = V3Dot(v, normal);
+		if(FAllGrtrOrEq(inflatedRadius, signedDist))
+		{
+			const Vec3V localPointB = V3Sub(localPointA, v);
+			manifoldContacts[numContacts].mLocalPointA = aToB.transformInv(localPointA);
+			manifoldContacts[numContacts].mLocalPointB = localPointB;
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), signedDist);
+			
+		}
+	}
+
+
+	void generatedContactsEEContacts(const CapsuleV& capsule, PolygonalData& polyData, const HullPolygonData& referencePolygon, SupportLocal* map,  const PsMatTransformV& aToB, 
+		PersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg contactDist, const Vec3VArg contactNormal)
+	{
+
+		const PxU8* inds = polyData.mPolygonVertexRefs + referencePolygon.mVRef8;
+
+		Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
+
+
+		//Transform all the verts from vertex space to shape space
+		map->populateVerts(inds, referencePolygon.mNbVerts, polyData.mVerts, points0In0);
+
+		const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
+
+		for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
+		{
+			generateEE(capsule.p0, capsule.p1, contactNormal,points0In0[rStart], points0In0[rEnd], aToB,  manifoldContacts, numContacts, inflatedRadius);
+		
+		}
+	}
+
+
+	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 PxU32 originalContacts = numContacts;
+
+		const Gu::HullPolygonData* referencePolygon = NULL;
+
+		if(doOverlapTest)
+		{
+			Ps::aos::FloatV minOverlap;
+			//overwrite the normal
+			if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, normal))
+				return false;
+
+			referencePolygon = &polyData.mPolygons[getPolygonIndex(polyData, map, V3Neg(normal))];
+		}
+		else
+		{
+			const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
+			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_EPS));
+			const FloatV tolerance = FMax(FMul(margin, eps), lowerEps);
+
+			referencePolygon = &polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(normal), closest, tolerance)];
+			
+		}
+
+		generatedCapsuleBoxFaceContacts(capsule, polyData, *referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, normal);
+		const PxU32 faceContacts = numContacts - originalContacts;
+		if(faceContacts < 2)
+		{
+			generatedContactsEEContacts(capsule, polyData, *referencePolygon, map, aToB, manifoldContacts, numContacts, contactDist, normal);
+		}
+
+		return true;
+	}
+
+		//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 PxU32 originalContacts = numContacts;
+
+		Vec3V tNormal = normal;
+	
+		if(doOverlapTest)
+		{	
+			FloatV minOverlap;
+			
+			//overwrite the normal with the sperating axis
+			if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, tNormal))
+				return false;
+
+			generatedFaceContacts(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
+
+			const PxU32 faceContacts = numContacts - originalContacts;
+			if(faceContacts < 2)
+			{
+				const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getPolygonIndex(polyData, map, V3Neg(tNormal))];
+				generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB,  manifoldContacts, numContacts, contactDist, tNormal);
+			}
+		}
+		else
+		{
+			generatedFaceContacts(capsule, polyData, map, aToB, manifoldContacts, numContacts, contactDist, tNormal);
+
+			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 toleranceA = FMax(FMul(margin, eps), lowerEps);
+
+				const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getWitnessPolygonIndex(polyData, map, V3Neg(tNormal), closest, toleranceA)];
+				generatedContactsEEContacts(capsule, polyData,referencePolygon, map, aToB,  manifoldContacts, numContacts, contactDist, tNormal);
+			}
+		}
+
+		normal = tNormal;
+
+		return true;
+	}
+
+	//sphere is in the local space of polyData, we treate sphere as capsule
+	bool generateSphereFullContactManifold(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, PersistentContact* manifoldContacts, PxU32& numContacts,
+		const FloatVArg contactDist, Vec3V& normal, const bool doOverlapTest)
+	{
+
+		if(doOverlapTest)
+		{	
+			FloatV minOverlap;
+			//overwrite the normal with the sperating axis
+			if(!testPolyDataAxis(capsule, polyData, map, contactDist, minOverlap, normal))
+				return false;
+		}
+
+		const FloatV zero = FZero();
+		FloatV tEnter=zero, tExit=zero;
+		const FloatV inflatedRadius = FAdd(capsule.radius, contactDist);
+		const Vec3V dir = V3Neg(normal);
+		const Vec3V vertexSpaceDir = M33MulV3(map->shape2Vertex, dir);
+		const Vec3V p0 = M33MulV3(map->shape2Vertex, capsule.p0);
+		if(intersectSegmentPolyhedron(p0, vertexSpaceDir, polyData, tEnter, tExit) && FAllGrtrOrEq(inflatedRadius, tEnter))
+		{
+			//ML: for sphere, the contact point A is always zeroV in its local space
+			manifoldContacts[numContacts].mLocalPointA = V3Zero();
+			manifoldContacts[numContacts].mLocalPointB = V3ScaleAdd(dir, tEnter, capsule.p0);
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), tEnter);
+		}
+
+		return true;
+
+	}
+
+
+	//capsule is in the shape space of polyData 
+	bool computeMTD(const CapsuleV& capsule, PolygonalData& polyData, SupportLocal* map, FloatV& penDepth, Vec3V& normal)
+	{
+		const FloatV contactDist = FZero();
+		Vec3V separatingAxis;
+		FloatV minOverlap;
+		if(!testSATCapsulePoly(capsule, polyData, map, contactDist, minOverlap, separatingAxis))
+			return false;
+
+		//transform normal in to the worl space
+		normal = map->transform.rotate(separatingAxis);
+		penDepth = minOverlap;
+		
+		return true;
+	}
+
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
new file mode 100644
index 00000000..4ce69a58
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenUtil.h
@@ -0,0 +1,428 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_CONTACT_GEN_UTIL_H
+#define GU_PCM_CONTACT_GEN_UTIL_H
+
+#include "PsVecMath.h"
+#include "CmPhysXCommon.h"
+#include "GuShapeConvex.h"
+#include "GuVecCapsule.h"
+#include "GuConvexSupportTable.h"
+
+#define	PCM_WITNESS_POINT_EPS				1e-1f
+#define	PCM_WITNESS_POINT_ABSOLUTE_EPS		1e-5f
+
+namespace physx
+{
+
+namespace Gu
+{
+
+	enum FeatureStatus
+	{
+		POLYDATA0,
+		POLYDATA1,
+		EDGE
+	};
+
+
+	PX_FORCE_INLINE bool contains(Ps::aos::Vec3V* verts, const PxU32 numVerts, const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg min, const Ps::aos::Vec3VArg max)
+	{
+		using namespace Ps::aos;
+
+		const BoolV tempCon = BOr(V3IsGrtr(min, p), V3IsGrtr(p, max));
+		const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+
+		if (BAllEqTTTT(con))
+			return false;
+
+		const FloatV tx = V3GetX(p);
+		const FloatV ty = V3GetY(p);
+
+		const FloatV eps = FEps();
+		const FloatV zero = FZero();
+		PxU32 intersectionPoints = 0;
+
+		PxU32 i = 0, j = numVerts - 1;
+
+		for (; i < numVerts; j = i++)
+		{
+			const FloatV jy = V3GetY(verts[j]);
+			const FloatV iy = V3GetY(verts[i]);
+
+			const FloatV jx = V3GetX(verts[j]);
+			const FloatV ix = V3GetX(verts[i]);
+
+			//if p is one of the end point, we will return intersect
+			const BoolV con0 = BAnd(FIsEq(tx, jx), FIsEq(ty, jy));
+			const BoolV con1 = BAnd(FIsEq(tx, ix), FIsEq(ty, iy));
+
+			if (BAllEqTTTT(BOr(con0, con1)))
+			{
+				return true;
+			}
+
+			//(verts[i].y > test.y) != (points[j].y > test.y) 
+			const PxU32 yflag0 = FAllGrtr(jy, ty);
+			const PxU32 yflag1 = FAllGrtr(iy, ty);
+
+			//ML: the only case the ray will intersect this segment is when the p's y is in between two segments y
+			if (yflag0 != yflag1)
+			{
+			
+				//ML: choose ray, which start at p and every points in the ray will have the same y component
+				//t1 = (yp - yj)/(yi - yj)
+				//qx = xj + t1*(xi-xj)
+				//t = qx - xp > 0 for the ray and segment intersection happen
+				const FloatV jix = FSub(ix, jx);
+				const FloatV jiy = FSub(iy, jy);
+				//const FloatV jtx = FSub(tx, jy);
+				const FloatV jty = FSub(ty, jy);
+				const FloatV part1 = FMul(jty, jix);
+				//const FloatV part2 = FMul(jx,  jiy);
+				//const FloatV part3 = FMul(V3Sub(tx, eps),  jiy);
+				const FloatV part2 = FMul(FAdd(jx, eps), jiy);
+				const FloatV part3 = FMul(tx, jiy);
+
+				const BoolV comp = FIsGrtr(jiy, zero);
+				const FloatV tmp = FAdd(part1, part2);
+				const FloatV comp1 = FSel(comp, tmp, part3);
+				const FloatV comp2 = FSel(comp, part3, tmp);
+
+
+				if (FAllGrtrOrEq(comp1, comp2))
+				{
+					if (intersectionPoints == 1)
+					{
+						return false;
+					}
+					intersectionPoints++;
+				}
+			}
+		}
+		return intersectionPoints> 0;
+	}
+
+
+	PX_FORCE_INLINE bool boxContainsInXY(const Ps::aos::FloatVArg x, const Ps::aos::FloatVArg y, const Ps::aos::Vec3VArg p, const Ps::aos::Vec3V* verts, const Ps::aos::Vec3VArg min, const Ps::aos::Vec3VArg max)
+	{
+		using namespace Ps::aos;
+
+		const BoolV tempCon = BOr(V3IsGrtr(min, p), V3IsGrtr(p, max));
+		const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+		
+		if(BAllEqTTTT(con))
+			return false;  
+		
+		const FloatV zero = FZero();
+		FloatV PreviousX = V3GetX(verts[3]);
+		FloatV PreviousY = V3GetY(verts[3]);
+
+
+		// Loop through quad vertices
+		for(PxI32 i=0; i<4; i++)
+		{
+			const FloatV CurrentX = V3GetX(verts[i]);
+			const FloatV CurrentY = V3GetY(verts[i]);
+
+			// |CurrentX - PreviousX      x - PreviousX|
+			// |CurrentY - PreviousY      y - PreviousY|
+			// => similar to backface culling, check each one of the 4 triangles are consistent, in which case
+			// the point is within the parallelogram.
+			const FloatV v00 = FSub(CurrentX, PreviousX);
+			const FloatV v01 = FSub(y,  PreviousY);
+			const FloatV v10 = FSub(CurrentY, PreviousY);
+			const FloatV v11 = FSub(x,  PreviousX);
+			const FloatV temp0 = FMul(v00, v01);
+			const FloatV temp1 = FMul(v10, v11);
+			if(FAllGrtrOrEq(FSub(temp0, temp1), zero))
+				return false;
+
+			PreviousX = CurrentX;
+			PreviousY = CurrentY;
+		}
+
+		return true;
+
+	}
+
+
+	PX_FORCE_INLINE Ps::aos::FloatV signed2DTriArea(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, const Ps::aos::Vec3VArg c)
+	{
+		using namespace Ps::aos;
+		const Vec3V ca = V3Sub(a, c);
+		const Vec3V cb = V3Sub(b, c);
+
+		const FloatV t0 = FMul(V3GetX(ca), V3GetY(cb));
+		const FloatV t1 = FMul(V3GetY(ca), V3GetX(cb));
+
+		return FSub(t0, t1);
+	}
+
+
+	PX_FORCE_INLINE PxI32 getPolygonIndex(const Gu::PolygonalData& polyData, SupportLocal* map, const Ps::aos::Vec3VArg normal)
+	{
+		using namespace Ps::aos;
+
+		//normal is in shape space, need to transform the vertex space
+		const Vec3V n = M33TrnspsMulV3(map->vertex2Shape, normal);
+		const Vec3V nnormal = V3Neg(n);
+		const Vec3V planeN_ = V3LoadU_SafeReadW(polyData.mPolygons[0].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+		FloatV minProj = V3Dot(n, planeN_);
+
+		const FloatV zero = FZero();
+		PxI32 closestFaceIndex = 0;
+
+		for(PxU32 i=1; i< polyData.mNbPolygons; ++i)
+		{
+			const Vec3V planeN = V3LoadU_SafeReadW(polyData.mPolygons[i].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+			const FloatV proj = V3Dot(n, planeN);
+			if(FAllGrtr(minProj, proj))
+			{
+				minProj = proj;
+				closestFaceIndex = PxI32(i);
+			}
+		}
+
+		const PxU32 numEdges = polyData.mNbEdges;
+		const PxU8* const edgeToFace = polyData.mFacesByEdges;
+
+		//Loop through edges
+		PxU32 closestEdge = 0xffffffff;
+		FloatV maxDpSq = FMul(minProj, minProj);
+
+		for(PxU32 i=0; i < numEdges; ++i)//, inc = VecI32V_Add(inc, vOne))
+		{
+			const PxU32 index = i*2;
+			const PxU8 f0 = edgeToFace[index];
+			const PxU8 f1 = edgeToFace[index+1];
+
+			const Vec3V planeNormal0 = V3LoadU_SafeReadW(polyData.mPolygons[f0].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+			const Vec3V planeNormal1 = V3LoadU_SafeReadW(polyData.mPolygons[f1].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+
+			// unnormalized edge normal
+			const Vec3V edgeNormal = V3Add(planeNormal0, planeNormal1);//polys[f0].mPlane.n + polys[f1].mPlane.n;
+			const FloatV enMagSq = V3Dot(edgeNormal, edgeNormal);//edgeNormal.magnitudeSquared();
+			//Test normal of current edge - squared test is valid if dp and maxDp both >= 0
+			const FloatV dp = V3Dot(edgeNormal, nnormal);//edgeNormal.dot(normal);
+			const FloatV sqDp = FMul(dp, dp);
+			
+			const BoolV con0 = FIsGrtrOrEq(dp, zero);
+			const BoolV con1 = FIsGrtr(sqDp, FMul(maxDpSq, enMagSq));
+			const BoolV con = BAnd(con0, con1);
+			if(BAllEqTTTT(con))
+			{
+				maxDpSq = FDiv(sqDp, enMagSq);
+				closestEdge = i;
+			}
+		} 
+
+		if(closestEdge!=0xffffffff)
+		{
+			const PxU8* FBE = edgeToFace;
+
+			const PxU32 index = closestEdge*2;
+			const PxU32 f0 = FBE[index];
+			const PxU32 f1 = FBE[index+1];
+
+			const Vec3V planeNormal0 = V3LoadU_SafeReadW(polyData.mPolygons[f0].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+			const Vec3V planeNormal1 = V3LoadU_SafeReadW(polyData.mPolygons[f1].mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+
+			const FloatV dp0 = V3Dot(planeNormal0, nnormal);
+			const FloatV dp1 = V3Dot(planeNormal1, nnormal);
+			if(FAllGrtr(dp0, dp1))
+			{
+				closestFaceIndex = PxI32(f0);
+			}
+			else
+			{
+				closestFaceIndex = PxI32(f1);
+			}
+		}
+
+		return closestFaceIndex;
+
+	}
+
+	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)
+	{
+		using namespace Ps::aos;
+
+		//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)
+		{
+			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)))
+			{
+				//rotate the plane normal to shape space
+				n = V3Normalize(M33TrnspsMulV3(map->shape2Vertex, n));
+				FloatV proj = V3Dot(n, normal);
+
+				if(FAllGrtr(bestProj, proj))
+				{
+					minDist= pd;
+					closestFaceIndex = i;
+					bestProj = proj;
+				}
+			}
+		}
+
+		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)
+	{
+		using namespace Ps::aos;
+		FloatV _minOverlap = FMax();//minOverlap;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		Vec3V tempAxis = V3UnitY();
+
+		//capsule in the local space of polyData
+		for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+
+			const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+			const FloatV planeDist = FLoad(polygon.mPlane.d);
+			const Vec3V vertexSpacePlaneNormal = V3LoadU_SafeReadW(polygon.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+
+			//transform plane n to shape space
+			const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map->shape2Vertex, vertexSpacePlaneNormal);
+
+			const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
+			//normalize shape space normal
+			const Vec3V planeN = V3Scale(shapeSpacePlaneNormal, magnitude);
+			//ML::use this to avoid LHS
+			min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
+			max0 = FMul(FNeg(planeDist), magnitude);
+
+		
+			const FloatV tempMin = V3Dot(capsule.p0, planeN);
+			const FloatV tempMax = V3Dot(capsule.p1, planeN);
+			min1 = FMin(tempMin, tempMax);
+			max1 = FMax(tempMin, tempMax);
+
+			min1 = FSub(min1, capsule.radius);
+			max1 = FAdd(max1, capsule.radius);
+
+
+			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+			if(BAllEqTTTT(con))
+				return false;
+
+			const FloatV tempOverlap = FSub(max0, min1);
+
+			if(FAllGrtr(_minOverlap, tempOverlap))
+			{
+				_minOverlap = tempOverlap;
+				tempAxis = planeN;
+			}
+		}
+
+		separatingAxis = tempAxis;
+		minOverlap = _minOverlap;
+
+		return true;
+
+	}  
+
+	//ML: this function is shared by sphere/capsule. Point a and direction d need to be in the local space of polyData
+	PX_FORCE_INLINE bool intersectSegmentPolyhedron(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg dir, const PolygonalData& polyData, Ps::aos::FloatV& tEnter, Ps::aos::FloatV& tExit)
+	{
+		using namespace Ps::aos;
+		const FloatV zero = FZero();
+		const FloatV one = FOne();
+		const FloatV eps = FLoad(1e-7f);
+		FloatV tFirst = zero;
+		FloatV tLast= one;
+
+		for(PxU32 k=0; k<polyData.mNbPolygons; ++k)
+		{
+			const Gu::HullPolygonData& data = polyData.mPolygons[k];
+			const Vec3V n = V3LoadU_SafeReadW(data.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+
+			FloatV d = FLoad(data.mPlane.d);
+	
+			const FloatV denominator = V3Dot(n, dir);
+			const FloatV distToPlane = FAdd(V3Dot(n, a), d);
+		
+			if(FAllGrtr(eps, FAbs(denominator)))
+			{
+				if(FAllGrtr(distToPlane, zero))
+					return false;
+			}
+			else
+			{
+				FloatV tTemp = FNeg(FDiv(distToPlane, denominator));
+
+				//ML: denominator < 0 means the ray is entering halfspace; denominator > 0 means the ray is exiting halfspace
+				const BoolV con = FIsGrtr(zero, denominator);
+				const BoolV con0= FIsGrtr(tTemp, tFirst); 
+				const BoolV con1 = FIsGrtr(tLast, tTemp);
+	
+				tFirst = FSel(BAnd(con, con0), tTemp, tFirst);
+				tLast = FSel(BAndNot(con1, con), tTemp, tLast);
+			}
+			
+			if(FAllGrtr(tFirst, tLast))
+				return false;
+		}
+
+		//calculate the intersect p in the local space
+		tEnter = tFirst;
+		tExit = tLast;
+	
+		return true;
+	}
+
+}//Gu
+}//physx
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactMeshCallback.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactMeshCallback.h
new file mode 100644
index 00000000..9917b429
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactMeshCallback.h
@@ -0,0 +1,203 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_CONTACT_MESH_CALLBACK_H
+#define GU_PCM_CONTACT_MESH_CALLBACK_H
+
+#include "GuMidphaseInterface.h"
+#include "GuEntityReport.h"
+#include "GuHeightFieldUtil.h"
+#include "GuTriangleCache.h"
+#include "GuConvexEdgeFlags.h"
+
+namespace physx
+{
+
+namespace Gu
+{
+
+template <typename Derived>
+struct PCMMeshContactGenerationCallback : MeshHitCallback<PxRaycastHit>
+{
+public:
+	const Cm::FastVertex2ShapeScaling&		mMeshScaling;
+	const PxU8* PX_RESTRICT					mExtraTrigData;
+	bool									mIdtMeshScale;
+	static const PxU32 CacheSize = 16;
+	Gu::TriangleCache<CacheSize>			mCache;
+
+	PCMMeshContactGenerationCallback(const Cm::FastVertex2ShapeScaling& meshScaling, const PxU8* extraTrigData, bool idtMeshScale)
+	:	MeshHitCallback<PxRaycastHit>(CallbackMode::eMULTIPLE),
+		mMeshScaling(meshScaling), mExtraTrigData(extraTrigData), mIdtMeshScale(idtMeshScale)
+	{
+	}
+
+	void flushCache() 
+	{
+		if (!mCache.isEmpty())
+		{
+			(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(mCache);
+			mCache.reset();
+		}
+	}
+
+	virtual PxAgain processHit(
+		const PxRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* vinds)
+	{
+		if (!(static_cast<Derived*>(this))->doTest(v0, v1, v2))
+			return true;
+
+		PxVec3 v[3];
+		if(mIdtMeshScale)
+		{
+			v[0] = v0;
+			v[1] = v1;
+			v[2] = v2;
+		}
+		else
+		{
+			const PxI32 winding = mMeshScaling.flipsNormal() ? 1 : 0;
+			v[0] = mMeshScaling * v0;
+			v[1 + winding] = mMeshScaling * v1;
+			v[2 - winding] = mMeshScaling * v2;
+		}
+
+		const PxU32 triangleIndex = hit.faceIndex;
+		const PxU8 extraData = getConvexEdgeFlags(mExtraTrigData, triangleIndex);
+
+		if (mCache.isFull())
+		{
+			(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(mCache);
+			mCache.reset();
+		}
+		mCache.addTriangle(v, vinds, triangleIndex, extraData);
+
+		return true;
+	}
+
+protected:
+	PCMMeshContactGenerationCallback& operator=(const PCMMeshContactGenerationCallback&);
+};
+
+template <typename Derived>
+struct PCMHeightfieldContactGenerationCallback : Gu::EntityReport<PxU32>
+{
+public:
+	const Gu::HeightFieldUtil&	mHfUtil;
+	const PxTransform&			mHeightfieldTransform;
+
+	PCMHeightfieldContactGenerationCallback(const Gu::HeightFieldUtil& hfUtil, const PxTransform& heightfieldTransform)	:
+		mHfUtil(hfUtil), mHeightfieldTransform(heightfieldTransform)
+	{
+
+	}
+
+	// PT: TODO: refactor/unify with similar code in other places
+	virtual PxAgain onEvent(PxU32 nb, PxU32* indices)
+	{
+		const PxU32 CacheSize = 16;
+		Gu::TriangleCache<CacheSize> cache;
+
+		const PxU32 nbPasses = (nb+(CacheSize-1))/CacheSize;
+		PxU32 nbTrigs = nb;
+		PxU32* inds0 = indices;
+
+		const PxU8 nextInd[] = {2,0,1};
+
+		for(PxU32 i = 0; i < nbPasses; ++i)
+		{
+			cache.mNumTriangles = 0;
+			PxU32 trigCount = PxMin(nbTrigs, CacheSize);
+			nbTrigs -= trigCount;
+			while(trigCount--)
+			{
+				PxU32 triangleIndex = *(inds0++);
+				PxU32 vertIndices[3];
+
+				PxTriangle currentTriangle;	// in world space
+
+				PxU32 adjInds[3];
+				mHfUtil.getTriangle(mHeightfieldTransform, currentTriangle, vertIndices, adjInds, triangleIndex, false, false);
+
+				PxVec3 normal;
+				currentTriangle.normal(normal);
+
+				PxU8 triFlags = 0; //KS - temporary until we can calculate triFlags for HF
+
+				for(PxU32 a = 0; a < 3; ++a)
+				{
+
+					if(adjInds[a] != 0xFFFFFFFF)
+					{
+						PxTriangle adjTri;
+						PxU32 inds[3];
+						mHfUtil.getTriangle(mHeightfieldTransform, adjTri, inds, NULL, adjInds[a], false, false);
+						//We now compare the triangles to see if this edge is active
+
+						PX_ASSERT(inds[0] == vertIndices[a] || inds[1] == vertIndices[a] || inds[2] == vertIndices[a]);
+						PX_ASSERT(inds[0] == vertIndices[(a+1)%3] || inds[1] == vertIndices[(a+1)%3] || inds[2] == vertIndices[(a+1)%3]);
+
+
+						PxVec3 adjNormal;
+						adjTri.denormalizedNormal(adjNormal);
+						PxU32 otherIndex = nextInd[a];
+						PxF32 projD = adjNormal.dot(currentTriangle.verts[otherIndex] - adjTri.verts[0]);
+
+						if(projD < 0.f)
+						{
+							adjNormal.normalize();
+
+							PxF32 proj = adjNormal.dot(normal);
+
+							if(proj < 0.997f)
+							{
+								triFlags |= (1 << (a+3));
+							}
+						}
+					}
+					else
+						triFlags |= (1 << (a+3));
+				}
+
+				cache.addTriangle(currentTriangle.verts, vertIndices, triangleIndex, triFlags);
+			}
+			PX_ASSERT(cache.mNumTriangles <= 16);
+
+			(static_cast<Derived*>(this))->template processTriangleCache< CacheSize >(cache);
+		}
+		return true;
+	}	
+protected:
+	PCMHeightfieldContactGenerationCallback& operator=(const PCMHeightfieldContactGenerationCallback&);
+};
+
+}//Gu
+}//physx
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp
new file mode 100644
index 00000000..68b30f56
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneBox.cpp
@@ -0,0 +1,220 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecBox.h"
+#include "GuGeometryUnion.h"
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+
+#include "GuPersistentContactManifold.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(shape0);
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(&manifold, 256);
+
+	// Get actual shape data
+	const PxBoxGeometry& shapeBox = shape1.get<const PxBoxGeometry>();
+
+	const PsTransformV transf0 = loadTransformA(transform1);//box transform
+	const PsTransformV transf1 = loadTransformA(transform0);//plane transform
+
+	//box to plane
+	const PsTransformV curTransf(transf1.transformInv(transf0));
+
+	//in world space
+	const Vec3V negPlaneNormal = V3Normalize(V3Neg(QuatGetBasisVector0(transf1.q)));
+	
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
+
+	const FloatV boxMargin = CalculatePCMBoxMargin(boxExtents);
+	const FloatV projectBreakingThreshold = FMul(boxMargin, FLoad(0.2f));
+	const PxU32 initialContacts = manifold.mNumContacts;
+	
+	manifold.refreshContactPoints(curTransf, projectBreakingThreshold, contactDist);
+
+	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
+		//work out the correct pentration for points
+		const Vec3V localNormal = V3UnitX();
+
+		manifold.mNumContacts = 0;
+		manifold.setRelativeTransform(curTransf);
+
+		const PsMatTransformV aToB(curTransf);
+		const FloatV bx = V3GetX(boxExtents);
+		const FloatV by = V3GetY(boxExtents);
+		const FloatV bz = V3GetZ(boxExtents);
+
+		const FloatV nbx = FNeg(bx);
+		const FloatV nby = FNeg(by);
+		const FloatV nbz = FNeg(bz);
+
+		const Vec3V temp0 = V3Scale(aToB.getCol0(), bx);
+		const Vec3V temp1 = V3Scale(aToB.getCol1(), by);
+		const Vec3V temp2 = V3Scale(aToB.getCol2(), bz);
+
+		const Vec3V ntemp2 = V3Neg(temp2);
+
+		const FloatV px = V3GetX(aToB.p);
+
+		//box's points in the local space of plane
+		const Vec3V temp01 = V3Add(temp0, temp1);//(x, y)
+		const Vec3V temp02 = V3Sub(temp0, temp1);//(x, -y)
+
+		const FloatV s0 = V3GetX(V3Add(temp2,	temp01));//(x, y, z)
+		const FloatV s1 = V3GetX(V3Add(ntemp2,	temp01));//(x, y, -z)
+		const FloatV s2 = V3GetX(V3Add(temp2,	temp02));//(x, -y, z)
+		const FloatV s3 = V3GetX(V3Add(ntemp2,	temp02));//(x, -y, -z)
+		const FloatV s4 = V3GetX(V3Sub(temp2,	temp02));//(-x, y, z)
+		const FloatV s5 = V3GetX(V3Sub(ntemp2,	temp02));//(-x, y, -z)
+		const FloatV s6 = V3GetX(V3Sub(temp2,	temp01));//(-x, -y, z)
+		const FloatV s7 = V3GetX(V3Sub(ntemp2,	temp01));//(-x, -y, -z)
+
+		const FloatV acceptanceDist = FSub(contactDist, px);
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		PxU32 numContacts = 0;
+
+		if(FAllGrtr(acceptanceDist, s0))
+		{
+			const FloatV pen = FAdd(s0, px);
+			//(x, y, z)
+			manifoldContacts[numContacts].mLocalPointA = boxExtents;//aToB.transformInv(p); 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(boxExtents)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+	
+		if(FAllGrtr(acceptanceDist, s1))
+		{
+			const FloatV pen = FAdd(s1, px);
+			//(x, y, -z)
+			const Vec3V p = V3Merge(bx, by, nbz);
+			//add to contact stream
+			manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p); 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+
+		if(FAllGrtr(acceptanceDist, s2))
+		{
+			const FloatV pen = FAdd(s2, px);
+			//(x, -y, z)
+			const Vec3V p = V3Merge(bx, nby, bz);
+			manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p); 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+
+		if(FAllGrtr(acceptanceDist, s3))
+		{
+			const FloatV pen = FAdd(s3, px);
+			//(x, -y, -z)
+			const Vec3V p = V3Merge(bx, nby, nbz);
+			manifoldContacts[numContacts].mLocalPointA = p; 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+	
+		if(FAllGrtr(acceptanceDist, s4))
+		{
+			const FloatV pen = FAdd(s4, px);
+			//(-x, y, z)
+			const Vec3V p =V3Merge(nbx, by, bz);
+			manifoldContacts[numContacts].mLocalPointA = p;
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+
+		if(FAllGrtr(acceptanceDist, s5))
+		{
+			const FloatV pen = FAdd(s5, px);
+			//(-x, y, -z)
+			const Vec3V p = V3Merge(nbx, by, nbz);
+			manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p); 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+
+	
+		if(FAllGrtr(acceptanceDist, s6))
+		{
+			const FloatV pen = FAdd(s6, px);
+			//(-x, -y, z)
+			const Vec3V p = V3Merge(nbx, nby, bz);
+			manifoldContacts[numContacts].mLocalPointA = p;//aToB.transformInv(p); 
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+	
+		if(FAllGrtr(acceptanceDist, s7))
+		{
+			const FloatV pen = FAdd(s7, px);
+			//(-x, -y, -z)
+			const Vec3V p = V3Merge(nbx, nby, nbz);
+			manifoldContacts[numContacts].mLocalPointA = p;
+			manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, pen, aToB.transform(p)); 
+			manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), pen);
+		}
+		
+		//reduce contacts
+		manifold.addBatchManifoldContactsCluster(manifoldContacts, numContacts);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
+
+		return manifold.getNumContacts() > 0;
+	}
+	else
+	{
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
+		
+		//manifold.drawManifold(*gRenderOutPut, transf0, transf1);
+		return manifold.getNumContacts() > 0;
+	}	
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneCapsule.cpp
new file mode 100644
index 00000000..0a8bd8e8
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneCapsule.cpp
@@ -0,0 +1,128 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecCapsule.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPersistentContactManifold.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool pcmContactPlaneCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(shape0);
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(&manifold, 256);
+
+	// Get actual shape data
+	const PxCapsuleGeometry& shapeCapsule = shape1.get<const PxCapsuleGeometry>();
+
+	const PsTransformV transf0 = loadTransformA(transform1);//capsule transform
+	const PsTransformV transf1 = loadTransformA(transform0);//plane transform
+	//capsule to plane
+	const PsTransformV aToB(transf1.transformInv(transf0));
+
+	//in world space
+	const Vec3V negPlaneNormal = V3Normalize(V3Neg(QuatGetBasisVector0(transf1.q)));
+	//ML:localNormal is the local space of plane normal, however, because shape1 is capulse and shape0 is plane, we need to use the reverse of contact normal(which will be the plane normal) to make the refreshContactPoints
+	//work out the correct pentration for points
+	const Vec3V localNormal = V3UnitX();
+
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const FloatV radius = FLoad(shapeCapsule.radius);
+	const FloatV halfHeight = FLoad(shapeCapsule.halfHeight);
+
+	//capsule is in the local space of plane(n = (1.f, 0.f, 0.f), d=0.f)
+	const Vec3V basisVector = QuatGetBasisVector0(aToB.q);
+	const Vec3V tmp = V3Scale(basisVector, halfHeight);
+	const Vec3V s = V3Add(aToB.p, tmp);
+	const Vec3V e = V3Sub(aToB.p, tmp);
+
+	const FloatV inflatedRadius = FAdd(radius, contactDist);
+	const FloatV replaceBreakingThreshold = FMul(radius, FLoad(0.001f));
+	const FloatV projectBreakingThreshold = FMul(radius, FLoad(0.05f));
+	const PxU32 initialContacts = manifold.mNumContacts;
+
+	//manifold.refreshContactPoints(curRTrans, projectBreakingThreshold, contactDist);
+	const FloatV refreshDist = FAdd(contactDist, radius);
+	manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
+
+	const PxU32 newContacts = manifold.mNumContacts;
+	const bool bLostContacts = (newContacts != initialContacts);//((initialContacts == 0) || (newContacts != initialContacts));
+
+	//PX_UNUSED(bLostContacts);
+	if(bLostContacts || manifold.invalidate_PrimitivesPlane(aToB, radius, FLoad(0.02f)))  
+	{
+		manifold.mNumContacts = 0;
+		manifold.setRelativeTransform(aToB);
+		//calculate the distance from s to the plane
+		const FloatV signDist0  = V3GetX(s);//V3Dot(localNormal, s);
+		if(FAllGrtr(inflatedRadius, signDist0))
+		{
+			const Vec3V localPointA = aToB.transformInv(s);
+			const Vec3V localPointB = V3NegScaleSub(localNormal, signDist0, s);
+			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist0);
+			//add to manifold
+			manifold.addManifoldPoint2(localPointA, localPointB, localNormalPen, replaceBreakingThreshold); 
+		}
+
+		const FloatV signDist1 = V3GetX(e);//V3Dot(localNormal, e);
+		if(FAllGrtr(inflatedRadius, signDist1))
+		{
+			const Vec3V localPointA = aToB.transformInv(e);
+			const Vec3V localPointB = V3NegScaleSub(localNormal, signDist1, e);
+			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist1);
+			//add to manifold
+			manifold.addManifoldPoint2(localPointA, localPointB, localNormalPen, replaceBreakingThreshold); 
+		}
+
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf0, radius, contactDist);
+
+
+		return manifold.getNumContacts() > 0;
+	}
+	else
+	{
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf0, radius, contactDist);
+		return manifold.getNumContacts() > 0;
+	}
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp
new file mode 100644
index 00000000..611dcfd5
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactPlaneConvex.cpp
@@ -0,0 +1,159 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecConvexHull.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPersistentContactManifold.h"
+
+
+namespace physx
+{
+namespace Gu
+{
+bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(shape0);
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+	Ps::prefetchLine(&manifold, 256);
+
+	// Get actual shape data
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+
+	const PsTransformV transf0 = loadTransformA(transform1);//convex transform
+	const PsTransformV transf1 = loadTransformA(transform0);//plane transform
+	//convex to plane
+	const PsTransformV curTransf(transf1.transformInv(transf0));
+	
+	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);
+	
+
+	//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));
+	const FloatV projectBreakingThreshold = FMul(convexMargin, FLoad(0.2f));
+	const PxU32 initialContacts = manifold.mNumContacts;
+	
+	manifold.refreshContactPoints(curTransf, projectBreakingThreshold, contactDist);
+
+	const PxU32 newContacts = manifold.mNumContacts;
+	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);
+		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
+
+		const Mat33V vertex2Shape = ConstructVertex2ShapeMatrix(vScale, vQuat);
+		
+		//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
+		//work out the correct pentration for points
+		const Vec3V localNormal = V3UnitX();
+
+		manifold.mNumContacts = 0;
+		manifold.setRelativeTransform(curTransf);
+		const PxVec3* PX_RESTRICT verts = hullData->getHullVertices();
+		const PxU8 numVerts = hullData->mNbHullVertices;
+
+		Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+		PxU32 numContacts = 0;
+		
+		const PsMatTransformV aToBVertexSpace(aToB.p, M33MulM33(aToB.rot, vertex2Shape));
+		//brute force each points
+		for(PxU8 i=0; i<numVerts; ++i)
+		{
+			//in the vertex space of convex
+			const Vec3V pInVertexSpace = V3LoadU(verts[i]);
+			
+			//transform p into plane space
+			const Vec3V pInPlaneSpace = aToBVertexSpace.transform(pInVertexSpace);//V3Add(aToB.p, M33MulV3(temp1, pInVertexSpace));
+		
+			const FloatV signDist = V3GetX(pInPlaneSpace);
+			
+			if(FAllGrtr(contactDist, signDist))
+			{
+				//transform p into shape space
+				const Vec3V pInShapeSpace = M33MulV3(vertex2Shape, pInVertexSpace);
+				//add to manifold
+				
+				manifoldContacts[numContacts].mLocalPointA = pInShapeSpace;
+				manifoldContacts[numContacts].mLocalPointB = V3NegScaleSub(localNormal, signDist, pInPlaneSpace); 
+				manifoldContacts[numContacts++].mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), signDist);
+
+				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);
+					numContacts = GU_MANIFOLD_CACHE_SIZE;
+					for(PxU32 j=0; j<GU_MANIFOLD_CACHE_SIZE; ++j)
+					{
+						manifoldContacts[j] = manifold.mContactPoints[j];
+					}
+				}
+			}
+		}
+
+		//reduce contacts
+		manifold.addBatchManifoldContacts(manifoldContacts, numContacts);
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+
+		return manifold.getNumContacts() > 0;
+	}
+	else
+	{
+		manifold.addManifoldContactsToContactBuffer(contactBuffer, negPlaneNormal, transf1, contactDist);
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1);
+#endif
+		return manifold.getNumContacts() > 0;
+	}	
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereBox.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereBox.cpp
new file mode 100644
index 00000000..2c0378e6
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereBox.cpp
@@ -0,0 +1,157 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+//#include "GuGJKWrapper.h"
+#include "GuVecBox.h"
+#include "GuVecSphere.h"
+#include "GuGeometryUnion.h"
+   
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+
+
+
+namespace physx
+{
+
+namespace Gu
+{
+bool pcmContactSphereBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	using namespace Ps::aos;
+	// Get actual shape data
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxBoxGeometry& shapeBox = shape1.get<const PxBoxGeometry>();
+	//
+
+	//const PsTransformV transf0(transform0);
+	const Vec3V sphereOrigin = V3LoadA(&transform0.p.x);
+	//const PsTransformV transf1(transform1);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV radius = FLoad(shapeSphere.radius);
+	
+	const PsTransformV transf1(p1, q1);
+	
+	const FloatV cDist = FLoad(params.mContactDistance);
+
+	const Vec3V boxExtents = V3LoadU(shapeBox.halfExtents);
+
+	//translate sphere center into the box space
+	const Vec3V sphereCenter = transf1.transformInv(sphereOrigin);
+
+	const Vec3V nBoxExtents = V3Neg(boxExtents);
+
+	//const FloatV radSq = FMul(radius, radius);
+
+	const FloatV inflatedSum = FAdd(radius, cDist);
+	const FloatV sqInflatedSum = FMul(inflatedSum, inflatedSum);
+
+	const Vec3V p = V3Clamp(sphereCenter, nBoxExtents, boxExtents);
+	const Vec3V v = V3Sub(sphereCenter, p);
+	const FloatV lengthSq = V3Dot(v, v);
+
+	PX_ASSERT(contactBuffer.count < ContactBuffer::MAX_CONTACTS);
+
+	if(FAllGrtr(sqInflatedSum, lengthSq))//intersect
+	{
+		//check whether the spherCenter is inside the box
+		const BoolV bInsideBox = V3IsGrtrOrEq(boxExtents, V3Abs(sphereCenter));
+		// PT: TODO: ??? revisit this, why do we have both BAllEqTTTT and BAllTrue3?
+		if(BAllEqTTTT(BAllTrue3(bInsideBox)))//sphere center inside the box
+		{
+			//Pick directions and sign
+			const Vec3V absP = V3Abs(p);
+			const Vec3V distToSurface = V3Sub(boxExtents, absP);//dist from embedded center to box surface along 3 dimensions.
+			
+			const FloatV x = V3GetX(distToSurface);
+			const FloatV y = V3GetY(distToSurface);
+			const FloatV z = V3GetZ(distToSurface);
+
+			const Vec3V xV = V3Splat(x);
+			const Vec3V zV = V3Splat(z);
+
+			//find smallest element of distToSurface
+			const BoolV con0 = BAllTrue3(V3IsGrtrOrEq(distToSurface, zV));
+			const BoolV con1 = BAllTrue3(V3IsGrtrOrEq(distToSurface, xV));
+			const Vec3V sign = V3Sign(p);
+	
+			const Vec3V tmpX = V3Mul(V3UnitX(), sign);
+			const Vec3V tmpY = V3Mul(V3UnitY(), sign);
+			const Vec3V tmpZ = V3Mul(V3UnitZ(), sign);
+			
+			const Vec3V locNorm= V3Sel(con0, tmpZ, V3Sel(con1, tmpX, tmpY));////local coords contact normal
+			const FloatV dist = FNeg(FSel(con0, z, FSel(con1, x, y)));
+
+			//separation so far is just the embedding of the center point; we still have to push out all of the radius.
+			const Vec3V point = sphereOrigin;
+			const Vec3V normal = transf1.rotate(locNorm);
+			const FloatV penetration = FSub(dist, radius);
+
+
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+			V4StoreA(Vec4V_From_Vec3V(normal), &contact.normal.x);
+			V4StoreA(Vec4V_From_Vec3V(point), &contact.point.x);
+			FStore(penetration, &contact.separation);
+
+			contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+			//context.mContactBuffer.contact(point, normal, penetration);
+		}
+		else
+		{
+			//get the closest point from the center to the box surface
+			const FloatV recipLength = FRsqrt(lengthSq);
+			const FloatV length = FRecip(recipLength);
+			const Vec3V locNorm = V3Scale(v, recipLength);
+			const FloatV penetration = FSub(length, radius);
+			const Vec3V normal = transf1.rotate(locNorm);
+			const Vec3V point = transf1.transform(p);
+
+			PX_ASSERT(contactBuffer.count < ContactBuffer::MAX_CONTACTS);
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+			V4StoreA(Vec4V_From_Vec3V(normal), &contact.normal.x);
+			V4StoreA(Vec4V_From_Vec3V(point), &contact.point.x);
+			FStore(penetration, &contact.separation);
+
+			contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+
+			//context.mContactBuffer.contact(point, normal, penetration);
+		}
+		return true;
+	}
+	return false;
+}
+
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereCapsule.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereCapsule.cpp
new file mode 100644
index 00000000..090c3152
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereCapsule.cpp
@@ -0,0 +1,121 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+//#include "GuGJKWrapper.h"
+#include "GuVecSphere.h"
+#include "GuVecCapsule.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+
+
+namespace physx
+{
+
+namespace Gu
+{
+PX_FORCE_INLINE Ps::aos::FloatV PxcDistancePointSegmentSquared(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, const Ps::aos::Vec3VArg p, Ps::aos::FloatV& param)
+{
+	using namespace Ps::aos;
+	const FloatV zero = FZero();
+	const FloatV one = FOne();
+
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V ab = V3Sub(b, a);
+	const FloatV nom = V3Dot(ap, ab);
+	
+	const FloatV denom = V3Dot(ab, ab);
+	const FloatV tValue = FClamp(FMul(nom, FDiv(one, denom)), zero, one);
+
+	const FloatV t = FSel(FIsEq(denom, zero), zero, tValue);
+	const Vec3V v = V3NegScaleSub(ab, t, ap);
+	param = t;
+	return V3Dot(v, v);
+}
+
+bool pcmContactSphereCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxCapsuleGeometry& shapeCapsule = shape1.get<const PxCapsuleGeometry>();
+
+	//Sphere in world space
+	const Vec3V sphereCenter =  V3LoadA(&transform0.p.x);
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV sphereRadius = FLoad(shapeSphere.radius);
+	const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
+	const FloatV cDist = FLoad(params.mContactDistance);
+
+	//const FloatV r0 = FloatV_From_F32(shapeCapsule.radius);
+	const FloatV halfHeight0 = FLoad(shapeCapsule.halfHeight);
+	const Vec3V basisVector0 = QuatGetBasisVector0(q1);
+	const Vec3V tmp0 = V3Scale(basisVector0, halfHeight0);
+	const Vec3V s = V3Add(p1, tmp0);
+	const Vec3V e = V3Sub(p1, tmp0);
+
+	
+	const FloatV radiusSum = FAdd(sphereRadius, capsuleRadius);
+	const FloatV inflatedSum = FAdd(radiusSum, cDist);
+
+	// Collision detection
+	FloatV t;
+	const FloatV squareDist = PxcDistancePointSegmentSquared(s, e, sphereCenter, t);
+	const FloatV sqInflatedSum = FMul(inflatedSum, inflatedSum);
+
+	if(FAllGrtr(sqInflatedSum, squareDist))//BAllEq(con, bTrue))
+	{
+		const Vec3V p = V3ScaleAdd(V3Sub(e, s), t, s);
+		const Vec3V dir = V3Sub(sphereCenter, p);
+		const Vec3V normal = V3NormalizeSafe(dir, V3UnitX());
+		const Vec3V point = V3NegScaleSub(normal, sphereRadius, sphereCenter);//transform back to the world space
+
+		const FloatV dist = FSub(FSqrt(squareDist), radiusSum);
+		//context.mContactBuffer.contact(point, normal, FSub(FSqrt(squareDist), radiusSum));
+		PX_ASSERT(contactBuffer.count < ContactBuffer::MAX_CONTACTS);
+		Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+
+		V4StoreA(Vec4V_From_Vec3V(normal), &contact.normal.x);
+		V4StoreA(Vec4V_From_Vec3V(point), &contact.point.x);
+		FStore(dist, &contact.separation);
+
+		contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+
+		return true;
+	}
+	return false;
+}
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp
new file mode 100644
index 00000000..379b90b1
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereConvex.cpp
@@ -0,0 +1,294 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuGJKPenetration.h"
+#include "GuEPA.h"
+#include "GuVecCapsule.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuVecShrunkConvexHull.h"
+#include "GuVecShrunkConvexHullNoScale.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuContactBuffer.h"
+#include "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+static void addToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg worldNormal, const Ps::aos::Vec3VArg worldPoint, const Ps::aos::FloatVArg penDep)
+{
+	using namespace Ps::aos;
+	Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+	V4StoreA(Vec4V_From_Vec3V(worldNormal), reinterpret_cast<PxF32*>(&contact.normal.x));
+	V4StoreA(Vec4V_From_Vec3V(worldPoint), reinterpret_cast<PxF32*>(&contact.point.x));
+	FStore(penDep, &contact.separation);
+
+	PX_ASSERT(contact.point.isFinite());
+	PX_ASSERT(contact.normal.isFinite());
+	PX_ASSERT(PxIsFinite(contact.separation));
+
+	contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+
+}
+
+static bool fullContactsGenerationSphereConvex(const Gu::CapsuleV& capsule, const Gu::ConvexHullV& convexHull, const Ps::aos::PsTransformV& transf0,const Ps::aos::PsTransformV& transf1,
+								Gu::PersistentContact* manifoldContacts, Gu::ContactBuffer& contactBuffer, const bool idtScale, Gu::PersistentContactManifold& manifold, 
+								Ps::aos::Vec3VArg normal, const Ps::aos::FloatVArg contactDist, bool doOverlapTest, Cm::RenderOutput* renderOutput)
+{
+	PX_UNUSED(renderOutput);
+	using namespace Ps::aos;
+	Gu::PolygonalData polyData;
+	getPCMConvexData(convexHull,idtScale, polyData);
+
+	PxU8 buff[sizeof(SupportLocalImpl<ConvexHullV>)];
+	SupportLocal* map = (idtScale ? static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullNoScaleV>)(static_cast<const ConvexHullNoScaleV&>(convexHull), transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)) : 
+	static_cast<SupportLocal*>(PX_PLACEMENT_NEW(buff, SupportLocalImpl<ConvexHullV>)(convexHull, transf1, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScale)));
+
+	PxU32 numContacts = 0;
+	if(generateSphereFullContactManifold(capsule, polyData, map, manifoldContacts, numContacts, contactDist, normal, doOverlapTest))
+	{
+
+		if(numContacts > 0)
+		{
+			
+			Gu::PersistentContact& p = manifold.getContactPoint(0);
+
+			p.mLocalPointA = manifoldContacts[0].mLocalPointA;
+			p.mLocalPointB = manifoldContacts[0].mLocalPointB;
+			p.mLocalNormalPen = manifoldContacts[0].mLocalNormalPen;
+			manifold.mNumContacts =1;
+
+			//transform normal to world space
+			const Vec3V worldNormal = transf1.rotate(normal);
+			const Vec3V worldP = V3NegScaleSub(worldNormal, capsule.radius, transf0.p);
+			const FloatV penDep = FSub(V4GetW(manifoldContacts[0].mLocalNormalPen), capsule.radius);
+
+#if	PCM_LOW_LEVEL_DEBUG
+			manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
+#endif
+
+			addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
+
+			return true;
+		}
+		
+	}
+
+	return false;
+}
+
+bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+
+	PX_ASSERT(transform1.q.isSane());
+	PX_ASSERT(transform0.q.isSane());
+	
+	
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+
+	Gu::PersistentContactManifold& manifold = cache.getManifold();
+
+	const Vec3V zeroV = V3Zero();
+
+	Ps::prefetchLine(shapeConvex.hullData);
+	const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const FloatV sphereRadius = FLoad(shapeSphere.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	const Gu::ConvexHullData* hullData = shapeConvex.hullData;
+	
+	//Transfer A into the local space of B
+	const PsTransformV transf0 = loadTransformA(transform0);
+	const PsTransformV transf1 = loadTransformA(transform1);
+	const PsTransformV curRTrans(transf1.transformInv(transf0));
+	const PsMatTransformV aToB(curRTrans);
+	
+	
+	const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
+
+	const PxU32 initialContacts = manifold.mNumContacts;
+	const FloatV minMargin = FMin(convexMargin, sphereRadius);
+	const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(0.05f));
+	
+	const FloatV refreshDistance = FAdd(sphereRadius, contactDist);
+	manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDistance);
+	//ML: after refreshContactPoints, we might lose some contacts
+	const bool bLostContacts = (manifold.mNumContacts != initialContacts);
+
+	GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
+
+		
+	Vec3V closestA(zeroV), closestB(zeroV);
+	Vec3V normal(zeroV); // from a to b
+	const FloatV zero = FZero();
+	FloatV penDep = zero;
+
+	PX_UNUSED(bLostContacts);
+
+
+	if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
+	{
+
+		manifold.setRelativeTransform(curRTrans);
+		
+		const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);  
+	
+		const bool idtScale = shapeConvex.scale.isIdentity();
+		//use the original shape
+		ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
+		convexHull.setMargin(zero);
+		//transform capsule into the local space of convexHull
+		CapsuleV capsule(aToB.p, sphereRadius);
+
+		LocalConvex<CapsuleV> convexA(capsule);
+		const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+		if(idtScale)
+		{
+			LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
+			status = gjkPenetration<LocalConvex<CapsuleV>,  LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, 
+				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
+		}
+		else
+		{
+			LocalConvex<ConvexHullV> convexB(convexHull);
+			status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
+				manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
+
+		}
+
+		if(status == GJK_NON_INTERSECT)
+		{
+			return false;
+		}
+		else if(status == GJK_CONTACT)
+		{
+			Gu::PersistentContact& p = manifold.getContactPoint(0);
+			p.mLocalPointA = zeroV;//sphere center
+			p.mLocalPointB = closestB;
+			p.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+			manifold.mNumContacts =1;
+
+#if	PCM_LOW_LEVEL_DEBUG
+			manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
+#endif
+			
+			//transform normal to world space
+			const Vec3V worldNormal = transf1.rotate(normal);
+			const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
+			penDep = FSub(penDep, sphereRadius);
+			addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
+			return true;
+
+		}
+		else if(status == GJK_DEGENERATE)
+		{
+			Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+			
+			return fullContactsGenerationSphereConvex(capsule, convexHull, transf0, transf1, manifoldContacts, contactBuffer, idtScale, 
+				manifold, normal, contactDist, true, renderOutput);
+		}
+		else if(status == EPA_CONTACT)
+		{
+			
+			if(idtScale)
+			{
+				LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
+				
+				status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+						closestA, closestB, normal, penDep, true);
+				
+			}
+			else
+			{
+				LocalConvex<ConvexHullV> convexB(convexHull);
+			
+				status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
+						closestA, closestB, normal, penDep, true);
+
+			}
+
+			if(status == EPA_CONTACT)
+			{
+				Gu::PersistentContact& p = manifold.getContactPoint(0);
+				p.mLocalPointA = zeroV;//sphere center
+				p.mLocalPointB = closestB;
+				p.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
+				manifold.mNumContacts =1;
+
+#if	PCM_LOW_LEVEL_DEBUG
+				manifold.drawManifold(*renderOutput, transf0, transf1, capsule.radius);
+#endif
+				
+				//transform normal to world space
+				const Vec3V worldNormal = transf1.rotate(normal);
+				const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
+				penDep = FSub(penDep, sphereRadius);
+
+				addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
+				return true;
+			}
+			else
+			{
+				Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
+				return fullContactsGenerationSphereConvex(capsule, convexHull, transf0, transf1, manifoldContacts,  contactBuffer, idtScale, 
+					manifold, normal, contactDist, true, renderOutput);
+
+			}
+			
+		}
+	}
+	else if(manifold.mNumContacts > 0)
+	{
+		//ML:: the manifold originally has contacts
+		Gu::PersistentContact& p = manifold.getContactPoint(0);
+		const Vec3V worldNormal = transf1.rotate(Vec3V_From_Vec4V(p.mLocalNormalPen));
+		const Vec3V worldP = V3NegScaleSub(worldNormal, sphereRadius, transf0.p);
+		penDep = FSub(V4GetW(p.mLocalNormalPen), sphereRadius);
+
+#if	PCM_LOW_LEVEL_DEBUG
+		manifold.drawManifold(*renderOutput, transf0, transf1, sphereRadius);
+#endif
+	
+		addToContactBuffer(contactBuffer, worldNormal, worldP, penDep);
+		return true;
+	}
+
+	return false;
+
+}  
+}//Gu
+}//phyxs
+
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereHeightField.cpp
new file mode 100644
index 00000000..cebb8142
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereHeightField.cpp
@@ -0,0 +1,155 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuVecBox.h"
+#include "GuVecShrunkBox.h"
+#include "GuVecConvexHull.h"
+#include "GuVecConvexHullNoScale.h"
+#include "GuVecTriangle.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuHeightField.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuPCMContactMeshCallback.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace physx::shdfnd::aos;
+
+namespace physx
+{
+
+struct PCMSphereVsHeightfieldContactGenerationCallback :  PCMHeightfieldContactGenerationCallback<PCMSphereVsHeightfieldContactGenerationCallback>
+{
+
+public:
+	PCMSphereVsMeshContactGeneration		mGeneration;
+
+	PCMSphereVsHeightfieldContactGenerationCallback(
+		const Ps::aos::Vec3VArg		sphereCenter,
+		const Ps::aos::FloatVArg	sphereRadius,
+		const Ps::aos::FloatVArg	contactDistance,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+	
+		const PsTransformV& sphereTransform, 
+		const PsTransformV& heightfieldTransform,
+		const PxTransform&	heightfieldTransform1,
+		Gu::MultiplePersistentContactManifold& multiManifold,
+		Gu::ContactBuffer& contactBuffer,
+		Gu::HeightFieldUtil& hfUtil 
+		
+		
+	) :
+		PCMHeightfieldContactGenerationCallback<PCMSphereVsHeightfieldContactGenerationCallback>(hfUtil, heightfieldTransform1),
+		mGeneration(sphereCenter, sphereRadius, contactDistance, replaceBreakingThreshold, sphereTransform, heightfieldTransform, multiManifold, contactBuffer)
+	{
+	}
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(Gu::TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMSphereVsMeshContactGeneration>(cache);
+	}
+
+};
+
+
+bool Gu::pcmContactSphereHeightField(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxHeightFieldGeometryLL& shapeHeight = shape1.get<const PxHeightFieldGeometryLL>();
+
+	Gu::MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&transform0.p.x);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV sphereRadius = FLoad(shapeSphere.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	
+	const PsTransformV sphereTransform(p0, q0);//sphere transform
+	const PsTransformV heightfieldTransform(p1, q1);//height feild
+	const PsTransformV curTransform = heightfieldTransform.transformInv(sphereTransform);
+	
+
+	// We must be in local space to use the cache
+
+	if(multiManifold.invalidate(curTransform, sphereRadius, FLoad(0.02f)))
+	{
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform);
+
+		const FloatV replaceBreakingThreshold = FMul(sphereRadius, FLoad(0.001f));
+		const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(shapeHeight.heightField);
+		Gu::HeightFieldUtil hfUtil(shapeHeight, hf);
+		const PxVec3 sphereCenterShape1Space = transform1.transformInv(transform0.p);
+		const Vec3V sphereCenter = V3LoadU(sphereCenterShape1Space);
+		PxReal inflatedRadius = shapeSphere.radius + params.mContactDistance;
+		PxVec3 inflatedRadiusV(inflatedRadius);
+
+		PxBounds3 bounds(sphereCenterShape1Space - inflatedRadiusV, sphereCenterShape1Space + inflatedRadiusV);
+
+		PCMSphereVsHeightfieldContactGenerationCallback blockCallback(
+			sphereCenter,
+			sphereRadius,
+			contactDist,
+			replaceBreakingThreshold,
+			sphereTransform,
+			heightfieldTransform,
+			transform1,
+			multiManifold,
+			contactBuffer,
+			hfUtil);
+
+		hfUtil.overlapAABBTriangles(transform1, bounds, 0, &blockCallback);
+
+		blockCallback.mGeneration.processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE, false);
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+		const FloatV projectBreakingThreshold = FMul(sphereRadius, FLoad(0.05f));
+		const FloatV refereshDistance = FAdd(sphereRadius, contactDist);
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
+
+	}
+
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, sphereTransform, heightfieldTransform, sphereRadius);
+}
+
+
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereMesh.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereMesh.cpp
new file mode 100644
index 00000000..430b7baa
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereMesh.cpp
@@ -0,0 +1,171 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "GuVecTriangle.h"
+#include "GuGeometryUnion.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuConvexUtilsInternal.h"
+#include "PxTriangleMesh.h"
+#include "GuContactBuffer.h"
+#include "GuTriangleCache.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuHeightFieldUtil.h"
+#include "GuPCMContactMeshCallback.h"
+#include "GuBox.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace physx::shdfnd::aos;
+
+namespace physx
+{
+
+struct PCMSphereVsMeshContactGenerationCallback : PCMMeshContactGenerationCallback< PCMSphereVsMeshContactGenerationCallback >
+{
+
+public:
+	PCMSphereVsMeshContactGeneration		mGeneration;
+	
+
+	PCMSphereVsMeshContactGenerationCallback(
+		const Ps::aos::Vec3VArg		sphereCenter,
+		const Ps::aos::FloatVArg	sphereRadius,
+		const Ps::aos::FloatVArg	contactDist,
+		const Ps::aos::FloatVArg	replaceBreakingThreshold,
+		const PsTransformV& sphereTransform,
+		const PsTransformV& meshTransform,
+		MultiplePersistentContactManifold& multiManifold,
+		ContactBuffer& contactBuffer,
+		const PxU8*		extraTriData,
+		const Cm::FastVertex2ShapeScaling& meshScaling,
+		bool idtMeshScale,
+		Cm::RenderOutput* renderOutput = NULL
+	) :
+		PCMMeshContactGenerationCallback<PCMSphereVsMeshContactGenerationCallback>(meshScaling, extraTriData, idtMeshScale), 
+		mGeneration(sphereCenter, sphereRadius, contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer, renderOutput)
+	{
+	}
+
+	PX_FORCE_INLINE bool doTest(const PxVec3&, const PxVec3&, const PxVec3&) { return true; }
+
+	template<PxU32 CacheSize>
+	void processTriangleCache(TriangleCache<CacheSize>& cache)
+	{
+		mGeneration.processTriangleCache<CacheSize, PCMSphereVsMeshContactGeneration>(cache);
+	}
+	
+};
+
+
+bool Gu::pcmContactSphereMesh(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+	MultiplePersistentContactManifold& multiManifold = cache.getMultipleManifold();
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	//gRenderOutPut = cache.mRenderOutput;
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&transform0.p.x);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&transform1.p.x);
+
+	const FloatV sphereRadius = FLoad(shapeSphere.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+	
+	const PsTransformV sphereTransform(p0, q0);//sphere transform
+	const PsTransformV meshTransform(p1, q1);//triangleMesh  
+	const PsTransformV curTransform = meshTransform.transformInv(sphereTransform);
+	
+	// We must be in local space to use the cache
+	if(multiManifold.invalidate(curTransform, sphereRadius, FLoad(0.02f)))
+	{
+		const FloatV replaceBreakingThreshold = FMul(sphereRadius, FLoad(0.001f));
+		const PxVec3 sphereCenterShape1Space = transform1.transformInv(transform0.p);
+		PxReal inflatedRadius = shapeSphere.radius + params.mContactDistance;
+
+		const Vec3V sphereCenter = V3LoadU(sphereCenterShape1Space);
+
+		const TriangleMesh* meshData = shapeMesh.meshData;
+
+		Cm::FastVertex2ShapeScaling meshScaling;	// PT: TODO: get rid of default ctor :(
+		const bool idtMeshScale = shapeMesh.scale.isIdentity();
+		if(!idtMeshScale)
+			meshScaling.init(shapeMesh.scale);
+
+		multiManifold.mNumManifolds = 0;
+		multiManifold.setRelativeTransform(curTransform); 
+
+		const PxU8* PX_RESTRICT extraData = meshData->getExtraTrigData();
+		// mesh scale is not baked into cached verts
+		PCMSphereVsMeshContactGenerationCallback callback(
+			sphereCenter,
+			sphereRadius,
+			contactDist,
+			replaceBreakingThreshold,
+			sphereTransform,
+			meshTransform,
+			multiManifold,
+			contactBuffer,
+			extraData,
+			meshScaling,
+			idtMeshScale);
+
+		PxVec3 obbCenter = sphereCenterShape1Space;
+		PxVec3 obbExtents = PxVec3(inflatedRadius);
+		PxMat33 obbRot(PxIdentity);
+		if(!idtMeshScale)
+			meshScaling.transformQueryBounds(obbCenter, obbExtents, obbRot);
+		const Box obb(obbCenter, obbExtents, obbRot);
+
+		Midphase::intersectOBB(meshData, obb, callback, true);
+
+		callback.flushCache();
+
+		callback.mGeneration.processContacts(GU_SPHERE_MANIFOLD_CACHE_SIZE, false);
+	}
+	else
+	{
+		const PsMatTransformV aToB(curTransform);
+		const FloatV projectBreakingThreshold = FMul(sphereRadius, FLoad(0.05f));
+		const FloatV refereshDistance = FAdd(sphereRadius, contactDist);
+		multiManifold.refreshManifold(aToB, projectBreakingThreshold, refereshDistance);
+	}
+	
+	//multiManifold.drawManifold(*gRenderOutPut, sphereTransform, meshTransform);
+	return multiManifold.addManifoldContactsToContactBuffer(contactBuffer, sphereTransform, meshTransform, sphereRadius);
+}
+
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSpherePlane.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSpherePlane.cpp
new file mode 100644
index 00000000..c9e6909d
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSpherePlane.cpp
@@ -0,0 +1,86 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuContactBuffer.h"
+#include "PsVecTransform.h"
+
+#include "GuGeometryUnion.h"
+#include "GuContactMethodImpl.h"
+
+namespace physx
+{
+
+namespace Gu
+{
+bool pcmContactSpherePlane(GU_CONTACT_METHOD_ARGS)
+{
+	using namespace Ps::aos;
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	PX_UNUSED(shape1);
+
+	// Get actual shape data
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+
+	//sphere transform
+	const Vec3V p0 = V3LoadU_SafeReadW(transform0.p);	// PT: safe because 'mRefCount' follows 'mTransform' in PxsTransform
+
+	//plane transform
+	const Vec3V p1 = V3LoadU_SafeReadW(transform1.p);	// PT: safe because 'mRefCount' follows 'mTransform' in PxsTransform
+	const QuatV q1 = QuatVLoadU(&transform1.q.x);
+
+	const FloatV radius = FLoad(shapeSphere.radius);
+	const FloatV contactDist = FLoad(params.mContactDistance);
+
+	const PsTransformV transf1(p1, q1);
+	//Sphere in plane space
+	const Vec3V sphereCenterInPlaneSpace = transf1.transformInv(p0);
+	
+
+	//Separation
+	const FloatV separation = FSub(V3GetX(sphereCenterInPlaneSpace), radius);
+
+	if(FAllGrtrOrEq(contactDist, separation))
+	{
+		//get the plane normal
+		const Vec3V worldNormal = QuatGetBasisVector0(q1);
+		const Vec3V worldPoint = V3NegScaleSub(worldNormal, radius, p0);
+		Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+		//Fast allign store
+		V4StoreA(Vec4V_From_Vec3V(worldNormal), &contact.normal.x);
+		V4StoreA(Vec4V_From_Vec3V(worldPoint), &contact.point.x);
+		FStore(separation, &contact.separation);
+		contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+	
+		return true;
+	}
+	return false;
+}
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereSphere.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereSphere.cpp
new file mode 100644
index 00000000..021fd9b3
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactSphereSphere.cpp
@@ -0,0 +1,86 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGeometryUnion.h"
+
+#include "GuContactBuffer.h"
+#include "GuContactMethodImpl.h"
+#include "PsVecTransform.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool pcmContactSphereSphere(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(renderOutput);
+
+	using namespace Ps::aos;
+	const PxSphereGeometry& shapeSphere0 = shape0.get<const PxSphereGeometry>();
+	const PxSphereGeometry& shapeSphere1 = shape1.get<const PxSphereGeometry>();
+	
+	const FloatV cDist	= FLoad(params.mContactDistance);
+	const Vec3V p0 =  V3LoadA(&transform0.p.x);
+	const Vec3V p1 =  V3LoadA(&transform1.p.x);
+
+	const FloatV r0		= FLoad(shapeSphere0.radius);
+	const FloatV r1		= FLoad(shapeSphere1.radius);
+	
+
+	const Vec3V _delta = V3Sub(p0, p1);
+	const FloatV distanceSq = V3Dot(_delta, _delta);
+	const FloatV radiusSum = FAdd(r0, r1);
+	const FloatV inflatedSum = FAdd(radiusSum, cDist);
+	
+	if(FAllGrtr(FMul(inflatedSum, inflatedSum), distanceSq))
+	{
+		const FloatV eps	=  FLoad(0.00001f);
+		const FloatV nhalf	= FLoad(-0.5f);
+		const FloatV magn = FSqrt(distanceSq);
+		const BoolV bCon = FIsGrtrOrEq(eps, magn);
+		const Vec3V normal = V3Sel(bCon, V3UnitX(), V3ScaleInv(_delta, magn)); 
+		const FloatV scale = FMul(FSub(FAdd(r0, magn), r1), nhalf);
+		const Vec3V point = V3ScaleAdd(normal, scale, p0);
+		const FloatV dist =  FSub(magn, radiusSum);
+		
+		PX_ASSERT(contactBuffer.count < ContactBuffer::MAX_CONTACTS);
+		Gu::ContactPoint& contact = contactBuffer.contacts[contactBuffer.count++];
+		V4StoreA(Vec4V_From_Vec3V(normal), &contact.normal.x);
+		V4StoreA(Vec4V_From_Vec3V(point), &contact.point.x);
+		FStore(dist, &contact.separation);
+
+		contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+
+		return true;
+	}
+	return false;
+}
+}//Gu
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.cpp
new file mode 100644
index 00000000..a95c46fa
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.cpp
@@ -0,0 +1,210 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGeometryUnion.h"
+#include "GuPCMShapeConvex.h"
+#include "GuVecConvexHull.h"
+#include "GuPCMContactGen.h"
+#include "CmRenderOutput.h"
+
+using namespace physx;
+using namespace Gu;
+
+namespace physx
+{
+namespace Gu
+{
+	const PxU8 gPCMBoxPolygonData[24] = 
+	{
+		0, 3, 2, 1,
+		1, 2, 6, 5,
+		5, 6, 7, 4,
+		4, 7, 3, 0,
+		3, 7, 6, 2,
+		4, 0, 1, 5,
+	};
+
+
+   Gu::PCMPolygonalBox::PCMPolygonalBox(const PxVec3& halfSide) : mHalfSide(halfSide)
+	{
+		//Precompute the convex data
+		//     7+------+6			0 = ---
+		//     /|     /|			1 = +--
+		//    / |    / |			2 = ++-
+		//   / 4+---/--+5			3 = -+-
+		// 3+------+2 /    y   z	4 = --+
+		//  | /    | /     |  /		5 = +-+
+		//  |/     |/      |/		6 = +++
+		// 0+------+1      *---x	7 = -++
+
+		PxVec3 minimum = -mHalfSide;
+		PxVec3 maximum = mHalfSide;
+		// Generate 8 corners of the bbox
+		mVertices[0] = PxVec3(minimum.x, minimum.y, minimum.z);
+		mVertices[1] = PxVec3(maximum.x, minimum.y, minimum.z);
+		mVertices[2] = PxVec3(maximum.x, maximum.y, minimum.z);
+		mVertices[3] = PxVec3(minimum.x, maximum.y, minimum.z);
+		mVertices[4] = PxVec3(minimum.x, minimum.y, maximum.z);
+		mVertices[5] = PxVec3(maximum.x, minimum.y, maximum.z);
+		mVertices[6] = PxVec3(maximum.x, maximum.y, maximum.z);
+		mVertices[7] = PxVec3(minimum.x, maximum.y, maximum.z);
+
+		//Setup the polygons
+		for(PxU8 i=0; i < 6; i++)
+		{
+			mPolygons[i].mNbVerts = 4;
+			mPolygons[i].mVRef8 = PxU16(i*4);
+		}
+
+		// ### planes needs *very* careful checks
+		// X axis
+		mPolygons[1].mPlane.n = PxVec3(1.0f, 0.0f, 0.0f);
+		mPolygons[1].mPlane.d = -mHalfSide.x;
+		mPolygons[3].mPlane.n = PxVec3(-1.0f, 0.0f, 0.0f);
+		mPolygons[3].mPlane.d = -mHalfSide.x;
+		
+		mPolygons[1].mMinIndex = 0;
+		mPolygons[3].mMinIndex = 1;
+
+		PX_ASSERT(mPolygons[1].getMin(mVertices) == -mHalfSide.x); 
+		PX_ASSERT(mPolygons[3].getMin(mVertices) == -mHalfSide.x);
+
+
+		// Y axis
+		mPolygons[4].mPlane.n = PxVec3(0.f, 1.0f, 0.0f);
+		mPolygons[4].mPlane.d = -mHalfSide.y;
+		mPolygons[5].mPlane.n = PxVec3(0.0f, -1.0f, 0.0f);
+		mPolygons[5].mPlane.d = -mHalfSide.y;
+
+		mPolygons[4].mMinIndex = 0;
+		mPolygons[5].mMinIndex = 2;
+
+
+		PX_ASSERT(mPolygons[4].getMin(mVertices) == -mHalfSide.y); 
+		PX_ASSERT(mPolygons[5].getMin(mVertices) == -mHalfSide.y);
+
+		// Z axis
+		mPolygons[2].mPlane.n = PxVec3(0.f, 0.0f, 1.0f);
+		mPolygons[2].mPlane.d = -mHalfSide.z;
+		mPolygons[0].mPlane.n = PxVec3(0.0f, 0.0f, -1.0f);
+		mPolygons[0].mPlane.d = -mHalfSide.z;
+
+		mPolygons[2].mMinIndex = 0;
+		mPolygons[0].mMinIndex = 4;
+		PX_ASSERT(mPolygons[2].getMin(mVertices) == -mHalfSide.z); 
+		PX_ASSERT(mPolygons[0].getMin(mVertices) == -mHalfSide.z);
+	}
+
+   void Gu::PCMPolygonalBox::getPolygonalData(Gu::PolygonalData* PX_RESTRICT dst) const
+	{
+		dst->mCenter				= PxVec3(0.0f, 0.0f, 0.0f);
+		dst->mNbVerts				= 8;
+		dst->mNbPolygons			= 6;
+		dst->mPolygons				= mPolygons;
+		dst->mNbEdges				= 0;
+		dst->mVerts					= mVertices;
+		dst->mPolygonVertexRefs		= gPCMBoxPolygonData;
+		dst->mFacesByEdges			= NULL;
+		dst->mVerticesByEdges		= NULL;
+		dst->mInternal.mRadius		= 0.0f;
+		dst->mInternal.mExtents[0]	= 0.0f;
+		dst->mInternal.mExtents[1]	= 0.0f;
+		dst->mInternal.mExtents[2]	= 0.0f;
+	}
+
+	static void getPCMPolygonalData_Convex(Gu::PolygonalData* PX_RESTRICT dst, const Gu::ConvexHullData* PX_RESTRICT src, const Ps::aos::Mat33V& vertexToShape)
+	{
+		using namespace Ps::aos;
+		const Vec3V vertexSpaceCenterOfMass = V3LoadU(src->mCenterOfMass);
+		const Vec3V shapeSpaceCenterOfMass = M33MulV3(vertexToShape, vertexSpaceCenterOfMass);
+		V3StoreU(shapeSpaceCenterOfMass, dst->mCenter);
+		dst->mNbVerts			= src->mNbHullVertices;
+		dst->mNbPolygons		= src->mNbPolygons;
+		dst->mNbEdges			= src->mNbEdges;
+		dst->mPolygons			= src->mPolygons;
+		dst->mVerts				= src->getHullVertices();
+		dst->mPolygonVertexRefs	= src->getVertexData8();
+		dst->mFacesByEdges		= src->getFacesByEdges8();
+		dst->mVerticesByEdges   = src->getVerticesByEdges16();
+
+		dst->mBigData			= src->mBigConvexRawData;
+
+		dst->mInternal			= src->mInternal;
+	}
+
+
+	static void getPCMPolygonalData_Convex(Gu::PolygonalData* PX_RESTRICT dst, const Gu::ConvexHullData* PX_RESTRICT src, const Cm::FastVertex2ShapeScaling& scaling)
+	{
+		dst->mCenter			= scaling * src->mCenterOfMass;
+		dst->mNbVerts			= src->mNbHullVertices;
+		dst->mNbPolygons		= src->mNbPolygons;
+		dst->mNbEdges			= src->mNbEdges;
+		dst->mPolygons			= src->mPolygons;
+		dst->mVerts				= src->getHullVertices();
+		dst->mPolygonVertexRefs	= src->getVertexData8();
+		dst->mFacesByEdges		= src->getFacesByEdges8();
+		dst->mVerticesByEdges   = src->getVerticesByEdges16();
+
+		dst->mBigData			= src->mBigConvexRawData;
+		dst->mInternal			= src->mInternal;
+	}
+
+
+	void getPCMConvexData(const Gu::ConvexHullV& convexHull, const bool idtScale, PolygonalData& polyData)
+	{
+	
+		PX_ASSERT(!convexHull.hullData->mAABB.isEmpty());  
+
+		//this is used to calculate the convex hull's center of mass
+		getPCMPolygonalData_Convex(&polyData, convexHull.hullData, convexHull.vertex2Shape);
+
+		if(!idtScale)
+			polyData.mInternal.reset();
+
+	}
+     
+	bool getPCMConvexData(const Gu::GeometryUnion& shape, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, PolygonalData& polyData)
+	{
+		const PxConvexMeshGeometryLL& shapeConvex = shape.get<const PxConvexMeshGeometryLL>();
+
+		const bool idtScale = shapeConvex.scale.isIdentity();
+		if(!idtScale)
+			scaling.init(shapeConvex.scale);
+
+		PX_ASSERT(!shapeConvex.hullData->mAABB.isEmpty());
+		bounds = shapeConvex.hullData->mAABB.transformFast(scaling.getVertex2ShapeSkew());
+
+		getPCMPolygonalData_Convex(&polyData, shapeConvex.hullData, scaling);
+
+		return idtScale;
+	}
+
+}
+}
+	
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.h
new file mode 100644
index 00000000..f69c2501
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMShapeConvex.h
@@ -0,0 +1,68 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_SHAPE_CONVEX_H
+#define GU_PCM_SHAPE_CONVEX_H
+
+#include "GuConvexSupportTable.h"
+#include "GuPersistentContactManifold.h"
+#include "GuShapeConvex.h"
+
+
+namespace physx
+{
+
+namespace Gu
+{
+	class GeometryUnion;
+
+	extern const PxU8 gPCMBoxPolygonData[24];
+	
+	class PCMPolygonalBox
+	{
+	public:
+									PCMPolygonalBox(const PxVec3& halfSide);
+
+									void					getPolygonalData(Gu::PolygonalData* PX_RESTRICT dst)	const;
+
+			const PxVec3&			mHalfSide;
+			PxVec3					mVertices[8];
+			Gu::HullPolygonData		mPolygons[6];
+	private:
+			PCMPolygonalBox& operator=(const PCMPolygonalBox&);
+	};
+
+
+	void getPCMConvexData(const Gu::ConvexHullV& convexHull,  const bool idtScale, Gu::PolygonalData& polyData);  
+	bool getPCMConvexData(const Gu::GeometryUnion& shape, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, Gu::PolygonalData& polyData);
+
+}
+}
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
new file mode 100644
index 00000000..ea400b94
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.cpp
@@ -0,0 +1,1250 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "GuGeometryUnion.h"
+#include "GuPCMTriangleContactGen.h"
+#include "GuPCMContactConvexCommon.h"
+#include "GuVecTriangle.h"
+#include "GuBarycentricCoordinates.h"
+
+#if	PCM_LOW_LEVEL_DEBUG
+#include "PxRenderBuffer.h"
+#endif
+
+#define EDGE_EDGE_GAUSS_MAP  0
+#define BRUTE_FORCE_EDGE_EDGE	0
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+namespace physx
+{
+	static bool testPolyFaceNormal(const Gu::TriangleV& triangle, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap, SupportLocal* polyMap,  const FloatVArg contactDist, 
+		FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, const FeatureStatus faceStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(triangle);
+
+		FloatV _minOverlap = FMax();
+		PxU32  _feature = 0;
+		Vec3V  _faceNormal = faceNormal;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		const FloatV eps = FEps();
+
+		if(polyMap->isIdentityScale)
+		{
+			//in the local space of polyData0
+			for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
+			{
+				const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+
+				const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				const FloatV planeDist = FLoad(polygon.mPlane.d);
+				//shapeSpace and vertexSpace are the same
+				const Vec3V planeNormal = V3LoadU_SafeReadW(polygon.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+	
+				//ML::avoid lHS, don't use the exiting function
+				min0 = V3Dot(planeNormal, minVert);
+				max0 = FNeg(planeDist);
+
+				triMap->doSupport(planeNormal, min1, max1);
+
+				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+				if(BAllEqTTTT(con))
+					return false;
+
+				const FloatV tempOverlap = FSub(max0, min1);
+
+				if(FAllGrtr(_minOverlap, tempOverlap))
+				{
+					_minOverlap = tempOverlap;
+					_feature = i;
+					_faceNormal = planeNormal;
+				}
+			}   
+		}
+		else
+		{
+		
+			//in the local space of polyData0
+			for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
+			{
+				const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+
+				const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				const FloatV planeDist = FLoad(polygon.mPlane.d);
+				const Vec3V vertexSpacePlaneNormal = V3LoadU_SafeReadW(polygon.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
+				//transform plane n to shape space
+				const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(polyMap->shape2Vertex, vertexSpacePlaneNormal);
+
+				const FloatV magnitude = FRsqrtFast(V3LengthSq(shapeSpacePlaneNormal)); //FRecip(V3Length(shapeSpacePlaneNormal));
+
+				//ML::avoid lHS, don't use the exiting function
+				min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
+				max0 = FMul(FNeg(planeDist), magnitude);
+
+				//normalize the shapeSpacePlaneNormal
+				const Vec3V planeN = V3Scale(shapeSpacePlaneNormal, magnitude);
+
+				triMap->doSupport(planeN, min1, max1);
+
+				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+				if(BAllEqTTTT(con))
+					return false;
+
+				const FloatV tempOverlap = FSub(max0, min1);
+
+				if(FAllGrtr(_minOverlap, tempOverlap))
+				{
+					_minOverlap = tempOverlap;
+					_feature = i;
+					_faceNormal = planeN;
+				}
+			} 
+		}
+
+		if(FAllGrtr(minOverlap, FAdd(_minOverlap, eps)))
+		{
+			faceNormal = _faceNormal;
+			minOverlap = _minOverlap;
+			status = faceStatus;
+		}
+
+		feature = _feature;
+
+		return true;
+
+	}
+
+
+
+	//triangle is in the local space of polyData
+	static bool testTriangleFaceNormal(const TriangleV& triangle, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap, SupportLocal* polyMap, const FloatVArg contactDist, 
+		FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, const FeatureStatus faceStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(triMap);
+		PX_UNUSED(polyData);
+
+		FloatV min1, max1;
+		const FloatV eps = FEps();
+
+		const Vec3V triangleLocNormal = triangle.normal();
+
+		const FloatV min0 = V3Dot(triangleLocNormal, triangle.verts[0]);
+		const FloatV max0 = min0;
+
+		//triangle normal is in the vertex space
+		polyMap->doSupport(triangleLocNormal, min1, max1);
+
+		const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+		if(BAllEqTTTT(con))
+			return false;
+
+		minOverlap = FSub(FSub(max0, min1), eps);
+		status = faceStatus;
+		feature = 0;
+		faceNormal=triangleLocNormal;
+
+		return true;
+
+	}
+
+	static bool testPolyEdgeNormal(const TriangleV& triangle, const PxU8 triFlags, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap, SupportLocal* polyMap, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& minNormal, const FeatureStatus edgeStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(triFlags);
+		FloatV overlap = minOverlap;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		const FloatV zero = FZero();
+		const Vec3V eps2 = V3Splat(FLoad(1e-6));
+		
+		const Vec3V v0 = M33MulV3(polyMap->shape2Vertex, triangle.verts[0]);
+		const Vec3V v1 = M33MulV3(polyMap->shape2Vertex, triangle.verts[1]);
+		const Vec3V v2 = M33MulV3(polyMap->shape2Vertex, triangle.verts[2]);
+
+		TriangleV vertexSpaceTriangle(v0, v1, v2);
+
+		PxU32 nbTriangleAxes = 0;
+		Vec3V triangleAxes[3];
+		for(PxI8 kStart = 0, kEnd =2; kStart<3; kEnd = kStart++)
+		{
+			bool active = (triFlags & (1 << (kEnd+3))) != 0;
+	
+			if(active)
+			{
+				const Vec3V p00 = vertexSpaceTriangle.verts[kStart];
+				const Vec3V p01 = vertexSpaceTriangle.verts[kEnd];
+				triangleAxes[nbTriangleAxes++] =  V3Sub(p01, p00);
+			}
+		}
+
+		if(nbTriangleAxes == 0)
+			return true;
+
+		//create localTriPlane in the vertex space
+		const Vec3V vertexSpaceTriangleNormal = vertexSpaceTriangle.normal();
+	
+		for(PxU32 i =0; i<polyData.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+			const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
+			const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);
+
+			//fast culling. 
+			if(FAllGrtr(V3Dot(vertexSpacePlaneNormal, vertexSpaceTriangleNormal), zero))
+				continue;
+
+			// Loop through polygon vertices == polygon edges;
+			for(PxU32 lStart = 0, lEnd =PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = PxU32(lStart++))
+			{
+				//in the vertex space
+				const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds[lStart]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds[lEnd]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+
+				const Vec3V convexEdge = V3Sub(p11, p10);
+
+				for (PxU32 j = 0; j < nbTriangleAxes; ++j)
+				{
+
+					const Vec3V currentPolyEdge = triangleAxes[j];
+					const Vec3V v = V3Cross(convexEdge, currentPolyEdge);
+
+					//two edges aren't parallel
+					if ((!V3AllGrtr(eps2, V3Abs(v))) && (FAllGrtr(V3Dot(v, vertexSpaceTriangleNormal), zero)))
+					{
+						//transform the v back to the shape space
+						const Vec3V shapeSpaceV = M33TrnspsMulV3(polyMap->shape2Vertex, v);
+						const Vec3V n0 = V3Normalize(shapeSpaceV);
+						triMap->doSupport(n0, min0, max0);
+						polyMap->doSupport(n0, min1, max1);
+						const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+						if (BAllEqTTTT(con))
+							return false;
+
+						const FloatV tempOverlap = FSub(max0, min1);
+
+						if (FAllGrtr(overlap, tempOverlap))
+						{
+							overlap = tempOverlap;
+							minNormal = n0;
+							status = edgeStatus;
+						}
+
+					}
+				}
+				
+			}
+		}
+		minOverlap = overlap;
+		
+		return true;
+
+	}
+
+#if BRUTE_FORCE_EDGE_EDGE
+
+
+	bool testPolyEdgeNormalBruteForce(const TriangleV& triangle, const PxU8 triFlags, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap, SupportLocal* polyMap, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& minNormal, const FeatureStatus edgeStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(triFlags);
+		FloatV min0, max0;
+		FloatV min1, max1;
+
+		FloatV bestDist = FLoad(PX_MAX_F32);
+		Vec3V bestAxis = V3Zero();
+
+		const Vec3V eps2 = V3Splat(FLoad(1e-6));
+
+		PxU32 bestPolyIndex = 0;
+		PxU32 bestStart = 0;
+		PxU32 bestEnd = 0;
+		PxI8 bestTriStart = 0;
+		PxI8 bestTriEnd = 0;
+
+		for(PxU32 i =0; i<polyData.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+			const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
+			
+			// Loop through polygon vertices == polygon edges;
+			for(PxU32 lStart = 0, lEnd =PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = PxU32(lStart++))
+			{
+				//in the vertex space
+				const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds[lStart]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds[lEnd]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+
+				//shape sapce
+				const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, p10);
+				const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, p11);
+
+				const Vec3V convexEdge = V3Sub(vertex11, vertex10);
+
+				for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
+				{
+
+					const Vec3V triVert0 = triangle.verts[kStart];
+					const Vec3V triVert1 = triangle.verts[kEnd];
+
+					const Vec3V triangleEdge = V3Sub(triVert1, triVert0);
+					const Vec3V v = V3Cross(convexEdge, triangleEdge);
+
+					if (!V3AllGrtr(eps2, V3Abs(v)))
+					{
+						//transform the v back to the shape space
+						const Vec3V n0 = V3Normalize(v);
+						triMap->doSupport(n0, min0, max0);
+						polyMap->doSupport(n0, min1, max1);
+						const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+						if(BAllEqTTTT(con))
+							return false;
+
+						const FloatV tempOverlap = FSub(max0, min1);
+
+						if (FAllGrtr(bestDist, tempOverlap))
+						{
+							bestDist = tempOverlap;
+							bestAxis = n0;
+							bestPolyIndex = i;
+							bestStart = lStart;
+							bestEnd = lEnd;
+							bestTriStart = kStart;
+							bestTriEnd = kEnd;
+						}
+
+					}
+				}
+			}
+		}
+
+		if (FAllGrtr(minOverlap, bestDist))
+		{
+			minOverlap = bestDist;
+			minNormal = bestAxis;
+			status = edgeStatus;
+		}
+
+		return true;
+
+	}
+
+	bool testPolyEdgeNormalBruteForceVertsByEdges(const TriangleV& triangle, const PxU8 triFlags, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap, SupportLocal* polyMap, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& minNormal, const FeatureStatus edgeStatus, FeatureStatus& status, PxU32& bestEdgeIndex, PxI8& bestTriStart, PxI8& bestTriEnd)
+	{
+
+		PX_UNUSED(triFlags);
+
+		const PxU32 numConvexEdges = polyData.mNbEdges;
+		const PxU16* verticesByEdges16 = polyData.mVerticesByEdges;
+		const PxVec3* vertices = polyData.mVerts;
+
+		FloatV bestDist = FLoad(PX_MAX_F32);
+		Vec3V bestAxis = V3Zero();
+		const Vec3V eps2 = V3Splat(FLoad(1e-6));
+
+		for (PxU32 convexEdgeIdx = 0; convexEdgeIdx < numConvexEdges; ++convexEdgeIdx)
+		{
+			const PxU16 v0idx1 = verticesByEdges16[convexEdgeIdx * 2];
+			const PxU32 v1idx1 = verticesByEdges16[convexEdgeIdx * 2 + 1];
+
+			//shape sapce
+			const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v0idx1]));
+			const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v1idx1]));
+
+			Vec3V convexEdge = V3Sub(vertex11, vertex10);
+
+
+			for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
+			{
+				const Vec3V triVert0 = triangle.verts[kStart];
+				const Vec3V triVert1 = triangle.verts[kEnd];
+				const Vec3V triEdge = V3Sub(triVert1, triVert0);
+
+				// compute the separation along this axis in vertex space
+				Vec3V axis = V3Cross(convexEdge, triEdge);
+
+				if (!V3AllGrtr(eps2, V3Abs(axis)))
+				{
+					axis = V3Normalize(axis);
+
+					FloatV min0, max0;
+					FloatV min1, max1;
+					triMap->doSupport(axis, min0, max0);
+					polyMap->doSupport(axis, min1, max1);
+					const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+					if (BAllEqTTTT(con))
+						return false;
+
+					const FloatV dist0 = FSub(max0, min1);
+					const FloatV dist1 = FSub(max1, min0);
+
+					if (FAllGrtr(dist0, dist1))
+						axis = V3Neg(axis);
+
+					const FloatV dist = FMin(dist0, dist1);
+
+					if (FAllGrtr(bestDist, dist))
+					{
+						bestDist = dist;
+						bestAxis = axis;
+
+						bestEdgeIndex = convexEdgeIdx;
+						bestTriStart = kStart;
+						bestTriEnd = kEnd;
+					}
+				}
+			}
+
+		}
+
+		if (FAllGrtr(minOverlap, bestDist))
+		{
+			minOverlap = bestDist;
+			minNormal = bestAxis;
+			status = edgeStatus;
+
+		}
+		return true;
+
+	}
+
+
+#endif
+
+#if EDGE_EDGE_GAUSS_MAP
+
+
+
+	bool isMinkowskiFace(const Vec3V& A, const Vec3V& B, const Vec3V& B_x_A, const Vec3V& C, const Vec3V& D, const Vec3V& D_x_C)
+	{
+
+		const FloatV zero = FZero();
+		// Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. 
+		// The associated arcs are defined by the adjacent face normals of each edge.  
+		const FloatV CBA = V3Dot(C, B_x_A);
+		const FloatV DBA = V3Dot(D, B_x_A);
+		const FloatV ADC = V3Dot(A, D_x_C);
+		const FloatV BDC = V3Dot(B, D_x_C);
+
+		const BoolV con0 = FIsGrtrOrEq(zero, FMul(CBA, DBA));
+		const BoolV con1 = FIsGrtrOrEq(zero, FMul(ADC, BDC));
+		const BoolV con2 = FIsGrtr(FMul(CBA, BDC), zero);
+
+		const BoolV con = BAnd(con2, BAnd(con0, con1));
+
+		return (BAllEqTTTT(con) != 0);
+
+		//return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
+	}
+
+#define SAT_VARIFY	0
+
+	bool testPolyEdgeNormalGaussMap(const TriangleV& triangle, const PxU8 triFlags, const PolygonalData& polyData, SupportLocalImpl<TriangleV>* triMap,  SupportLocal* polyMap, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& minNormal, const FeatureStatus edgeStatus, FeatureStatus& status, PxU32& gaussMapBestEdgeIndex, PxI8& gaussMapBestTriStart, PxI8& gaussMapBestTriEnd)
+	{
+	
+		PX_UNUSED(triFlags);
+		const FloatV zero = FZero();
+
+		const Vec3V triNormal = triangle.normal();
+
+		const PxU32 numConvexEdges = polyData.mNbEdges;
+		const PxU16* verticesByEdges16 = polyData.mVerticesByEdges;
+		const PxU8* facesByEdges8 = polyData.mFacesByEdges;
+		const PxVec3* vertices = polyData.mVerts;
+
+		FloatV bestDist = FLoad(-PX_MAX_F32);
+		Vec3V axis = V3Zero();
+		Vec3V bestAxis = V3Zero();
+		const Vec3V eps2 = V3Splat(FLoad(1e-6));
+
+
+		//Center is in shape space
+		const Vec3V shapeSpaceCOM =V3LoadU(polyData.mCenter);
+		const Vec3V vertexSpaceCOM = M33MulV3(polyMap->shape2Vertex, shapeSpaceCOM);
+
+		PxVec3 vertexCOM;
+		V3StoreU(vertexSpaceCOM, vertexCOM);
+
+		for (PxU32 convexEdgeIdx = 0; convexEdgeIdx < numConvexEdges; ++convexEdgeIdx)
+		{
+
+			const PxU16 v0idx1 = verticesByEdges16[convexEdgeIdx*2];
+			const PxU32 v1idx1 = verticesByEdges16[convexEdgeIdx * 2 + 1];
+
+			const PxU8 f10 = facesByEdges8[convexEdgeIdx * 2];
+			const PxU8 f11 = facesByEdges8[convexEdgeIdx * 2 + 1];
+
+			//shape sapce
+			const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v0idx1]));
+			const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v1idx1]));
+
+			Vec3V convexEdge = V3Sub(vertex11, vertex10);
+
+			const Gu::HullPolygonData& face0 = polyData.mPolygons[f10];
+			const Gu::HullPolygonData& face1 = polyData.mPolygons[f11];
+
+			const Vec3V convexNormal0 = M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(face0.mPlane.n));
+			const Vec3V convexNormal1 = M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(face1.mPlane.n));
+
+			float signDist0 = face0.mPlane.distance(vertexCOM);
+			float signDist1 = face1.mPlane.distance(vertexCOM);
+
+			PX_ASSERT(signDist0 < 0.f);
+			PX_ASSERT(signDist1 < 0.f);
+
+			for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
+			{ 
+				
+				const Vec3V triVert0 = triangle.verts[kStart];
+				const Vec3V triVert1 = triangle.verts[kEnd];
+				const Vec3V triEdge = V3Sub(triVert1, triVert0);
+
+				//if (isMinkowskiFace(convexNormal0, convexNormal1, V3Neg(convexEdge), V3Neg(triNormal), triNormal, V3Neg(triEdge)))
+				if (isMinkowskiFace(convexNormal0, convexNormal1, convexEdge, V3Neg(triNormal), triNormal, triEdge))
+				{
+
+					// compute the separation along this axis in vertex space
+					axis = V3Cross(convexEdge, triEdge);
+
+					if (!V3AllGrtr(eps2, V3Abs(axis)))
+					{
+						axis = V3Normalize(axis);
+
+						const Vec3V v = V3Sub(vertex10, shapeSpaceCOM);
+
+						// ensure the axis is outward pointing on the edge on the minkowski sum. Assure normal points from convex hull to triangle
+						const FloatV temp = V3Dot(axis, v);
+						if (FAllGrtr(zero, temp))
+							axis = V3Neg(axis);
+
+						//compute the distance from any of the verts in the triangle to plane(axis, vertex10)(n.dot(p-a))
+						const Vec3V ap = V3Sub(triVert0, vertex10);
+
+						const FloatV dist = V3Dot(axis, ap);
+
+						if (FAllGrtr(dist, contactDist))
+							return false;
+
+#if SAT_VARIFY
+						FloatV min0, max0;
+						FloatV min1, max1;
+						triMap->doSupport(V3Neg(axis), min0, max0);
+						polyMap->doSupport(V3Neg(axis), min1, max1);
+						const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+						if (BAllEqTTTT(con))
+							return false;
+
+						/*const FloatV dist = FSub(max0, min1);
+
+						if (FAllGrtr(bestDist, dist))
+						{
+						bestDist = dist;
+						bestAxis = axis;
+						}*/
+						const FloatV tempDist = FSub(max0, min1);
+
+						const FloatV dif = FAbs(FAdd(tempDist, dist));
+						PX_UNUSED(dif);
+						PX_ASSERT(FAllGrtr(FLoad(1e-4f), dif));
+
+#endif
+
+						if (FAllGrtr(dist, bestDist))
+						{
+							bestDist = dist;
+							bestAxis = axis;
+
+							gaussMapBestEdgeIndex = convexEdgeIdx;
+							gaussMapBestTriStart = kStart;
+							gaussMapBestTriEnd = kEnd;
+						}
+					}
+				}
+			}
+			
+		}
+
+		if (FAllGrtr(minOverlap, bestDist))
+		{
+			minOverlap = bestDist;
+			minNormal = bestAxis;
+			status = edgeStatus;
+		}
+		return true;
+			
+	}
+
+#endif
+
+	static PX_FORCE_INLINE PxU32 addMeshContacts(MeshPersistentContact* manifoldContacts, const Vec3V& pA, const Vec3V& pB, const Vec4V& normalPen, const PxU32 triangleIndex, const PxU32 numContacts)
+	{
+		manifoldContacts[numContacts].mLocalPointA = pA;
+		manifoldContacts[numContacts].mLocalPointB = pB;
+		manifoldContacts[numContacts].mLocalNormalPen = normalPen;
+		manifoldContacts[numContacts].mFaceIndex = triangleIndex;
+		return numContacts+1;
+	}
+
+
+	static void generatedTriangleContacts(const Gu::TriangleV& triangle, const PxU32 triangleIndex, const PxU8/* _triFlags*/, const Gu::PolygonalData& polyData1, const Gu::HullPolygonData& incidentPolygon,  Gu::SupportLocal* map1, Gu::MeshPersistentContact* manifoldContacts, PxU32& numManifoldContacts, 
+		const Ps::aos::FloatVArg contactDist, const Ps::aos::Vec3VArg contactNormal, Cm::RenderOutput* renderOutput)
+	{
+
+		PX_UNUSED(renderOutput);
+		using namespace Ps::aos;
+
+		//PxU8 triFlags = _triFlags;
+		const PxU32 previousContacts = numManifoldContacts;
+		
+		const FloatV zero = FZero();
+	
+		const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);
+
+		const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;
+
+		Vec3V points0In0[3];
+		Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
+		FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));
+		bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));
+		
+
+		points0In0[0] = triangle.verts[0];
+		points0In0[1] = triangle.verts[1];
+		points0In0[2] = triangle.verts[2];
+
+
+
+		//Transform all the verts from vertex space to shape space
+		map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);
+
+#if PCM_LOW_LEVEL_DEBUG
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, incidentPolygon.mNbVerts, (PxU32)PxDebugColor::eARGB_RED);
+		//Gu::PersistentContactManifold::drawTriangle(*renderOutput, map1->transform.transform(points1In0[0]), map1->transform.transform(points0In0[1]), map1->transform.transform(points0In0[2]), (PxU32)PxDebugColor::eARGB_BLUE);
+#endif
+ 
+		Vec3V eps = Vec3V_From_FloatV(FEps());
+		Vec3V max = Vec3V_From_FloatV(FMax());
+		Vec3V nmax = V3Neg(max); 
+
+		//transform reference polygon to 2d, calculate min and max
+		Vec3V rPolygonMin= max;
+		Vec3V rPolygonMax = nmax;
+		for(PxU32 i=0; i<3; ++i)
+		{
+			points0In0[i] = M33MulV3(rot, points0In0[i]);
+			rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
+			rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
+		}
+		
+		
+		rPolygonMin = V3Sub(rPolygonMin, eps);
+		rPolygonMax = V3Add(rPolygonMax, eps);
+
+		const FloatV d = V3GetZ(points0In0[0]);
+		const FloatV rd = FAdd(d, contactDist);
+
+		Vec3V iPolygonMin= max; 
+		Vec3V iPolygonMax = nmax;
+
+
+		PxU32 inside = 0;
+		for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
+		{
+			const Vec3V vert1 =points1In0[i]; //this still in polyData1's local space
+			points1In0[i] = M33MulV3(rot, vert1);
+			const FloatV z = V3GetZ(points1In0[i]);
+			points1In0TValue[i] = FSub(z, d);
+			points1In0[i] = V3SetZ(points1In0[i], d);
+			iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
+			iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
+			if(FAllGrtr(rd, z))
+			{
+				points1In0Penetration[i] = true;
+
+				if(contains(points0In0, 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 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)
+					{
+						//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
+						Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+						numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+					}
+				}
+			}
+			
+		}
+
+
+
+		if(inside == incidentPolygon.mNbVerts)
+		{
+			return;
+		}
+
+		inside = 0;
+		iPolygonMin = V3Sub(iPolygonMin, eps);
+		iPolygonMax = V3Add(iPolygonMax, eps);
+
+		const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
+		const FloatV iPlaneD = V3Dot(incidentNormal, M33MulV3(map1->vertex2Shape, V3LoadU(polyData1.mVerts[inds1[0]])));
+
+		for(PxU32 i=0; i<3; ++i)
+		{
+			if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
+			{
+				
+				inside++;
+
+				const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
+				const FloatV t = FSub(V3Dot(incidentNormal, vert0), iPlaneD);
+
+				if(FAllGrtr(t, contactDist))
+					continue;
+
+
+				const Vec3V projPoint = V3NegScaleSub(incidentNormal, t, vert0);
+
+				const Vec3V v = V3Sub(projPoint, vert0);
+				const FloatV t3 = V3Dot(v, contactNormal);
+
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t3);
+				numManifoldContacts = addMeshContacts(manifoldContacts, projPoint, vert0, 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)
+				{
+					//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
+					Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+					numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+				}
+			}
+				
+		}
+
+
+		if(inside == 3)
+			return;  
+	
+		//(2) segment intesection
+		for (PxU32 rStart = 0, rEnd = 2; rStart < 3; rEnd = rStart++)
+		{
+
+
+			const Vec3V rpA = points0In0[rStart];
+			const Vec3V rpB = points0In0[rEnd];
+
+			const Vec3V rMin = V3Min(rpA, rpB);
+			const Vec3V rMax = V3Max(rpA, rpB);
+
+			for (PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
+			{
+				if ((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd]))//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
+					continue;
+
+				const Vec3V ipA = points1In0[iStart];
+				const Vec3V ipB = points1In0[iEnd];
+
+				const Vec3V iMin = V3Min(ipA, ipB);
+				const Vec3V iMax = V3Max(ipA, ipB);
+
+				const BoolV tempCon = BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
+				const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+
+				if (BAllEqTTTT(con))
+					continue;
+
+				FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
+				FloatV a2 = signed2DTriArea(rpA, rpB, ipB);
+
+				if (FAllGrtr(zero, FMul(a1, a2)))
+				{
+					FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
+					FloatV a4 = signed2DTriArea(ipA, ipB, rpB);
+
+					if (FAllGrtr(zero, FMul(a3, a4)))
+					{
+
+						//these two segment intersect in 2d
+						const FloatV t = FMul(a1, FRecip(FSub(a2, a1)));
+
+						const Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
+						const Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));
+
+						const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri);
+						const Vec3V pAA = V3SetZ(pBB, d);
+						const Vec3V pA = M33TrnspsMulV3(rot, pAA);
+						const Vec3V pB = M33TrnspsMulV3(rot, pBB);
+						const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
+
+						if (FAllGrtr(pen, contactDist))
+							continue;
+
+						const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
+						numManifoldContacts = addMeshContacts(manifoldContacts, pB, pA, 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)
+						{
+							//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
+							Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+							numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+						}
+					}
+				}
+			}
+
+		}
+		
+	}
+
+
+	static void generatedPolyContacts(const Gu::PolygonalData& polyData0, const Gu::HullPolygonData& referencePolygon, const Gu::TriangleV& triangle, const PxU32 triangleIndex, const PxU8 triFlags, 
+		Gu::SupportLocal* map0, Gu::MeshPersistentContact* manifoldContacts, PxU32& numManifoldContacts, const Ps::aos::FloatVArg contactDist, const Ps::aos::Vec3VArg contactNormal, 
+		Cm::RenderOutput* renderOutput)
+	{
+		PX_UNUSED(triFlags);
+		PX_UNUSED(renderOutput);
+
+		using namespace Ps::aos;
+
+		const FloatV zero = FZero();
+
+		const PxU32 previousContacts = numManifoldContacts;
+
+		const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;
+
+		const Vec3V nContactNormal = V3Neg(contactNormal);
+
+		//this is the matrix transform all points to the 2d plane
+		const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);
+
+		Vec3V* points0In0=reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
+		Vec3V points1In0[3];
+		FloatV points1In0TValue[3];
+
+		bool points1In0Penetration[3];
+		
+		//Transform all the verts from vertex space to shape space
+		map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);
+
+		points1In0[0] = triangle.verts[0];
+		points1In0[1] = triangle.verts[1];
+		points1In0[2] = triangle.verts[2];
+
+
+#if PCM_LOW_LEVEL_DEBUG
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, referencePolygon.mNbVerts, (PxU32)PxDebugColor::eARGB_GREEN);
+		//Gu::PersistentContactManifold::drawTriangle(*gRenderOutPut, map0->transform.transform(points1In0[0]), map0->transform.transform(points1In0[1]), map0->transform.transform(points1In0[2]), (PxU32)PxDebugColor::eARGB_BLUE);
+#endif
+
+		//the first point in the reference plane
+		const Vec3V referencePoint = points0In0[0];
+ 
+		Vec3V eps = Vec3V_From_FloatV(FEps());
+		Vec3V max = Vec3V_From_FloatV(FMax());
+		Vec3V nmax = V3Neg(max); 
+
+		//transform reference polygon to 2d, calculate min and max
+		Vec3V rPolygonMin= max;
+		Vec3V rPolygonMax = nmax;
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			//points0In0[i].vertext = M33TrnspsMulV3(rot, Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]));
+			points0In0[i] = M33MulV3(rot, points0In0[i]);
+			rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
+			rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
+		}
+		
+		rPolygonMin = V3Sub(rPolygonMin, eps);
+		rPolygonMax = V3Add(rPolygonMax, eps);
+
+		
+		
+		const FloatV d = V3GetZ(points0In0[0]);
+
+		const FloatV rd = FAdd(d, contactDist);
+
+		Vec3V iPolygonMin= max; 
+		Vec3V iPolygonMax = nmax;
+
+		PxU32 inside = 0;
+		for(PxU32 i=0; i<3; ++i)
+		{
+			const Vec3V vert1 =points1In0[i]; //this still in polyData1's local space
+			points1In0[i] = M33MulV3(rot, vert1);
+			const FloatV z = V3GetZ(points1In0[i]);
+			points1In0TValue[i] = FSub(z, d);
+			points1In0[i] = V3SetZ(points1In0[i], d);
+			iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
+			iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
+			if(FAllGrtr(rd, z))
+			{
+				points1In0Penetration[i] = true;
+
+				//ML : check to see whether all the points of triangles in 2D space are within reference polygon's range
+				if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
+				{
+					inside++;
+
+					//calculate projection point
+					const FloatV t = V3Dot(contactNormal, V3Sub(vert1, referencePoint));
+					const Vec3V projectPoint = V3NegScaleSub(contactNormal, t, vert1);
+					
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), t);
+					numManifoldContacts = addMeshContacts(manifoldContacts, projectPoint, vert1, 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)
+					{
+						//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
+						Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+						numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+					}
+				}
+			}
+			
+		}
+
+
+		if(inside == 3)
+		{
+			return;
+		}
+
+		inside = 0;
+		iPolygonMin = V3Sub(iPolygonMin, eps);
+		iPolygonMax = V3Add(iPolygonMax, eps);
+
+		const Vec3V incidentNormal = triangle.normal();
+		const FloatV iPlaneD = V3Dot(incidentNormal, triangle.verts[0]);
+		const FloatV one = FOne();
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			if(contains(points1In0, 3, points0In0[i], iPolygonMin, iPolygonMax))
+			{
+			
+				const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
+
+				const FloatV t =FSub(V3Dot(incidentNormal, vert0), iPlaneD);
+				
+				if(FAllGrtr(t, contactDist))
+					continue;
+
+				const Vec3V projPoint = V3NegScaleSub(incidentNormal, t, vert0);
+
+				FloatV u, w;
+				barycentricCoordinates(projPoint, triangle.verts[0], triangle.verts[1], triangle.verts[2], u, w);
+				const BoolV con = BAnd(FIsGrtrOrEq(u, zero), BAnd(FIsGrtrOrEq(w, zero),  FIsGrtrOrEq(one, FAdd(u, w))));
+			
+				if(BAllEqTTTT(con))
+				{
+					inside++;
+
+					const Vec3V v = V3Sub(projPoint, vert0);
+					const FloatV t3 = V3Dot(v, contactNormal);
+					
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), t3);
+					numManifoldContacts = addMeshContacts(manifoldContacts, vert0, projPoint, 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)
+					{
+						//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
+						Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+						numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+					}
+				}
+					
+			}
+				
+		}
+
+		if(inside == referencePolygon.mNbVerts)
+			return;
+
+
+	
+		//Always generate segment contacts
+		//(2) segment intesection
+		for (PxU32 iStart = 0, iEnd = 2; iStart < 3; iEnd = iStart++)
+		{
+			if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )
+				continue;
+			
+			const Vec3V ipA = points1In0[iStart];
+			const Vec3V ipB = points1In0[iEnd];
+
+			const Vec3V iMin = V3Min(ipA, ipB);
+			const Vec3V iMax = V3Max(ipA, ipB);
+		
+			for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
+			{
+	
+				const Vec3V rpA = points0In0[rStart];
+				const Vec3V rpB = points0In0[rEnd];
+
+				const Vec3V rMin = V3Min(rpA, rpB);
+				const Vec3V rMax = V3Max(rpA, rpB);
+				
+				const BoolV tempCon =BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
+				const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+		
+				if(BAllEqTTTT(con))
+					continue;
+			
+					
+				FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
+				FloatV a2 = signed2DTriArea(rpA, rpB, ipB);
+
+
+				if(FAllGrtr(zero, FMul(a1, a2)))
+				{
+					FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
+					FloatV a4 = signed2DTriArea(ipA, ipB, rpB);
+
+					if(FAllGrtr(zero, FMul(a3, a4)))
+					{
+						
+						//these two segment intersect
+						const FloatV t = FMul(a1, FRecip(FSub(a2, a1)));
+
+						const Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
+						const Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));
+
+						const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri); 
+						const Vec3V pAA = V3SetZ(pBB, d);
+						const Vec3V pA = M33TrnspsMulV3(rot, pAA);
+						const Vec3V pB = M33TrnspsMulV3(rot, pBB);
+						const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
+				
+						if(FAllGrtr(pen, contactDist))
+							continue;
+		
+					
+						const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), pen);
+						numManifoldContacts = addMeshContacts(manifoldContacts, pA, pB, 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)
+						{
+							//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
+							Gu::SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
+							numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
+						}
+					}
+				}
+			}
+		}
+			
+	}
+
+
+	bool Gu::PCMConvexVsMeshContactGeneration::generateTriangleFullContactManifold(Gu::TriangleV& localTriangle, const PxU32 triangleIndex, const PxU32* triIndices, const PxU8 triFlags, const Gu::PolygonalData& polyData,  Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal)
+	{
+	
+		using namespace Ps::aos;
+
+		const FloatV threshold = FLoad(0.7071f);//about 45 degree0
+		PX_UNUSED(threshold);
+		{
+				
+			FeatureStatus status = POLYDATA0;
+			FloatV minOverlap = FMax();
+			//minNormal will be in the local space of polyData
+			Vec3V minNormal = V3Zero();
+
+
+			PxU32 feature0;
+			if(!testTriangleFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+				return false;
+
+			PxU32 feature1;
+			if(!testPolyFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+				return false;
+
+
+			if (!testPolyEdgeNormal(localTriangle, triFlags, polyData, localTriMap, polyMap, contactDist, minOverlap, minNormal, EDGE, status))
+				return false;
+
+
+			const Vec3V triNormal = localTriangle.normal();
+
+			if(status == POLYDATA0)
+			{
+				//minNormal is the triangle normal and it is in the local space of polydata0
+				const Gu::HullPolygonData& referencePolygon = polyData.mPolygons[getPolygonIndex(polyData, polyMap, minNormal)];
+
+				patchNormal = triNormal;
+				generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, mRenderOutput);
+			
+			}
+			else
+			{
+
+				if(status == POLYDATA1)
+				{
+					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);
+
+					if(FAllGrtr(cosTheta, threshold))
+					{
+						patchNormal = triNormal;
+						generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, *referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, mRenderOutput);
+					}
+					else
+					{
+						//ML : defer the contacts generation
+						const PxU32 nb = sizeof(PCMDeferredPolyData)/sizeof(PxU32);
+						PxU32 newSize = nb + mDeferredContacts.size();
+	                    mDeferredContacts.reserve(newSize);
+						PCMDeferredPolyData* PX_RESTRICT data = reinterpret_cast<PCMDeferredPolyData*>(mDeferredContacts.end());
+	                    mDeferredContacts.forceSize_Unsafe(newSize);
+
+						data->mTriangleIndex = triangleIndex;
+						data->mFeatureIndex = feature1;
+						data->triFlags = triFlags;  
+						data->mInds[0] = triIndices[0];
+						data->mInds[1] = triIndices[1];
+						data->mInds[2] = triIndices[2];
+						V3StoreU(localTriangle.verts[0], data->mVerts[0]);
+						V3StoreU(localTriangle.verts[1], data->mVerts[1]);
+						V3StoreU(localTriangle.verts[2], data->mVerts[2]);
+						return true;
+
+					}
+				}
+				else
+				{
+					feature1 = PxU32(getPolygonIndex(polyData, polyMap, minNormal));
+					const Gu::HullPolygonData* referencePolygon = &polyData.mPolygons[feature1];
+			
+					const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(referencePolygon->mPlane.n)));
+					const Vec3V nContactNormal = V3Neg(contactNormal);
+
+					//if the minimum sperating axis is edge case, we don't defer it because it is an activeEdge
+					patchNormal = nContactNormal;
+					generatedPolyContacts(polyData, *referencePolygon, localTriangle, triangleIndex, triFlags, polyMap, manifoldContacts, numContacts, contactDist, contactNormal, mRenderOutput);
+
+				}
+
+
+			}
+
+		}
+
+		return true;
+	}
+
+
+	bool Gu::PCMConvexVsMeshContactGeneration::generateTriangleFullContactManifold(Gu::TriangleV& localTriangle,  const PxU32 triangleIndex, const PxU8 triFlags, const Gu::PolygonalData& polyData,  Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
+		const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal, Cm::RenderOutput* renderOutput)
+	{
+	
+		using namespace Ps::aos;
+
+		const FloatV threshold = FLoad(0.7071f);//about 45 degree
+		PX_UNUSED(threshold);
+		{
+			
+			FeatureStatus status = POLYDATA0;
+			FloatV minOverlap = FMax();
+			//minNormal will be in the local space of polyData
+			Vec3V minNormal = V3Zero();
+
+			PxU32 feature0;
+			if(!testTriangleFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+				return false;
+			
+			PxU32 feature1;
+			if(!testPolyFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+				return false;
+
+			if(!testPolyEdgeNormal(localTriangle, triFlags, polyData, localTriMap, polyMap, contactDist, minOverlap, minNormal, EDGE, status))
+				return false;
+
+			const Vec3V triNormal = localTriangle.normal();
+			patchNormal = triNormal;
+
+			const Gu::HullPolygonData* referencePolygon = &polyData.mPolygons[getPolygonIndex(polyData, polyMap, triNormal)];
+			generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, *referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, renderOutput);
+			
+		}
+
+		return true;
+	}
+
+	bool Gu::PCMConvexVsMeshContactGeneration::generatePolyDataContactManifold(Gu::TriangleV& localTriangle, const PxU32 featureIndex,  const PxU32 triangleIndex, const PxU8 triFlags, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts, const Ps::aos::FloatVArg contactDist, Ps::aos::Vec3V& patchNormal)
+	{
+	
+		using namespace Ps::aos;
+
+		const Gu::HullPolygonData* referencePolygon = &mPolyData.mPolygons[featureIndex];
+		
+		const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(mPolyMap->shape2Vertex, V3LoadU(referencePolygon->mPlane.n)));
+		const Vec3V nContactNormal = V3Neg(contactNormal);
+		
+		patchNormal = nContactNormal;
+		generatedPolyContacts(mPolyData, *referencePolygon, localTriangle, triangleIndex, triFlags, mPolyMap, manifoldContacts, numContacts, contactDist, contactNormal, mRenderOutput);
+
+		return true;
+	}
+
+
+}//physx
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.h
new file mode 100644
index 00000000..ae115bc2
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMTriangleContactGen.h
@@ -0,0 +1,69 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_PCM_TRIANGLE_CONTACT_GEN_H
+#define GU_PCM_TRIANGLE_CONTACT_GEN_H
+
+#include "GuPCMContactGenUtil.h"
+#include "GuPersistentContactManifold.h"
+
+namespace physx
+{
+	struct PxTriangleMeshGeometryLL;
+	class PxHeightFieldGeometry;
+
+namespace Gu
+{
+
+	bool PCMContactConvexMesh(const Gu::PolygonalData& polyData0, 
+						Gu::SupportLocal* polyMap, 
+						const Ps::aos::FloatVArg minMargin, 
+						const PxBounds3& hullAABB, 
+						const PxTriangleMeshGeometryLL& shapeMesh,
+						const PxTransform& transform0, const PxTransform& transform1,
+						PxReal contactDistance, Gu::ContactBuffer& contactBuffer,
+						const Cm::FastVertex2ShapeScaling& convexScaling, const Cm::FastVertex2ShapeScaling& meshScaling,
+						bool idtConvexScale, bool idtMeshScale, Gu::MultiplePersistentContactManifold& multiManifold,
+						Cm::RenderOutput* renderOutput);
+
+	bool PCMContactConvexHeightfield(const Gu::PolygonalData& polyData0, 
+						Gu::SupportLocal* polyMap, 
+						const Ps::aos::FloatVArg minMargin, 
+						const PxBounds3& hullAABB, 
+						const PxHeightFieldGeometry& shapeHeightfield,
+						const PxTransform& transform0, const PxTransform& transform1,
+						PxReal contactDistance, Gu::ContactBuffer& contactBuffer,
+						const Cm::FastVertex2ShapeScaling& convexScaling, bool idtConvexScale, Gu::MultiplePersistentContactManifold& multiManifold,
+						Cm::RenderOutput* renderOutput);
+
+
+}
+}
+
+#endif
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp
new file mode 100644
index 00000000..6e70c27b
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp
@@ -0,0 +1,2281 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxMemory.h"
+#include "CmPhysXCommon.h"
+#include "GuPersistentContactManifold.h"
+#include "GuContactBuffer.h"
+#include "PsAllocator.h"
+#include "PsVecTransform.h"
+#include "PsUtilities.h"
+
+using namespace physx;
+
+namespace physx
+{
+namespace Gu
+{
+
+/*
+	This local function is to avoid DLL call
+*/
+static Ps::aos::FloatV distancePointSegmentSquaredLocal(const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, const Ps::aos::Vec3VArg p)
+{
+	using namespace Ps::aos;
+	const FloatV zero = FZero();
+	const FloatV one = FOne();
+
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V ab = V3Sub(b, a);
+	const FloatV nom = V3Dot(ap, ab);
+	
+	const FloatV denom = V3Dot(ab, ab);
+	const FloatV tValue = FClamp(FDiv(nom, denom), zero, one);
+
+	const FloatV t = FSel(FIsEq(denom, zero), zero, tValue);
+	const Vec3V v = V3NegScaleSub(ab, t, ap);
+	return V3Dot(v, v);
+}
+
+/*
+	This local function is to avoid DLL call
+*/
+static Ps::aos::FloatV distancePointTriangleSquaredLocal(	const Ps::aos::Vec3VArg p, 
+													const Ps::aos::Vec3VArg a, 
+													const Ps::aos::Vec3VArg b, 
+													const Ps::aos::Vec3VArg c)
+{
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+	//const Vec3V zero = V3Zero();
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+	const Vec3V bc = V3Sub(c, b);
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V bp = V3Sub(p, b);
+	const Vec3V cp = V3Sub(p, c);
+
+	const FloatV d1 = V3Dot(ab, ap); //  snom
+	const FloatV d2 = V3Dot(ac, ap); //  tnom
+	const FloatV d3 = V3Dot(ab, bp); // -sdenom
+	const FloatV d4 = V3Dot(ac, bp); //  unom = d4 - d3
+	const FloatV d5 = V3Dot(ab, cp); //  udenom = d5 - d6
+	const FloatV d6 = V3Dot(ac, cp); // -tdenom
+	const FloatV unom = FSub(d4, d3);
+	const FloatV udenom = FSub(d5, d6);
+	
+	//check if p in vertex region outside a
+	const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
+	const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
+	const BoolV con0 = BAnd(con00, con01); // vertex region a
+
+	if(BAllEqTTTT(con0))
+	{
+		const Vec3V vv = V3Sub(p, a);
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in vertex region outside b
+	const BoolV con10 = FIsGrtrOrEq(d3, zero);
+	const BoolV con11 = FIsGrtrOrEq(d3, d4);
+	const BoolV con1 = BAnd(con10, con11); // vertex region b
+	if(BAllEqTTTT(con1))
+	{
+		const Vec3V vv = V3Sub(p, b);
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in vertex region outside c
+	const BoolV con20 = FIsGrtrOrEq(d6, zero);
+	const BoolV con21 = FIsGrtrOrEq(d6, d5); 
+	const BoolV con2 = BAnd(con20, con21); // vertex region c
+	if(BAllEqTTTT(con2))
+	{
+		const Vec3V vv = V3Sub(p, c);
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of AB
+	const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
+	
+	const BoolV con30 = FIsGrtr(zero, vc);
+	const BoolV con31 = FIsGrtrOrEq(d1, zero);
+	const BoolV con32 = FIsGrtr(zero, d3);
+	const BoolV con3 = BAnd(con30, BAnd(con31, con32));
+	if(BAllEqTTTT(con3))
+	{
+		const FloatV sScale = FDiv(d1, FSub(d1, d3));
+		const Vec3V closest3 = V3ScaleAdd(ab, sScale, a);//V3Add(a, V3Scale(ab, sScale));
+		const Vec3V vv = V3Sub(p, closest3);
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of BC
+	const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
+	const BoolV con40 = FIsGrtr(zero, va);
+	const BoolV con41 = FIsGrtrOrEq(d4, d3);
+	const BoolV con42 = FIsGrtrOrEq(d5, d6);
+	const BoolV con4 = BAnd(con40, BAnd(con41, con42)); 
+	if(BAllEqTTTT(con4))
+	{
+		const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
+		const Vec3V closest4 = V3ScaleAdd(bc, uScale, b);//V3Add(b, V3Scale(bc, uScale));
+		const Vec3V vv = V3Sub(p, closest4);
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of AC
+	const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
+	const BoolV con50 = FIsGrtr(zero, vb);
+	const BoolV con51 = FIsGrtrOrEq(d2, zero);
+	const BoolV con52 = FIsGrtr(zero, d6);
+	const BoolV con5 = BAnd(con50, BAnd(con51, con52));
+	if(BAllEqTTTT(con5))
+	{
+		const FloatV tScale = FDiv(d2, FSub(d2, d6));
+		const Vec3V closest5 = V3ScaleAdd(ac, tScale, a);//V3Add(a, V3Scale(ac, tScale));
+		const Vec3V vv = V3Sub(p, closest5);
+		return V3Dot(vv, vv);
+	}
+
+	//P must project inside face region. Compute Q using Barycentric coordinates
+	const Vec3V n = V3Cross(ab, ac);
+	const FloatV nn = V3Dot(n,n);
+	const FloatV t = FSel(FIsGrtr(nn, zero),FDiv(V3Dot(n, V3Sub(a, p)), nn), zero);
+	const Vec3V closest6 = V3Add(p, V3Scale(n, t));
+
+	const Vec3V vv = V3Sub(p, closest6);
+
+	return V3Dot(vv, vv);
+}
+
+
+
+
+//This is the translational threshold used in invalidate_BoxConvexHull. 0.5 is 50% of the object margin. we use different threshold between
+//0 and 4 points. This threashold is a scale that is multiplied by the objects' margins.
+const PxF32 invalidateThresholds[5] = {	0.5f, 0.125f, 0.25f, 0.375f, 0.375f	};
+
+//This is the translational threshold used in invalidate_SphereCapsule. 0.5 is 50% of the object margin, we use different threshold between
+//0 and 2 points. This threshold is a scale that is multiplied by the objects' margin
+
+const PxF32 invalidateThresholds2[3] = { 0.5f, 0.1f, 0.75f	};
+
+//This is the rotational threshold used in invalidate_BoxConvexHull. 0.9998 is a threshold for quat difference
+//between previous and current frame
+const PxF32 invalidateQuatThresholds[5] = {	0.9998f, 0.9999f, 0.9999f, 0.9999f, 0.9999f	};
+
+
+//This is the rotational threshold used in invalidate_SphereCapsule. 0.9995f is a threshold for quat difference
+//between previous and current frame
+const PxF32 invalidateQuatThresholds2[3] = { 0.9995f, 0.9999f, 0.9997f	};
+
+}
+}
+
+
+#if VISUALIZE_PERSISTENT_CONTACT
+#include "CmRenderOutput.h"
+
+static void drawManifoldPoint(const Gu::PersistentContact& manifold,  const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, Cm::RenderOutput& out, PxU32 color=0xffffff)
+{
+	PX_UNUSED(color);
+
+	using namespace Ps::aos;
+	const Vec3V worldA = trA.transform(manifold.mLocalPointA);
+	const Vec3V worldB = trB.transform(manifold.mLocalPointB);
+	const Vec3V localNormal = Vec3V_From_Vec4V(manifold.mLocalNormalPen);
+	const FloatV pen = V4GetW(manifold.mLocalNormalPen);
+	
+	const Vec3V worldNormal = trB.rotate(localNormal);
+	PxVec3 a, b, v;
+	V3StoreU(worldA, a);
+	V3StoreU(worldB, b);
+	V3StoreU(worldNormal, v);
+	PxF32 dist;
+	FStore(pen, &dist);
+	PxVec3 e = a - v*dist;
+
+	PxF32 size = 0.05f;
+	const PxVec3 up(0.f, size, 0.f);
+	const PxVec3 right(size, 0.f, 0.f);
+	const PxVec3 forwards(0.f, 0.f, size);
+
+	PxF32 size2 = 0.1f;
+	const PxVec3 up2(0.f, size2, 0.f);
+	const PxVec3 right2(size2, 0.f, 0.f);
+	const PxVec3 forwards2(0.f, 0.f, size2);
+	
+	PxMat44 m = PxMat44(PxIdentity);
+	
+	out << 0xffff00ff << m << Cm::RenderOutput::LINES  << a << e;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + up << a - up;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + right << a - right;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + forwards << a - forwards;
+
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + up2 << b - up2;
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + right2 << b - right2;
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + forwards2 << b - forwards2;
+
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << a << b;
+
+}
+
+static void drawManifoldPoint(const Gu::PersistentContact& manifold,  const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, const Ps::aos::FloatVArg radius, Cm::RenderOutput& out, PxU32 color=0xffffff)
+{
+	PX_UNUSED(color);
+
+	using namespace Ps::aos;
+
+	const Vec3V localNormal = Vec3V_From_Vec4V(manifold.mLocalNormalPen);
+	const Vec3V worldNormal = trB.rotate(localNormal);
+	const Vec3V worldA = V3NegScaleSub(worldNormal, radius, trA.transform(manifold.mLocalPointA));
+	const Vec3V worldB = trB.transform(manifold.mLocalPointB);
+	const FloatV pen = FSub(V4GetW(manifold.mLocalNormalPen), radius);
+	
+	PxVec3 a, b, v;
+	V3StoreU(worldA, a);
+	V3StoreU(worldB, b);
+	V3StoreU(worldNormal, v);
+	PxF32 dist;
+	FStore(pen, &dist);
+	PxVec3 e = a - v*dist;
+
+	PxF32 size = 0.05f;
+	const PxVec3 up(0.f, size, 0.f);
+	const PxVec3 right(size, 0.f, 0.f);
+	const PxVec3 forwards(0.f, 0.f, size);
+
+	PxF32 size2 = 0.1f;
+	const PxVec3 up2(0.f, size2, 0.f);
+	const PxVec3 right2(size2, 0.f, 0.f);
+	const PxVec3 forwards2(0.f, 0.f, size2);
+	
+	PxMat44 m = PxMat44(PxIdentity);
+	
+	out << 0xffff00ff << m << Cm::RenderOutput::LINES  << a << e;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + up << a - up;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + right << a - right;
+	out << 0xff00ffff << m << Cm::RenderOutput::LINES << a + forwards << a - forwards;
+
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + up2 << b - up2;
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + right2 << b - right2;
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << b + forwards2 << b - forwards2;
+
+	out << 0xffff0000 << m << Cm::RenderOutput::LINES << a << b;
+
+}
+
+
+static PxU32 gColors[8] = { 0xff0000ff, 0xff00ff00, 0xffff0000,
+							0xff00ffff, 0xffff00ff, 0xffffff00,
+							0xff000080, 0xff008000};
+
+#endif
+
+/*
+	SIMD version 
+*/
+Ps::aos::Mat33V Gu::findRotationMatrixFromZAxis(const Ps::aos::Vec3VArg to)
+{
+	using namespace Ps::aos;
+	
+	const FloatV one = FOne();
+	const FloatV threshold = FLoad(0.9999f);
+
+	const FloatV e = V3GetZ(to);
+	const FloatV f = FAbs(e);
+
+	if(FAllGrtr(threshold, f))
+	{
+		const FloatV vx = FNeg(V3GetY(to));
+		const FloatV vy = V3GetX(to);
+		const FloatV h = FRecip(FAdd(one, e)); 
+		const FloatV hvx = FMul(h,vx);
+		const FloatV hvxy = FMul(hvx, vy);
+
+		const Vec3V col0 = V3Merge(FScaleAdd(hvx, vx, e),		hvxy,							      vy);
+		const Vec3V col1 = V3Merge(hvxy,				   FScaleAdd(h, FMul(vy, vy), e),			FNeg(vx));
+		const Vec3V col2 = V3Merge(FNeg(vy),					   vx,							  e);
+
+		return Mat33V(col0, col1, col2);
+
+	}
+	else
+	{
+
+		const FloatV two = FLoad(2.f);
+		const Vec3V from = V3UnitZ();
+		const Vec3V absFrom = V3UnitY();
+
+		const Vec3V u = V3Sub(absFrom, from);
+		const Vec3V v = V3Sub(absFrom, to);
+
+		const FloatV dotU = V3Dot(u, u);
+		const FloatV dotV = V3Dot(v, v);
+		const FloatV dotUV = V3Dot(u, v);
+
+		const FloatV c1 = FNeg(FDiv(two, dotU));
+		const FloatV c2 = FNeg(FDiv(two, dotV));
+		const FloatV c3 = FMul(c1, FMul(c2, dotUV));
+
+		const Vec3V c1u = V3Scale(u, c1);
+		const Vec3V c2v = V3Scale(v, c2);
+		const Vec3V c3v = V3Scale(v, c3);
+
+
+		FloatV temp0 = V3GetX(c1u);
+		FloatV temp1 = V3GetX(c2v);
+		FloatV temp2 = V3GetX(c3v);
+
+		Vec3V col0 = V3ScaleAdd(u, temp0, V3ScaleAdd(v, temp1, V3Scale(u, temp2)));
+		col0 = V3SetX(col0, FAdd(V3GetX(col0), one));
+
+		temp0 = V3GetY(c1u);
+		temp1 = V3GetY(c2v);
+		temp2 = V3GetY(c3v);
+
+		Vec3V col1 = V3ScaleAdd(u, temp0, V3ScaleAdd(v, temp1, V3Scale(u, temp2)));
+		col1 = V3SetY(col1, FAdd(V3GetY(col1), one));
+
+		temp0 = V3GetZ(c1u);
+		temp1 = V3GetZ(c2v);
+		temp2 = V3GetZ(c3v);
+
+		Vec3V col2 = V3ScaleAdd(u, temp0, V3ScaleAdd(v, temp1, V3Scale(u, temp2)));
+		col2 = V3SetZ(col2, FAdd(V3GetZ(col2), one));
+
+		return Mat33V(col0, col1, col2);
+
+	}
+}
+
+
+void Gu::PersistentContactManifold::drawManifold( Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+
+	PxVec3 a, b;
+	V3StoreU(trA.p, a);
+	V3StoreU(trB.p, b);
+
+	for(PxU32 i = 0; i< mNumContacts; ++i)
+	{
+		Gu::PersistentContact& m = mContactPoints[i];
+		drawManifoldPoint(m, trA, trB, out, gColors[i]);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(trA);
+	PX_UNUSED(trB);
+#endif
+}
+
+void Gu::PersistentContactManifold::drawManifold( Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, const Ps::aos::FloatVArg radius)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+
+	PxVec3 a, b;
+	V3StoreU(trA.p, a);
+	V3StoreU(trB.p, b);
+
+	for(PxU32 i = 0; i< mNumContacts; ++i)
+	{
+		Gu::PersistentContact& m = mContactPoints[i];
+		drawManifoldPoint(m, trA, trB, radius, out, gColors[i]);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(trA);
+	PX_UNUSED(trB);
+	PX_UNUSED(radius);
+#endif
+}
+
+void Gu::PersistentContactManifold::drawManifold(const Gu::PersistentContact& m, Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB)
+{	
+#if VISUALIZE_PERSISTENT_CONTACT
+	drawManifoldPoint(m, trA, trB, out, gColors[0]);
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(trA);
+	PX_UNUSED(trB);
+	PX_UNUSED(m);
+#endif
+}
+
+void Gu::PersistentContactManifold::drawPoint(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p, const PxF32 size, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	const PxVec3 up(0.f, size, 0.f);
+	const PxVec3 right(size, 0.f, 0.f);
+	const PxVec3 forwards(0.f, 0.f, size);
+
+	PxVec3 a;
+	V3StoreU(p, a);
+
+	PxMat44 m = PxMat44(PxIdentity);
+	
+	out << color << m << Cm::RenderOutput::LINES << a + up << a - up;
+	out << color << m << Cm::RenderOutput::LINES << a + right << a - right;
+	out << color << m << Cm::RenderOutput::LINES << a + forwards << a - forwards;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p);
+	PX_UNUSED(size);
+	PX_UNUSED(color);
+#endif
+}
+
+void Gu::PersistentContactManifold::drawLine(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	PxVec3 a, b;
+	V3StoreU(p0, a);
+	V3StoreU(p1, b);
+
+	PxMat44 m = PxMat44(PxIdentity);
+	out << color << m << Cm::RenderOutput::LINES << a << b;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p0);
+	PX_UNUSED(p1);
+	PX_UNUSED(color);
+#endif  
+}
+
+void Gu::PersistentContactManifold::drawTriangle(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const Ps::aos::Vec3VArg p2, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	PxVec3 a, b, c;
+	V3StoreU(p0, a);
+	V3StoreU(p1, b);
+	V3StoreU(p2, c);
+
+	PxMat44 m = PxMat44(PxIdentity);
+	out << color << m << Cm::RenderOutput::TRIANGLES << a << b << c;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p0);
+	PX_UNUSED(p1);
+	PX_UNUSED(p2);
+	PX_UNUSED(color);
+#endif
+}
+
+void Gu::PersistentContactManifold::drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	for(PxU32 i=0; i<numVerts; ++i)
+	{
+		Vec3V tempV0 = points[i == 0 ? numVerts-1 : i-1];
+		Vec3V tempV1 = points[i];
+		
+		drawLine(out, transform.transform(tempV0), transform.transform(tempV1), color);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(transform);
+	PX_UNUSED(points);
+	PX_UNUSED(numVerts);
+	PX_UNUSED(color);
+#endif
+
+}
+
+void Gu::PersistentContactManifold::drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsMatTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	for(PxU32 i=0; i<numVerts; ++i)
+	{
+		Vec3V tempV0 = points[i == 0 ? numVerts-1 : i-1];
+		Vec3V tempV1 = points[i];
+		
+		drawLine(out, transform.transform(tempV0), transform.transform(tempV1), color);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(transform);
+	PX_UNUSED(points);
+	PX_UNUSED(numVerts);
+	PX_UNUSED(color);
+#endif
+
+}
+
+/*
+	If a new point and the exisitng point's distance are within some replace breaking threshold, we will replace the existing point with the new point. This is used for
+	incremental manifold strategy.
+*/
+bool Gu::PersistentContactManifold::replaceManifoldPoint(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen
+														, const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	using namespace Ps::aos;
+
+	const FloatV shortestDist =  FMul(replaceBreakingThreshold, replaceBreakingThreshold); 
+	
+	for( PxU32 i = 0; i < mNumContacts; ++i )
+	{
+		const PersistentContact& mp = mContactPoints[i];
+
+		const Vec3V diffB =  V3Sub(mp.mLocalPointB, localPointB);
+		const FloatV sqDifB = V3Dot(diffB, diffB);
+		const Vec3V diffA = V3Sub(mp.mLocalPointA, localPointA);
+		const FloatV sqDifA = V3Dot(diffA, diffA);
+		const FloatV minSqDif = FMin(sqDifB, sqDifA);
+
+		if (FAllGrtr(shortestDist, minSqDif))
+		{
+			mContactPoints[i].mLocalPointA = localPointA;
+			mContactPoints[i].mLocalPointB = localPointB;
+			mContactPoints[i].mLocalNormalPen = localNormalPen;
+			return true;
+		}
+
+	}
+
+	return false;
+}
+
+
+
+PxU32 Gu::PersistentContactManifold::reduceContactSegment(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen)
+{
+	using namespace Ps::aos;
+	const Vec3V p  = localPointB;
+	const Vec3V p0 = mContactPoints[0].mLocalPointB;
+	const Vec3V p1 = mContactPoints[1].mLocalPointB;
+	const Vec3V v0 = V3Sub(p0, p);
+	const Vec3V v1 = V3Sub(p1, p);
+	const FloatV dist0 = V3Dot(v0, v0);
+	const FloatV dist1 = V3Dot(v1, v1);
+	if(FAllGrtr(dist0, dist1))
+	{
+		mContactPoints[1].mLocalPointA = localPointA;
+		mContactPoints[1].mLocalPointB = localPointB;
+		mContactPoints[1].mLocalNormalPen = localNormalPen;
+	}
+	else
+	{
+		mContactPoints[0].mLocalPointA = localPointA;
+		mContactPoints[0].mLocalPointB = localPointB;
+		mContactPoints[0].mLocalNormalPen = localNormalPen;
+	}
+
+	return 0;
+}
+
+
+
+PxU32 Gu::PersistentContactManifold::reduceContactsForPCM(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen)
+{
+	using namespace Ps::aos;
+
+
+	bool chosen[5];
+	physx::PxMemZero(chosen, sizeof(bool)*5);
+	const FloatV negMax = FNeg(FMax());
+	PersistentContact tempContacts[5];
+	
+	for(PxU32 i=0; i<4; ++i)
+	{
+		tempContacts[i] = mContactPoints[i];
+	}
+	tempContacts[4].mLocalPointA = localPointA;
+	tempContacts[4].mLocalPointB = localPointB;
+	tempContacts[4].mLocalNormalPen = localNormalPen;
+
+	//ML: we set the start point to be the 4th point
+	FloatV maxDist =V4GetW(localNormalPen);
+	PxI32 index = 4;
+	//Choose deepest point
+	for(PxI32 i=0; i<4; ++i)
+	{
+		const FloatV pen = V4GetW(tempContacts[i].mLocalNormalPen);
+		if(FAllGrtr(maxDist, pen))
+		{
+			maxDist = pen;
+			index = i;
+		}
+	}
+
+	chosen[index] = true;
+	mContactPoints[0] = tempContacts[index];
+
+	//ML: we set the start point to be the 0th point 
+	Vec3V dir= V3Sub(tempContacts[0].mLocalPointB, mContactPoints[0].mLocalPointB);
+	maxDist = V3Dot(dir, dir);
+	index = 0;
+
+	for(PxI32 i=1; i<5; ++i)
+	{
+		if(!chosen[i])
+		{
+			dir = V3Sub(tempContacts[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+			const FloatV d = V3Dot(dir, dir);
+			if(FAllGrtr(d, maxDist))
+			{
+				maxDist = d;
+				index = i;
+			}
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	chosen[index] = true;
+	mContactPoints[1] = tempContacts[index];
+
+	maxDist = negMax;	
+	for(PxI32 i=0; i<5; ++i)
+	{
+		if(!chosen[i])
+		{
+			const FloatV sqDif = distancePointSegmentSquaredLocal(mContactPoints[0].mLocalPointB, mContactPoints[1].mLocalPointB, tempContacts[i].mLocalPointB);
+			if(FAllGrtr(sqDif, maxDist))
+			{
+				maxDist = sqDif;
+				index = i;
+			}
+		}
+	}
+	//PX_ASSERT(chosen[index] == false);
+	chosen[index] = true;
+	mContactPoints[2]=tempContacts[index];
+
+	//Find point farthest away from segment tempContactPoints[0] - tempContactPoints[1]
+	maxDist = negMax;
+	for(PxI32 i=0; i<5; ++i)
+	{
+		if(!chosen[i])
+		{
+			const FloatV sqDif = distancePointTriangleSquaredLocal(	tempContacts[i].mLocalPointB, mContactPoints[0].mLocalPointB, mContactPoints[1].mLocalPointB, mContactPoints[2].mLocalPointB); 
+			if(FAllGrtr(sqDif, maxDist))
+			{
+				maxDist= sqDif;
+				index = i;
+			}
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	if(chosen[index] == true)
+	{
+		//if we don't have any new contacts, which means the leftover contacts are inside the triangles
+		mNumContacts = 3;
+		return 0;
+	}
+	else
+	{
+		chosen[index] = true;
+		mContactPoints[3]=tempContacts[index];
+	}
+
+	//Final pass, we work out the index that we didn't choose and bind it to its closest point. We then consider whether we want to swap the point if the
+	//point we were about to discard is deeper...
+
+	PxU32 notChosenIndex = 0;
+	for(PxU32 a = 0; a < 5; ++a)
+	{
+		if(!chosen[a])
+		{
+			notChosenIndex = a;
+			break;
+		}
+	}
+
+	FloatV closest = FMax();
+	index = 0;
+	for(PxI32 a = 0; a < 4; ++a)
+	{
+		Vec3V dif = V3Sub(mContactPoints[a].mLocalPointA, tempContacts[notChosenIndex].mLocalPointA);
+		const FloatV d2 = V3Dot(dif, dif);
+		if(FAllGrtr(closest, d2))
+		{
+			closest = d2;
+			index = a;
+		}
+	}
+
+	if(FAllGrtr(V4GetW(mContactPoints[index].mLocalNormalPen), V4GetW(tempContacts[notChosenIndex].mLocalNormalPen)))
+	{
+		//Swap
+		mContactPoints[index] = tempContacts[notChosenIndex];
+	}
+
+
+	return 0;
+}
+
+
+/*
+	This function is for box/convexHull vs box/convexHull.
+*/
+void Gu::PersistentContactManifold::addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsTransformV& transf1, const Ps::aos::FloatVArg contactOffset)
+{
+	using namespace Ps::aos;
+	//add the manifold contacts;
+	PxU32 contactCount = 0;//contactBuffer.count;
+	for(PxU32 i=0; (i< mNumContacts) & (contactCount < Gu::ContactBuffer::MAX_CONTACTS); ++i)
+	{
+		PersistentContact& p = getContactPoint(i);
+		
+		const FloatV dist = V4GetW(p.mLocalNormalPen);
+
+		//Either the newly created points or the cache points might have dist/penetration larger than contactOffset
+		if(FAllGrtrOrEq(contactOffset, dist))
+		{
+			const Vec3V worldP =transf1.transform(p.mLocalPointB);
+
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactCount++];
+			//Fast allign store
+			V4StoreA(Vec4V_From_Vec3V(normal), reinterpret_cast<PxF32*>(&contact.normal.x));
+			V4StoreA(Vec4V_From_Vec3V(worldP), reinterpret_cast<PxF32*>(&contact.point.x));
+			FStore(dist, &contact.separation);
+
+			PX_ASSERT(contact.point.isFinite());
+			PX_ASSERT(contact.normal.isFinite());
+			PX_ASSERT(PxIsFinite(contact.separation));
+
+			contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+		}
+	
+	}
+
+	contactBuffer.count = contactCount;
+}
+
+/*
+	This function is for direct implementation for box vs box. We don't need to discard the contacts based on whether the contact offset is larger than penetration/dist because
+	the direct implementation will guarantee the penetration/dist won't be larger than the contact offset. Also, we won't have points from the cache if the direct implementation
+	of box vs box is called.
+*/
+void Gu::PersistentContactManifold::addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsMatTransformV& transf1)
+{
+	using namespace Ps::aos;
+
+	//add the manifold contacts;
+	PxU32 contactCount = 0;//contactBuffer.count;
+	for(PxU32 i=0; (i< mNumContacts) & (contactCount < Gu::ContactBuffer::MAX_CONTACTS); ++i)
+	{
+	
+		PersistentContact& p = getContactPoint(i);
+		
+		const Vec3V worldP =transf1.transform(p.mLocalPointB);
+		const FloatV dist = V4GetW(p.mLocalNormalPen);
+
+		Gu::ContactPoint& contact = contactBuffer.contacts[contactCount++];
+		//Fast allign store
+		V4StoreA(Vec4V_From_Vec3V(normal), reinterpret_cast<PxF32*>(&contact.normal.x));
+		V4StoreA(Vec4V_From_Vec3V(worldP), reinterpret_cast<PxF32*>(&contact.point.x));
+		FStore(dist, &contact.separation);
+		PX_ASSERT(PxIsFinite(contact.point.x));
+		PX_ASSERT(PxIsFinite(contact.point.y));
+		PX_ASSERT(PxIsFinite(contact.point.z));
+
+		//PX_ASSERT(contact.separation > -0.2f);
+
+		contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+	}
+
+	contactBuffer.count = contactCount;
+}
+
+/*
+	This function is for sphere/capsule vs other primitives. We treat sphere as a point and capsule as a segment in the contact gen and store the sphere center or a point in the segment for capsule
+	in the manifold. 
+*/
+void Gu::PersistentContactManifold::addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsTransformV& transf0, const Ps::aos::FloatVArg radius, const Ps::aos::FloatVArg contactOffset)
+{
+	using namespace Ps::aos;
+
+	//add the manifold contacts;
+	PxU32 contactCount = 0;//contactBuffer.count;
+	for(PxU32 i=0; (i< mNumContacts) & (contactCount < Gu::ContactBuffer::MAX_CONTACTS); ++i)
+	{
+	
+		PersistentContact& p = getContactPoint(i);
+		const FloatV dist = FSub(V4GetW(p.mLocalNormalPen), radius);
+
+		//The newly created points should have a dist < contactOffset. However, points might come from the PCM contact cache so we might still have points which are
+		//larger than contactOffset. The reason why we don't want to discard the contacts in the contact cache whose dist > contactOffset is because the contacts may 
+		//only temporarily become separated. The points still project onto roughly the same spot but so, if the bodies move together again, the contacts may be valid again.
+		//This is important when simulating at large time-steps because GJK can only generate 1 point of contact and missing contacts for just a single frame with large time-steps
+		//may introduce noticeable instability.
+		if(FAllGrtrOrEq(contactOffset, dist))
+		{
+			const Vec3V worldP =V3NegScaleSub(normal, radius, transf0.transform(p.mLocalPointA));
+
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactCount++];
+			//Fast allign store
+			V4StoreA(Vec4V_From_Vec3V(normal), reinterpret_cast<PxF32*>(&contact.normal.x));
+			V4StoreA(Vec4V_From_Vec3V(worldP), reinterpret_cast<PxF32*>(&contact.point.x));
+			FStore(dist, &contact.separation);
+			//PX_ASSERT(PxAbs(contact.separation) < 2.f);
+
+			contact.internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
+		}
+	}
+
+	contactBuffer.count = contactCount;
+}
+
+
+/*
+	This function is used in the box/convexhull full manifold contact genenation. We will pass in a list of manifold contacts. If the number of contacts are more than
+	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)
+{
+
+	if(numPoints <= GU_MANIFOLD_CACHE_SIZE)
+	{
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			mContactPoints[i].mLocalPointA =  manifoldContacts[i].mLocalPointA;
+			mContactPoints[i].mLocalPointB =  manifoldContacts[i].mLocalPointB;
+			mContactPoints[i].mLocalNormalPen =  manifoldContacts[i].mLocalNormalPen;
+		}
+		mNumContacts = Ps::to8(numPoints);
+	}
+	else
+	{
+		reduceBatchContacts(manifoldContacts, numPoints);
+		mNumContacts = GU_MANIFOLD_CACHE_SIZE;
+	}
+}  
+
+/*
+	This function is for the plane and box contact gen. If the number of points passed in is more than 4, we need to do contact reduction 
+*/
+void Gu::PersistentContactManifold::addBatchManifoldContactsCluster(const PersistentContact* manifoldContacts, const PxU32 numPoints)
+{
+
+	if(numPoints <= GU_MANIFOLD_CACHE_SIZE)
+	{
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			mContactPoints[i].mLocalPointA =  manifoldContacts[i].mLocalPointA;
+			mContactPoints[i].mLocalPointB =  manifoldContacts[i].mLocalPointB;
+			mContactPoints[i].mLocalNormalPen =  manifoldContacts[i].mLocalNormalPen;
+		}
+		mNumContacts = Ps::to8(numPoints);
+	}
+	else
+	{
+		reduceBatchContactsCluster(manifoldContacts, numPoints);
+		mNumContacts = GU_MANIFOLD_CACHE_SIZE;
+	}
+}
+
+
+/*
+	This function is called by addBatchManifoldContactCluster. The logic in this funtion is:
+	(1)get the furthest away point from origin and store in mContactPoints[0]
+	(2)get the furthest away point from mContactPoints[0] and store in mContactPoints[1]
+	(3)calculate the min and max distance point away the segment (mContactPoints[0] and mContactPoints[1]) and store the max distance point to mContactPoints[2]
+	(4)if the min and max distance on the same side of the segment, we need to chose the min distance point again and store this point to mContactPoints[3]; 
+	(5)cluster around that 4 points and chose the deepest points
+	(6)reassign contact points
+*/
+void Gu::PersistentContactManifold::reduceBatchContactsCluster(const PersistentContact* manifoldPoints, const PxU32 numPoints)
+{
+	using namespace Ps::aos;
+	//get the deepest points
+
+	bool chosen[64];
+	physx::PxMemZero(chosen, sizeof(bool)*numPoints);
+	const FloatV max = FMax();
+	const FloatV nmax = FNeg(max);
+	FloatV maxDist = nmax;
+	PxU32 index = 0;
+
+	PxU32 indices[4];
+	
+	//get the furthest away point from itself
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		const FloatV dist = V3Dot(manifoldPoints[i].mLocalPointB, manifoldPoints[i].mLocalPointB);
+		if(FAllGrtr(dist, maxDist))
+		{
+			maxDist = dist;
+			index = i;
+		}
+	}
+
+	//keep the furthest points in the first position
+	mContactPoints[0] =  manifoldPoints[index];
+	chosen[index] = true;
+	indices[0] = index;
+
+
+	//calculate the furthest away points from mContactPoints[0]
+	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, mContactPoints[0].mLocalPointB);
+	maxDist = V3Dot(v, v);
+	index = 0;
+
+	for(PxU32 i=1; i<numPoints; ++i)
+	{
+		v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+		const FloatV d = V3Dot(v, v);
+		if(FAllGrtr(d, maxDist))
+		{
+			maxDist = d;
+			index = i;
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	mContactPoints[1] =  manifoldPoints[index];
+	chosen[index] = true;
+	indices[1] = index;
+
+
+	maxDist = nmax;
+	index = PxU32(-1);
+	v = V3Sub(mContactPoints[1].mLocalPointB, mContactPoints[0].mLocalPointB);
+	Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen)));
+
+	FloatV minDist = max;
+	PxU32 index1 = PxU32(-1);
+	
+
+	//calculate the min and max point away from the segment
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		if(!chosen[i])
+		{
+			v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+			const FloatV d = V3Dot(v, norm);
+			if(FAllGrtr(d, maxDist))
+			{
+				maxDist = d;
+				index = i;
+			}
+
+			if(FAllGrtr(minDist, d))
+			{
+				minDist = d;
+				index1 = i;
+			}
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false && chosen[index1] == false);
+	mContactPoints[2] =  manifoldPoints[index];
+	chosen[index] = true;
+	indices[2] = index;
+
+	//if min and max in the same side, chose again
+	const FloatV temp = FMul(minDist, maxDist);
+	if(FAllGrtr(temp, FZero()))
+	{
+		//chose again
+		maxDist = nmax;
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			if(!chosen[i])
+			{
+				v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+				const FloatV d = V3Dot(v, norm);
+				if(FAllGrtr(d, maxDist))
+				{
+					maxDist = d;
+					index1 = i;
+				}
+			}
+		}
+	}
+
+	mContactPoints[3] = manifoldPoints[index1];
+	chosen[index1] = true;
+	indices[3] = index1;
+
+	//cluster around that 4 points and chose the deepest points
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		if(!chosen[i])
+		{
+			maxDist = max;
+			const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
+			index = 0;
+			for(PxU32 j=0; j<4; ++j)
+			{
+				const Vec3V v1 = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[j].mLocalPointB);
+				const FloatV dist = V3Dot(v1, v1);
+				if(FAllGrtr(maxDist, dist))
+				{
+					maxDist = dist;
+					index = j;
+				}
+			}
+
+			//check to see whether the penetration is deeper than the point in mContactPoints
+			const FloatV tempPen = V4GetW(manifoldPoints[indices[index]].mLocalNormalPen);
+			if(FAllGrtr(tempPen, pen))
+			{
+				//swap the indices
+				indices[index] = i;
+			}
+		}
+	}
+
+	mContactPoints[0] = manifoldPoints[indices[0]];
+	mContactPoints[1] = manifoldPoints[indices[1]];
+	mContactPoints[2] = manifoldPoints[indices[2]];
+	mContactPoints[3] = manifoldPoints[indices[3]];
+	
+				
+}
+
+/*
+	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)
+{
+	using namespace Ps::aos;
+	
+	bool chosen[64];
+	physx::PxMemZero(chosen, sizeof(bool)*numPoints);
+	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)
+	{
+		const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
+		if(FAllGrtr(maxDist, pen))
+		{
+			maxDist = pen;
+			index = PxI32(i);
+		}
+	}
+	//keep the deepest points in the first position
+	mContactPoints[0] =  manifoldPoints[index];
+	chosen[index] = true;
+
+
+	//calculate the furthest away points
+	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, mContactPoints[0].mLocalPointB);
+	maxDist = V3Dot(v, v);
+	index = 0;
+
+	for(PxU32 i=1; i<numPoints; ++i)
+	{
+		v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+		const FloatV d = V3Dot(v, v);
+		if(FAllGrtr(d, maxDist))
+		{
+			maxDist = d;
+			index = PxI32(i);
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	mContactPoints[1] =  manifoldPoints[index];
+	chosen[index] = true;
+
+
+	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());
+
+	FloatV minDist = max;
+	PxI32 index1 = -1;
+	
+
+	//calculate the min and max point away from the segment
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		if(!chosen[i])
+		{
+			v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+			const FloatV d = V3Dot(v, norm);
+			if(FAllGrtr(d, maxDist))
+			{
+				maxDist = d;
+				index = PxI32(i);
+			}
+
+			if(FAllGrtr(minDist, d))
+			{
+				minDist = d;
+				index1 = PxI32(i);
+			}
+		}
+	}
+
+	mContactPoints[2] =  manifoldPoints[index];
+	chosen[index] = true;
+
+	//if min and max in the same side, chose again
+	const FloatV temp = FMul(minDist, maxDist);
+	if(FAllGrtr(temp, FZero()))
+	{
+		//chose again
+		maxDist = nmax;
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			if(!chosen[i])
+			{
+				v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+				const FloatV d = V3Dot(v, norm);
+				if(FAllGrtr(d, maxDist))
+				{
+					maxDist = d;
+					index1 = PxI32(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
+*/
+void Gu::PersistentContactManifold::reduceBatchContacts2(const PersistentContact* manifoldPoints, const PxU32 numPoints)  
+{
+	using namespace Ps::aos;
+	
+	PX_ASSERT(numPoints < 64);
+	bool chosen[64];
+	physx::PxMemZero(chosen, sizeof(bool)*numPoints);
+	FloatV maxDis = V4GetW(manifoldPoints[0].mLocalNormalPen);
+	PxI32 index = 0;
+	//keep the deepest point
+	for(PxU32 i=1; i<numPoints; ++i)
+	{
+		const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
+		if(FAllGrtr(maxDis, pen))
+		{  
+			maxDis = pen;
+			index = PxI32(i);
+		}
+	}
+	//keep the deepest points in the first position
+	mContactPoints[0] =  manifoldPoints[index];
+	chosen[index] = true;
+
+
+	//calculate the furthest away points
+	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, mContactPoints[0].mLocalPointB);
+	maxDis = V3Dot(v, v);
+	index = 0;
+
+	for(PxU32 i=1; i<numPoints; ++i)
+	{
+		v = V3Sub(manifoldPoints[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+		const FloatV d = V3Dot(v, v);
+		if(FAllGrtr(d, maxDis))
+		{
+			maxDis = d;
+			index = PxI32(i);
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	mContactPoints[1] =  manifoldPoints[index];
+	chosen[index] = true;
+
+	PxI32 secondIndex = index;
+	FloatV maxDepth = V4GetW(manifoldPoints[index].mLocalNormalPen);
+	for(PxU32 i = 0; i < numPoints; ++i)
+	{
+		if(!chosen[i])
+		{
+			Vec3V d0 = V3Sub(mContactPoints[0].mLocalPointB, manifoldPoints[i].mLocalPointB);
+			Vec3V d1 = V3Sub(mContactPoints[1].mLocalPointB, manifoldPoints[i].mLocalPointB);
+			const FloatV dd0 = V3Dot(d0, d0);
+			const FloatV dd1 = V3Dot(d1, d1);
+
+			if(FAllGrtr(dd0, dd1))
+			{
+				//This clusters to point 1
+				if(FAllGrtr(maxDepth, V4GetW(manifoldPoints[i].mLocalNormalPen)))
+				{
+					secondIndex = PxI32(i);
+				}
+			}
+		}
+	}
+
+	if(secondIndex != index)
+	{
+		mContactPoints[1] = manifoldPoints[secondIndex];
+	}	
+}
+
+PxU32 Gu::PersistentContactManifold::addManifoldPoint(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen
+													 , const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	using namespace Ps::aos;
+	
+	if(replaceManifoldPoint(localPointA, localPointB, localNormalPen, replaceBreakingThreshold)) //replace the new point with the old one
+		return 0;
+
+	switch(mNumContacts)
+	{
+	case 0:
+	case 1:
+	case 2:
+	case 3:
+		mContactPoints[mNumContacts].mLocalPointA = localPointA;
+		mContactPoints[mNumContacts].mLocalPointB = localPointB;
+		mContactPoints[mNumContacts++].mLocalNormalPen = localNormalPen;
+
+		return 1;
+	default:
+		return reduceContactsForPCM(localPointA, localPointB, localNormalPen);//should be always return zero
+	};
+
+}
+
+
+/*
+	This function is for capsule vs other primitives. If the manifold originally has contacts and we can incrementally add a point at a time, we will
+	use this function to add a point to manifold. If the number of contacts inside the manifold is more than 2, we will reduce contacts to 2 points.
+*/
+PxU32 Gu::PersistentContactManifold::addManifoldPoint2(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen
+													  , const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	using namespace Ps::aos;
+	
+	if(replaceManifoldPoint(localPointA, localPointB, localNormalPen, replaceBreakingThreshold)) //replace the new point with the old one
+		return 0;
+
+	switch(mNumContacts)
+	{
+	case 0:
+	case 1:      
+		mContactPoints[mNumContacts].mLocalPointA = localPointA;
+		mContactPoints[mNumContacts].mLocalPointB = localPointB;
+		mContactPoints[mNumContacts++].mLocalNormalPen = localNormalPen;
+		return 1;
+	case 2:
+		return reduceContactSegment(localPointA, localPointB, localNormalPen);
+	default:
+		PX_ASSERT(0);
+	};
+	return 0;
+
+}
+
+/*
+	This function is used in the capsule full manifold contact genenation. We will pass in a list of manifold contacts. If the number of contacts are more than
+	2, 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::addBatchManifoldContacts2(const PersistentContact* manifoldContacts, const PxU32 numPoints)
+{
+	using namespace Ps::aos;
+	
+	if(numPoints <= 2)
+	{
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			mContactPoints[i].mLocalPointA =  manifoldContacts[i].mLocalPointA;
+			mContactPoints[i].mLocalPointB =  manifoldContacts[i].mLocalPointB;
+			mContactPoints[i].mLocalNormalPen =  manifoldContacts[i].mLocalNormalPen;
+		}
+
+		mNumContacts = Ps::to8(numPoints);
+	}
+	else
+	{
+		reduceBatchContacts2(manifoldContacts, numPoints);
+		mNumContacts = 2;
+	}
+
+}
+
+
+/*
+	If the patch total number of manifold contacts are less than or equal to GU_SINGLE_MANIFOLD_CACHE_SIZE, we will add the manifold contacts in the contact stream to the manifold contact buffer
+	which is associated to this single persistent contact manifold. Otherwise, we will reduce the manifold contacts to GU_SINGLE_MANIFOLD_CACHE_SIZE.
+*/
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::addBatchManifoldContactsConvex(const MeshPersistentContact* manifoldContact, const PxU32 numContactExt, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	PX_UNUSED(replaceBreakingThreshold);
+
+	using namespace Ps::aos;
+	
+	if(patch.mTotalSize <= GU_SINGLE_MANIFOLD_CACHE_SIZE)
+	{
+		PCMContactPatch* currentPatch = &patch;
+
+		//this is because we already add the manifold contacts into manifoldContact array
+		PxU32 tempNumContacts = 0;
+		while(currentPatch)
+		{
+			for(PxU32 j=currentPatch->mStartIndex; j<currentPatch->mEndIndex; ++j)
+			{
+				mContactPoints[tempNumContacts++] =  manifoldContact[j];
+			}
+			currentPatch = currentPatch->mNextPatch;
+		} 
+		mNumContacts = tempNumContacts;
+		return patch.mPatchMaxPen;
+	}
+	else
+	{
+		//contact reduction
+		const FloatV maxPen = reduceBatchContactsConvex(manifoldContact, numContactExt, patch);
+		mNumContacts = GU_SINGLE_MANIFOLD_CACHE_SIZE;
+		return maxPen;
+	}
+}
+
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::addBatchManifoldContactsSphere(const MeshPersistentContact* manifoldContact, const PxU32 numContactExt, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	PX_UNUSED(replaceBreakingThreshold);
+
+	using namespace Ps::aos;
+
+	const FloatV maxPen = reduceBatchContactsSphere(manifoldContact, numContactExt, patch);
+	mNumContacts = 1;
+	return maxPen;
+}
+
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::addBatchManifoldContactsCapsule(const MeshPersistentContact* manifoldContact, const PxU32 numContactExt, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold)
+{
+	PX_UNUSED(replaceBreakingThreshold);
+
+	using namespace Ps::aos;
+
+	if(patch.mTotalSize <=GU_CAPSULE_MANIFOLD_CACHE_SIZE)
+	{
+		PCMContactPatch* currentPatch = &patch;
+
+		//this is because we already add the manifold contacts into manifoldContact array
+		PxU32 tempNumContacts = 0;
+		while(currentPatch)
+		{
+			for(PxU32 j=currentPatch->mStartIndex; j<currentPatch->mEndIndex; ++j)
+			{
+				mContactPoints[tempNumContacts++] = manifoldContact[j];
+			}
+			currentPatch = currentPatch->mNextPatch;
+		} 
+		mNumContacts = tempNumContacts;
+		return patch.mPatchMaxPen;
+	}
+	else
+	{
+		
+		const FloatV maxPen = reduceBatchContactsCapsule(manifoldContact, numContactExt, patch);
+		mNumContacts = GU_CAPSULE_MANIFOLD_CACHE_SIZE;
+		return maxPen;
+	}
+
+}
+
+
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsSphere(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch)
+{
+	PX_UNUSED(numContacts);
+
+	using namespace Ps::aos;
+	FloatV max = FMax();
+	FloatV maxDist = max;
+	PxI32 index = -1;
+
+	
+	PCMContactPatch* currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			const FloatV pen = V4GetW(manifoldContactExt[i].mLocalNormalPen);
+			if(FAllGrtr(maxDist, pen))
+			{
+				maxDist = pen;
+				index = PxI32(i);
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	PX_ASSERT(index!=-1);
+	mContactPoints[0] = manifoldContactExt[index];
+
+	return maxDist;
+
+}
+
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsCapsule(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch)
+{
+	using namespace Ps::aos;
+
+	bool* chosen = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*numContacts));
+	physx::PxMemZero(chosen, sizeof(bool)*numContacts);
+	const FloatV max = FMax();
+	FloatV maxDis = max;
+	PxI32 index = -1;
+
+	FloatV maxPen = max;
+
+
+	PCMContactPatch* currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			const FloatV pen = V4GetW(manifoldContactExt[i].mLocalNormalPen);
+			if(FAllGrtr(maxDis, pen))
+			{
+				maxDis = pen;
+				index = PxI32(i);
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	chosen[index] = true;
+	mContactPoints[0] = manifoldContactExt[index];
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
+
+
+	//calculate the furthest away points
+	Vec3V v = V3Sub(manifoldContactExt[patch.mStartIndex].mLocalPointB, mContactPoints[0].mLocalPointB);
+	maxDis = V3Dot(v, v);
+	index = PxI32(patch.mStartIndex);
+
+	currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			v = V3Sub(manifoldContactExt[i].mLocalPointB, mContactPoints[0].mLocalPointB);
+			const FloatV d = V3Dot(v, v);
+			if(FAllGrtr(d, maxDis))
+			{
+				maxDis = d;
+				index = PxI32(i);
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	chosen[index] = true;
+	mContactPoints[1] = manifoldContactExt[index];
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
+
+	//keep the second deepest point
+	maxDis = max;
+	currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			if(!chosen[i])
+			{
+				const FloatV pen = V4GetW(manifoldContactExt[i].mLocalNormalPen);
+				//const FloatV v = V3Dot(manifoldContactExt[i].mLocalPointB, manifoldContactExt[i].mLocalPointB);
+				if(FAllGrtr(maxDis, pen))
+				{
+					maxDis = pen;
+					index = PxI32(i);
+				}
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+	//PX_ASSERT(chosen[index] == false);
+	chosen[index] = true;
+	mContactPoints[2] = manifoldContactExt[index];
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
+
+	return maxPen;
+}
+
+Ps::aos::FloatV Gu::SinglePersistentContactManifold::reduceBatchContactsConvex(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch)
+{
+	using namespace Ps::aos;
+
+	bool* chosen = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*numContacts));
+	physx::PxMemZero(chosen, sizeof(bool)*numContacts);
+	const FloatV max = FMax();
+	const FloatV nmax = FNeg(max);
+	FloatV maxDis = nmax;
+	PxI32 index = -1;
+
+	PCMContactPatch* currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			//const FloatV pen = V4GetW(manifoldContactExt[i].localNormalPen);
+			const FloatV v = V3Dot(manifoldContactExt[i].mLocalPointB, manifoldContactExt[i].mLocalPointB);
+			if(FAllGrtr(v, maxDis))
+			{
+				maxDis = v;
+				index = PxI32(i);
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	chosen[index] = true;
+
+	const Vec3V contact0 = manifoldContactExt[index].mLocalPointB;
+	mContactPoints[0] = manifoldContactExt[index];
+
+	FloatV maxPen = V4GetW(manifoldContactExt[index].mLocalNormalPen);
+
+	//calculate the furthest away points
+	Vec3V v = V3Sub(manifoldContactExt[patch.mStartIndex].mLocalPointB, contact0);
+	maxDis = V3Dot(v, v);
+	index = PxI32(patch.mStartIndex);
+
+	currentPatch = &patch;
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			v = V3Sub(manifoldContactExt[i].mLocalPointB, contact0);
+			const FloatV d = V3Dot(v, v);
+			if(FAllGrtr(d, maxDis))
+			{
+				maxDis = d;
+				index = PxI32(i);
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	chosen[index] = true;
+	const Vec3V contact1 = manifoldContactExt[index].mLocalPointB;
+	mContactPoints[1] = manifoldContactExt[index];
+
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
+
+	maxDis = nmax;
+	index = -1;
+	v = V3Sub(contact1, contact0);
+	Vec3V norm = V3Normalize(V3Cross(v, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen)));
+	FloatV minDis = max;
+	PxI32 index1 = -1;
+	
+
+	//calculate the point furthest way to the segment
+	currentPatch = &patch;
+	
+	while(currentPatch)
+	{
+		for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+		{
+			if(!chosen[i])
+			{
+				v = V3Sub(manifoldContactExt[i].mLocalPointB, contact0);
+				const FloatV d = V3Dot(v, norm);
+				if(FAllGrtr(d, maxDis))
+				{
+					maxDis = d;
+					index = PxI32(i);
+				}
+
+				if(FAllGrtr(minDis, d))
+				{
+					minDis = d;
+					index1 = PxI32(i);
+				}
+			}
+		}
+		currentPatch = currentPatch->mNextPatch;
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+
+	chosen[index] = true;
+	mContactPoints[2] = manifoldContactExt[index];
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index].mLocalNormalPen));
+
+	//if min and max in the same side, choose again
+	const FloatV temp = FMul(minDis, maxDis);
+	if(FAllGrtr(temp, FZero()))
+	{
+		//choose again
+		maxDis = nmax;
+		//calculate the point furthest way to the segment
+		currentPatch = &patch;
+		
+		while(currentPatch)
+		{
+			for(PxU32 i=currentPatch->mStartIndex; i<currentPatch->mEndIndex; ++i)
+			{
+				if(!chosen[i])
+				{
+					v = V3Sub(manifoldContactExt[i].mLocalPointB, contact0);
+					const FloatV d = V3Dot(v, norm);
+					if(FAllGrtr(d, maxDis))
+					{
+						maxDis = d;
+						index1 = PxI32(i);
+					}
+				}
+			}
+			currentPatch = currentPatch->mNextPatch;
+		}
+	}
+
+	//PX_ASSERT(chosen[index1] == false);
+
+	chosen[index1] = true;
+	mContactPoints[3] = manifoldContactExt[index1];
+	maxPen = FMin(maxPen, V4GetW(manifoldContactExt[index1].mLocalNormalPen));
+
+	const PxU32 NB_TO_ADD = GU_SINGLE_MANIFOLD_CACHE_SIZE - 4;
+
+	FloatV pens[NB_TO_ADD];
+	PxU32 inds[NB_TO_ADD];
+	for (PxU32 a = 0; a < NB_TO_ADD; ++a)
+	{
+		pens[a] = FMax();
+		inds[a] = 0;
+	}
+
+	
+
+	{
+		currentPatch = &patch;
+
+		while (currentPatch)
+		{
+			for (PxU32 i = currentPatch->mStartIndex; i < currentPatch->mEndIndex; ++i)
+			{
+				if (!chosen[i])
+				{
+					const FloatV pen = V4GetW(manifoldContactExt[i].mLocalNormalPen);
+					for (PxU32 a = 0; a < NB_TO_ADD; ++a)
+					{
+						if (FAllGrtr(pens[a], pen))
+						{
+							for (PxU32 b = a + 1; b < NB_TO_ADD; ++b)
+							{
+								pens[b] = pens[b - 1];
+								inds[b] = inds[b - 1];
+							}
+							pens[a] = pen;
+							inds[a] = i;
+							break;
+						}
+					}
+				}
+			}
+			currentPatch = currentPatch->mNextPatch;
+		}
+		for (PxU32 i = 0; i < NB_TO_ADD; ++i)
+		{
+			mContactPoints[i + 4] = manifoldContactExt[inds[i]];
+			maxPen = FMin(maxPen, pens[i]);
+		}
+	}
+
+
+
+
+	return maxPen;
+}
+
+PxU32 Gu::SinglePersistentContactManifold::reduceContacts(MeshPersistentContact* manifoldPoints, PxU32 numPoints)
+{
+	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;
+	MeshPersistentContact newManifold[GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE];
+
+	//keep the deepest point
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
+		if(FAllGrtr(maxDis, pen))
+		{
+			maxDis = pen;
+			index = i;
+		}
+	}
+	//keep the deepest points in the first position
+	newManifold[0] = manifoldPoints[index];
+	chosen[index] = true;
+
+
+	//calculate the furthest away points
+	Vec3V v = V3Sub(manifoldPoints[0].mLocalPointB, newManifold[0].mLocalPointB);
+	maxDis = V3Dot(v, v);
+	index = 0;
+
+	for(PxU32 i=1; i<numPoints; ++i)
+	{
+		v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
+		const FloatV d = V3Dot(v, v);
+		if(FAllGrtr(d, maxDis))
+		{
+			maxDis = d;
+			index = i;
+		}  
+	}
+
+	//PX_ASSERT(chosen[index] == false);
+	newManifold[1] = manifoldPoints[index];
+	chosen[index] = true;
+
+
+	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;
+	
+
+	//calculate the point furthest way to the segment
+	for(PxU32 i=0; i<numPoints; ++i)
+	{
+		if(!chosen[i])
+		{
+			v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
+			const FloatV d = V3Dot(v, norm);
+			if(FAllGrtr(d, maxDis))
+			{
+				maxDis = d;
+				index = i;
+			}
+
+			if(FAllGrtr(minDis, d))
+			{
+				minDis = d;
+				index1 = i;
+			}
+		}
+	}
+
+	//PX_ASSERT(chosen[index] == false && chosen[index1]== false);
+
+	chosen[index] = true;
+	newManifold[2] = manifoldPoints[index];
+	
+	const FloatV temp = FMul(minDis, maxDis);
+	if(FAllGrtr(temp, FZero()))
+	{
+		//chose the something further away from newManifold[2] 
+		maxDis = FNeg(max);
+		for(PxU32 i=0; i<numPoints; ++i)
+		{
+			if(!chosen[i])
+			{
+				v = V3Sub(manifoldPoints[i].mLocalPointB, newManifold[0].mLocalPointB);
+				const FloatV d = V3Dot(v, norm);
+				if(FAllGrtr(d, maxDis))
+				{
+					maxDis = d;
+					index1 = i;
+				}
+			}
+		}
+
+	}
+
+	newManifold[3]= manifoldPoints[index1];
+	chosen[index1] = true;
+
+	maxDis = max;
+	index = 0xffffffff;
+	//choose the 5 point, second deepest in the left overlap point
+	for (PxU32 i = 0; i < numPoints; ++i)
+	{
+		if (!chosen[i])
+		{
+			const FloatV pen = V4GetW(manifoldPoints[i].mLocalNormalPen);
+			if (FAllGrtr(maxDis, pen))
+			{
+				maxDis = pen;
+				index = i;
+			}
+		}
+	}
+
+	newManifold[4] = manifoldPoints[index];
+	chosen[index] = true;
+
+	//copy the new manifold back
+	for(PxU32 i=0; i<GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE; ++i)
+	{
+		manifoldPoints[i] = newManifold[i];
+	}
+
+	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;
+	const FloatV sqProjectBreakingThreshold =  FMul(projectBreakingThreshold, projectBreakingThreshold); 
+
+	FloatV maxPen = FZero();
+	// first refresh worldspace positions and distance
+	for (PxU32 i=mNumContacts; i > 0; --i)
+	{
+		MeshPersistentContact& manifoldPoint = mContactPoints[i-1];
+		const Vec3V localAInB = aToB.transform( manifoldPoint.mLocalPointA ); // from a to b
+		const Vec3V localBInB = manifoldPoint.mLocalPointB;
+		const Vec3V v = V3Sub(localAInB, localBInB); 
+
+		const Vec3V localNormal = Vec3V_From_Vec4V(manifoldPoint.mLocalNormalPen); // normal in b space
+		const FloatV dist= V3Dot(v, localNormal);
+
+		const Vec3V projectedPoint = V3NegScaleSub(localNormal,  dist, localAInB);//manifoldPoint.worldPointA - manifoldPoint.worldPointB * manifoldPoint.m_distance1;
+		const Vec3V projectedDifference = V3Sub(localBInB, projectedPoint);
+
+		const FloatV distance2d = V3Dot(projectedDifference, projectedDifference);
+		//const BoolV con = BOr(FIsGrtr(dist, contactOffset), FIsGrtr(distance2d, sqProjectBreakingThreshold));
+		const BoolV con = FIsGrtr(distance2d, sqProjectBreakingThreshold);
+		if(BAllEqTTTT(con))
+		{
+			removeContactPoint(i-1);
+		} 
+		else
+		{
+			manifoldPoint.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), dist);
+			maxPen = FMin(maxPen, dist);
+		}
+	}
+
+	return maxPen;
+}
+
+
+void Gu::SinglePersistentContactManifold::drawManifold( Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+
+	PxVec3 a, b;
+	V3StoreU(trA.p, a);
+	V3StoreU(trB.p, b);
+
+	for(PxU32 i = 0; i< mNumContacts; ++i)
+	{
+		Gu::MeshPersistentContact& m = mContactPoints[i];
+		drawManifoldPoint(m, trA, trB, out, gColors[i]);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(trA);
+	PX_UNUSED(trB);
+#endif
+}
+
+void Gu::MultiplePersistentContactManifold::drawManifold( Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB)
+{
+	for(PxU32 i=0; i<mNumManifolds; ++i)
+	{
+		SinglePersistentContactManifold* manifold = getManifold(i);
+		manifold->drawManifold(out, trA, trB);
+	}
+}
+
+void Gu::MultiplePersistentContactManifold::drawLine(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	PxVec3 a, b;
+	V3StoreU(p0, a);
+	V3StoreU(p1, b);
+
+	PxMat44 m = PxMat44(PxIdentity);
+	out << color << m << Cm::RenderOutput::LINES << a << b;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p0);
+	PX_UNUSED(p1);
+	PX_UNUSED(color);
+
+#endif
+}
+
+void Gu::MultiplePersistentContactManifold::drawLine(Cm::RenderOutput& out, const PxVec3 p0, const PxVec3 p1, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+
+	PxMat44 m = PxMat44(PxIdentity);
+	out << color << m << Cm::RenderOutput::LINES << p0 << p1;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p0);
+	PX_UNUSED(p1);
+	PX_UNUSED(color);
+#endif
+}
+
+void Gu::MultiplePersistentContactManifold::drawPoint(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p, const PxF32 size, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	const PxVec3 up(0.f, size, 0.f);
+	const PxVec3 right(size, 0.f, 0.f);
+	const PxVec3 forwards(0.f, 0.f, size);
+
+	PxVec3 a;
+	V3StoreU(p, a);
+
+	PxMat44 m = PxMat44(PxIdentity);
+	
+	out << color << m << Cm::RenderOutput::LINES << a + up << a - up;
+	out << color << m << Cm::RenderOutput::LINES << a + right << a - right;
+	out << color << m << Cm::RenderOutput::LINES << a + forwards << a - forwards;
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(p);
+	PX_UNUSED(size);
+	PX_UNUSED(color);
+#endif
+}
+
+
+void Gu::MultiplePersistentContactManifold::drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color)
+{
+	using namespace Ps::aos;
+#if VISUALIZE_PERSISTENT_CONTACT
+	for(PxU32 i=0; i<numVerts; ++i)
+	{
+		Vec3V tempV0 = points[i == 0 ? numVerts-1 : i-1];
+		Vec3V tempV1 = points[i];
+		
+		drawLine(out, transform.transform(tempV0), transform.transform(tempV1), color);
+	}
+#else
+	PX_UNUSED(out);
+	PX_UNUSED(transform);
+	PX_UNUSED(points);
+	PX_UNUSED(numVerts);
+	PX_UNUSED(color);
+#endif
+
+}
+
+static Ps::aos::FloatV addBatchManifoldContactsToSingleManifold(Gu::SinglePersistentContactManifold* manifold, Gu::MeshPersistentContact* manifoldContact, PxU32 numManifoldContacts, Gu::PCMContactPatch* patch,  const Ps::aos::FloatVArg sqReplaceBreakingThreshold, PxU8 maxContactsPerManifold)
+{
+	using namespace Ps::aos;
+	switch(maxContactsPerManifold)
+	{
+	case GU_SPHERE_MANIFOLD_CACHE_SIZE://sphere
+		return manifold->addBatchManifoldContactsSphere(manifoldContact, numManifoldContacts, *patch,  sqReplaceBreakingThreshold);
+	case GU_CAPSULE_MANIFOLD_CACHE_SIZE://capsule, need to implement keep two deepest
+		return manifold->addBatchManifoldContactsCapsule(manifoldContact, numManifoldContacts, *patch, sqReplaceBreakingThreshold);
+	default://cache size GU_SINGLE_MANIFOLD_CACHE_SIZE
+		return manifold->addBatchManifoldContactsConvex(manifoldContact, numManifoldContacts, *patch, sqReplaceBreakingThreshold);
+	};
+}
+
+/*
+	This function adds the manifold contacts with different patches into the corresponding single persistent contact manifold
+*/
+void Gu::MultiplePersistentContactManifold::addManifoldContactPoints(MeshPersistentContact* manifoldContact, PxU32 numManifoldContacts, PCMContactPatch** contactPatch, const PxU32 numPatch, const Ps::aos::FloatVArg sqReplaceBreakingThreshold, const Ps::aos::FloatVArg acceptanceEpsilon, PxU8 maxContactsPerManifold)
+{
+	using namespace Ps::aos;
+	
+	if(mNumManifolds == 0)
+	{
+		for(PxU32 i=0; i<numPatch; ++i)
+		{
+			PCMContactPatch* patch = contactPatch[i];
+			//this mean the patch hasn't been add to the manifold
+			if(patch->mRoot == patch)
+			{
+				
+				SinglePersistentContactManifold* manifold = getEmptyManifold();
+				if(manifold)
+				{
+					const FloatV _maxPen = addBatchManifoldContactsToSingleManifold(manifold, manifoldContact, numManifoldContacts, patch, sqReplaceBreakingThreshold, maxContactsPerManifold);
+					FStore(_maxPen, &mMaxPen[mManifoldIndices[mNumManifolds]]);
+					mNumManifolds++;
+				}
+				else
+				{
+					//We already pre-sorted the patches so we know we can return here
+					return;
+				}
+				
+			}
+		}
+	
+		
+	}
+	else
+	{
+		//we do processContacts() when the number of contacts are more than 16, such that, we might call processContacts() multiple times when we have very detailed mesh
+		//or very large contact offset/objects. In this case, the mNumManifolds will be large than 0. 
+		PCMContactPatch tempPatch;
+		for(PxU32 i=0; i<numPatch; ++i)
+		{
+			bool found = false;
+			PCMContactPatch* patch = contactPatch[i];
+			//this mean the patch has't been add to the manifold
+			if(patch->mRoot == patch)
+			{
+				PX_ASSERT(mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
+				for(PxU32 j=0; j<mNumManifolds; ++j)
+				{
+					PX_ASSERT(mManifoldIndices[j] < GU_MAX_MANIFOLD_SIZE);
+					SinglePersistentContactManifold& manifold = *getManifold(j);
+
+					const Vec3V pNor = manifold.getLocalNormal();
+					const FloatV d = V3Dot(patch->mPatchNormal, pNor);
+
+					if(FAllGrtrOrEq(d, acceptanceEpsilon))
+					{
+						//appending the existing contacts to the manifold contact stream
+						for(PxU32 k=0; k< manifold.mNumContacts; ++k)
+						{
+							PxU32 index = k + numManifoldContacts;
+							//not duplicate point
+							PX_ASSERT(index < 64);
+							manifoldContact[index] = manifold.mContactPoints[k];
+						}
+
+						//create a new patch for the exiting manifold
+						tempPatch.mStartIndex = numManifoldContacts;
+						tempPatch.mEndIndex = numManifoldContacts + manifold.mNumContacts;
+						tempPatch.mPatchNormal = pNor;//manifold.getLocalNormal();
+						tempPatch.mRoot = patch;
+						tempPatch.mNextPatch = NULL;
+						
+						patch->mEndPatch->mNextPatch = &tempPatch;
+				
+						//numManifoldContacts += manifold.numContacts;
+						patch->mTotalSize += manifold.mNumContacts;
+						patch->mPatchMaxPen = FMin(patch->mPatchMaxPen, FLoad(mMaxPen[mManifoldIndices[j]]));
+					
+						//manifold.numContacts = 0;
+
+						PX_ASSERT((numManifoldContacts+manifold.mNumContacts) <= 64);
+						const FloatV _maxPen = addBatchManifoldContactsToSingleManifold(&manifold, manifoldContact, numManifoldContacts+manifold.mNumContacts, patch, sqReplaceBreakingThreshold, maxContactsPerManifold);
+						FStore(_maxPen, &mMaxPen[mManifoldIndices[j]]);
+						found = true;
+						break;
+					}
+				}
+
+				if(!found)// && numManifolds < 4)
+				{
+					SinglePersistentContactManifold* manifold = getEmptyManifold();
+					//we still have slot to create a new manifold
+					if(manifold)
+					{
+						const FloatV _maxPen = addBatchManifoldContactsToSingleManifold(manifold, manifoldContact, numManifoldContacts, patch, sqReplaceBreakingThreshold, maxContactsPerManifold);
+						FStore(_maxPen, &mMaxPen[mManifoldIndices[mNumManifolds]]);
+						mNumManifolds++;
+					}	
+					else
+					{
+						//we can't allocate a new manifold  and no existing manifold has the same normal as this patch, we need to find the shallowest penetration manifold. If this manifold is shallower than
+						//the current patch, replace the manifold with the current patch
+						PxU32 index = 0;
+						for(PxU32 j=1; j<mNumManifolds; ++j)
+						{
+							//if(FAllGrtr(mMaxPen[mManifoldIndices[i]], mMaxPen[mManifoldIndices[index]]))
+							if(mMaxPen[mManifoldIndices[j]] > mMaxPen[mManifoldIndices[index]])
+							{
+								index = j;
+							}
+						}
+
+						if(FAllGrtr(FLoad(mMaxPen[mManifoldIndices[index]]), patch->mPatchMaxPen))
+						{
+							manifold = getManifold(index);
+							manifold->mNumContacts = 0;
+							const FloatV _maxPen = addBatchManifoldContactsToSingleManifold(manifold, manifoldContact, numManifoldContacts, patch, sqReplaceBreakingThreshold, maxContactsPerManifold);
+							FStore(_maxPen, &mMaxPen[mManifoldIndices[index]]);
+
+						}
+						return;
+					}
+				}
+			}
+		}
+	}
+}
+
+//This function adds the multi manifold contacts to the contact buffer for box/convexhull
+bool Gu::MultiplePersistentContactManifold::addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::PsTransformV& meshTransform)
+{
+	using namespace Ps::aos;
+	PxU32 contactCount = 0;//contactBuffer.count;
+	PxU32 numContacts = 0;
+	mNumTotalContacts = 0;
+	//drawManifold(*gRenderOutPut, convexTransform, meshTransform);
+	for(PxU32 i=0; i < mNumManifolds; ++i)
+	{
+		Gu::SinglePersistentContactManifold& manifold = *getManifold(i);
+		numContacts = manifold.getNumContacts();
+		PX_ASSERT(mNumTotalContacts + numContacts <= 0xFF);
+		mNumTotalContacts += Ps::to8(numContacts);
+		const Vec3V normal = manifold.getWorldNormal(meshTransform);
+		
+		for(PxU32 j=0; (j< numContacts) & (contactCount < ContactBuffer::MAX_CONTACTS); ++j)
+		{
+			Gu::MeshPersistentContact& p = manifold.getContactPoint(j);
+			
+			const Vec3V worldP =meshTransform.transform(p.mLocalPointB);
+			const FloatV dist = V4GetW(p.mLocalNormalPen);
+			
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactCount++];
+			//Fast allign store
+			V4StoreA(Vec4V_From_Vec3V(normal), reinterpret_cast<PxF32*>(&contact.normal.x));
+			V4StoreA(Vec4V_From_Vec3V(worldP), reinterpret_cast<PxF32*>(&contact.point.x));
+			FStore(dist, &contact.separation);
+
+			contact.internalFaceIndex1 = p.mFaceIndex;
+
+		}
+	}
+
+	PX_ASSERT(contactCount <= 64);
+	contactBuffer.count = contactCount;
+	return contactCount > 0;
+}
+
+//This function adds the multi manifold contacts to the contact buffer for sphere and capsule, radius is corresponding to the sphere radius/capsule radius
+bool Gu::MultiplePersistentContactManifold::addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, const Ps::aos::FloatVArg radius)
+{
+	using namespace Ps::aos;
+	PxU32 contactCount = 0;
+	PxU32 numContacts = 0;
+	mNumTotalContacts = 0;
+
+	for(PxU32 i=0; i < mNumManifolds; ++i)
+	{
+		//get a single manifold
+		Gu::SinglePersistentContactManifold& manifold = *getManifold(i);
+		numContacts = manifold.getNumContacts();
+		PX_ASSERT(mNumTotalContacts + numContacts <= 0xFF);
+		mNumTotalContacts += Ps::to8(numContacts);
+		const Vec3V normal = manifold.getWorldNormal(trB);
+		
+		//iterate all the contacts in this single manifold and add contacts to the contact buffer
+		//each manifold contact point store two points which are in each other's local space. In this
+		//case, mLocalPointA is stored as the center of sphere/a point in the segment for capsule
+		for(PxU32 j=0; (j< numContacts) & (contactCount < ContactBuffer::MAX_CONTACTS); ++j)
+		{
+			Gu::MeshPersistentContact& p = manifold.getContactPoint(j);
+			
+			const Vec3V worldP =V3NegScaleSub(normal, radius, trA.transform(p.mLocalPointA));
+			const FloatV dist = FSub(V4GetW(p.mLocalNormalPen), radius);
+			
+			Gu::ContactPoint& contact = contactBuffer.contacts[contactCount++];
+			//Fast allign store
+			V4StoreA(Vec4V_From_Vec3V(normal), reinterpret_cast<PxF32*>(&contact.normal.x));
+			V4StoreA(Vec4V_From_Vec3V(worldP), reinterpret_cast<PxF32*>(&contact.point.x));
+			FStore(dist, &contact.separation);
+
+			contact.internalFaceIndex1 = p.mFaceIndex;
+
+		}
+	}
+
+	PX_ASSERT(contactCount <= 64);
+	contactBuffer.count = contactCount;
+	return contactCount > 0;
+}
+
+
+/*
+	This function copies the mesh persistent contact from compress buffer(NpCacheStreamPair in the PxcNpThreadContext) to the multiple manifold
+*/
+// PT: function moved to cpp to go around a compiler bug on PS4
+void physx::Gu::MultiplePersistentContactManifold::fromBuffer(PxU8* PX_RESTRICT buffer)
+{
+	using namespace Ps::aos;
+	PxU32 numManifolds = 0;
+	if(buffer != NULL)
+	{
+		PX_ASSERT(((uintptr_t(buffer)) & 0xF) == 0);
+		PxU8* PX_RESTRICT buff = buffer;
+		MultiPersistentManifoldHeader* PX_RESTRICT header = reinterpret_cast<MultiPersistentManifoldHeader*>(buff);
+		buff += sizeof(MultiPersistentManifoldHeader);
+
+		numManifolds = header->mNumManifolds;
+
+		PX_ASSERT(numManifolds <= GU_MAX_MANIFOLD_SIZE);
+		mRelativeTransform = header->mRelativeTransform;
+
+		for(PxU32 a = 0; a < numManifolds; ++a)
+		{
+			mManifoldIndices[a] = PxU8(a);
+			SingleManifoldHeader* PX_RESTRICT manHeader = reinterpret_cast<SingleManifoldHeader*>(buff);
+			buff += sizeof(SingleManifoldHeader);
+			PxU32 numContacts = manHeader->mNumContacts;
+			PX_ASSERT(numContacts <= GU_SINGLE_MANIFOLD_CACHE_SIZE);
+			SinglePersistentContactManifold& manifold = mManifolds[a];
+			manifold.mNumContacts = numContacts;
+			PX_ASSERT((uintptr_t(buff) & 0xf) == 0);
+			CachedMeshPersistentContact* contacts = reinterpret_cast<CachedMeshPersistentContact*>(buff);
+			for(PxU32 b=0; b<manifold.mNumContacts; ++b)
+			{
+				manifold.mContactPoints[b].mLocalPointA = Vec3V_From_Vec4V(V4LoadA(&contacts[b].mLocalPointA.x));
+				manifold.mContactPoints[b].mLocalPointB = Vec3V_From_Vec4V(V4LoadA(&contacts[b].mLocalPointB.x));
+				manifold.mContactPoints[b].mLocalNormalPen = V4LoadA(&contacts[b].mLocalNormal.x);
+				manifold.mContactPoints[b].mFaceIndex = contacts[b].mFaceIndex;
+			}
+			buff += sizeof(Gu::CachedMeshPersistentContact) * numContacts;
+		}
+	}
+	else
+	{
+		mRelativeTransform.Invalidate();
+	}
+	mNumManifolds = PxU8(numManifolds);
+	for(PxU32 a = numManifolds; a < GU_MAX_MANIFOLD_SIZE; ++a)
+	{
+		mManifoldIndices[a] = PxU8(a);
+	}
+}
diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h
new file mode 100644
index 00000000..7ee6c484
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.h
@@ -0,0 +1,846 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef Gu_PERSISTENTCONTACTMANIFOLD_H
+#define Gu_PERSISTENTCONTACTMANIFOLD_H
+
+#include "PxPhysXCommonConfig.h"
+#include "foundation/PxUnionCast.h"
+#include "foundation/PxMemory.h"
+#include "CmPhysXCommon.h"
+#include "PsVecTransform.h"
+
+#define PCM_LOW_LEVEL_DEBUG	0
+
+namespace physx
+{
+
+#define VISUALIZE_PERSISTENT_CONTACT 1
+//This is for pritimives vs primitives 
+#define GU_MANIFOLD_CACHE_SIZE 4
+//These are for pritimives vs mesh
+#define GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE 5
+#define GU_SINGLE_MANIFOLD_CACHE_SIZE 6
+#define GU_SPHERE_MANIFOLD_CACHE_SIZE 1
+#define GU_CAPSULE_MANIFOLD_CACHE_SIZE 3
+#define GU_MAX_MANIFOLD_SIZE 6
+#define GU_MESH_CONTACT_REDUCTION_THRESHOLD	16
+
+
+//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 
+//recycling thresholds are proportionate to margin so it makes it less likely to discard previous contacts due to separation
+#define GU_PCM_MESH_MANIFOLD_EPSILON 0.05f
+
+
+namespace Cm
+{
+	class RenderOutput;
+}
+
+  
+namespace Gu
+{
+	struct ContactPoint;
+	class ContactBuffer;
+
+extern const PxF32 invalidateThresholds[5]; 
+extern const PxF32 invalidateQuatThresholds[5];
+extern const PxF32 invalidateThresholds2[3]; 
+extern const PxF32 invalidateQuatThresholds2[3];
+
+
+Ps::aos::Mat33V findRotationMatrixFromZAxis(const Ps::aos::Vec3VArg to);
+
+//This contact is used in the primitives vs primitives contact gen
+class PersistentContact
+{
+public:
+	PersistentContact()
+	{
+	}
+
+	PersistentContact(Ps::aos::Vec3V _localPointA, Ps::aos::Vec3V _localPointB, Ps::aos::Vec4V _localNormalPen) : 
+	mLocalPointA(_localPointA), mLocalPointB(_localPointB), mLocalNormalPen(_localNormalPen)
+	{
+
+	}
+
+	Ps::aos::Vec3V mLocalPointA;
+	Ps::aos::Vec3V mLocalPointB;
+	Ps::aos::Vec4V mLocalNormalPen; // the (x, y, z) is the local normal, and the w is the penetration depth
+};
+
+//This contact is used in the mesh contact gen to store an extra variable
+class MeshPersistentContact : public PersistentContact
+{
+public:
+	PxU32 mFaceIndex;
+};
+
+//This contact is used in the compress stream buffer(NpCacheStreamPair in the PxcNpThreadContext) to store the data we need
+PX_ALIGN_PREFIX(16)
+class CachedMeshPersistentContact
+{
+public:
+	PxVec3 mLocalPointA; //16 byte aligned
+	PxU32 mFaceIndex;	 //face index
+	PxVec3 mLocalPointB; //16 byte aligned
+	PxU32  mPad;		 //pad
+	PxVec3 mLocalNormal; //16 byte aligned
+	PxReal mPen;		 //penetration
+}PX_ALIGN_SUFFIX(16);
+
+
+#if PX_VC 
+    #pragma warning(push)   
+	#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
+#endif
+
+//This class is used to store the start and end index of the mesh persistent contacts in an array.
+struct PCMContactPatch
+{
+public:
+	PCMContactPatch()
+	{
+		mNextPatch = NULL;
+		mEndPatch = NULL;
+		mRoot = this;
+		mPatchMaxPen = Ps::aos::FMax();
+	}
+	Ps::aos::Vec3V mPatchNormal;
+	PCMContactPatch* mNextPatch;//store the next patch pointer in the patch list
+	PCMContactPatch* mEndPatch;//store the last patch pointer in the patch list
+	PCMContactPatch* mRoot;//if this is the start of the patch list which has very similar patch normal, the root will be itself
+	Ps::aos::FloatV mPatchMaxPen;//store the deepest penetration of the whole patch
+	PxU32 mStartIndex;//store the start index of the manifold contacts in the manifold contacts stream
+	PxU32 mEndIndex;//store the end index of the manifold contacts in the manifold contacts stream
+	PxU32 mTotalSize;//if this is the root, the total size will store the total number of manifold contacts in the whole patch list
+};
+
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+//ML: this is needed because it seems NEON doesn't force the alignment on SIMD type, which cause some of the PCM unit tests fail
+PX_ALIGN_PREFIX(16)
+class PersistentContactManifold
+{
+public:
+
+	PersistentContactManifold(PersistentContact* contactPointsBuff, PxU8 capacity): mNumContacts(0), mCapacity(capacity), mNumWarmStartPoints(0), mContactPoints(contactPointsBuff)
+	{
+		mRelativeTransform.Invalidate();
+	}
+
+	PX_FORCE_INLINE PxU32	getNumContacts() const { return mNumContacts;}
+
+	PX_FORCE_INLINE bool	isEmpty() { return mNumContacts==0; }
+
+	PX_FORCE_INLINE PersistentContact& getContactPoint(const PxU32 index)
+	{
+		PX_ASSERT(index < GU_MANIFOLD_CACHE_SIZE);
+		return mContactPoints[index];
+	}
+	
+	PX_FORCE_INLINE Ps::aos::FloatV maxTransformdelta(const Ps::aos::PsTransformV& curTransform)
+	{
+		using namespace Ps::aos;
+		const Vec4V p0 = Vec4V_From_Vec3V(mRelativeTransform.p);
+		const Vec4V q0 = mRelativeTransform.q;
+		const Vec4V p1 = Vec4V_From_Vec3V(curTransform.p);
+		const Vec4V q1 = curTransform.q;
+
+		const Vec4V dp = V4Abs(V4Sub(p1, p0));
+		const Vec4V dq = V4Abs(V4Sub(q1, q0));
+
+		const Vec4V max0 = V4Max(dp, dq);
+
+		//need to work out max from a single vector...
+		return V4ExtractMax(max0);
+	}
+
+	PX_FORCE_INLINE Ps::aos::Vec4V maxTransformdelta2(const Ps::aos::PsTransformV& curTransform)
+	{
+		using namespace Ps::aos;
+		const Vec4V p0 = Vec4V_From_Vec3V(mRelativeTransform.p);
+		const Vec4V q0 = mRelativeTransform.q;
+		const Vec4V p1 = Vec4V_From_Vec3V(curTransform.p);
+		const Vec4V q1 = curTransform.q;
+
+		const Vec4V dp = V4Abs(V4Sub(p1, p0));
+		const Vec4V dq = V4Abs(V4Sub(q1, q0));
+
+		const Vec4V max0 = V4Max(dp, dq);
+
+		//need to work out max from a single vector...
+		return max0;
+	}
+
+	PX_FORCE_INLINE Ps::aos::FloatV maxTransformPositionDelta(const Ps::aos::Vec3V& curP)
+	{
+		using namespace Ps::aos;
+	
+		const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
+		const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
+		//need to work out max from a single vector...
+		return V4ExtractMax(delta);
+	}
+
+	PX_FORCE_INLINE Ps::aos::FloatV maxTransformQuatDelta(const Ps::aos::QuatV& curQ)
+	{
+		using namespace Ps::aos;
+	
+		const Vec4V deltaQ = V4Sub(curQ, mRelativeTransform.q);
+		const Vec4V delta = V4Abs(deltaQ);
+		//need to work out max from a single vector...
+		return V4ExtractMax(delta);
+	}
+
+	PX_FORCE_INLINE void setRelativeTransform(const Ps::aos::PsTransformV& transform)
+	{
+		mRelativeTransform = transform;
+	}
+
+	//This is used for the box/convexhull vs box/convexhull contact gen to decide whether the relative movement of a pair of objects are 
+	//small enough. In this case, we can skip the collision detection all together
+	PX_FORCE_INLINE PxU32 invalidate_BoxConvex(const Ps::aos::PsTransformV& curRTrans, const Ps::aos::FloatVArg minMargin)
+	{
+		using namespace Ps::aos;
+		PX_ASSERT(mNumContacts <= GU_MANIFOLD_CACHE_SIZE);
+		const FloatV ratio = FLoad(invalidateThresholds[mNumContacts]);
+		const FloatV thresholdP = FMul(minMargin, ratio);
+		const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
+
+		const FloatV thresholdQ = FLoad(invalidateQuatThresholds[mNumContacts]);
+		const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
+		const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
+
+		return BAllEqTTTT(con);
+	}
+
+	//This is used for the sphere/capsule vs other primitives contact gen to decide whether the relative movement of a pair of objects are 
+	//small enough. In this case, we can skip the collision detection all together
+	PX_FORCE_INLINE PxU32 invalidate_SphereCapsule(const Ps::aos::PsTransformV& curRTrans, const Ps::aos::FloatVArg minMargin)
+	{
+		using namespace Ps::aos;
+		PX_ASSERT(mNumContacts <= 2);
+		const FloatV ratio = FLoad(invalidateThresholds2[mNumContacts]);
+		
+		const FloatV thresholdP = FMul(minMargin, ratio);
+		const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
+
+		const FloatV thresholdQ = FLoad(invalidateQuatThresholds2[mNumContacts]);
+		const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
+		const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
+
+		return BAllEqTTTT(con);
+	}
+
+	//This is used for plane contact gen to decide whether the relative movement of a pair of objects are small enough. In this case, 
+	//we can skip the collision detection all together
+	PX_FORCE_INLINE PxU32 invalidate_PrimitivesPlane(const Ps::aos::PsTransformV& curRTrans, const Ps::aos::FloatVArg minMargin, const Ps::aos::FloatVArg ratio)
+	{
+		using namespace Ps::aos;
+		const FloatV thresholdP = FMul(minMargin, ratio);
+		const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
+	
+		const FloatV thresholdQ = FLoad(0.9998f);//about 1 degree
+		const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
+		const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
+
+		return BAllEqTTTT(con);
+	}
+
+	PX_FORCE_INLINE void removeContactPoint (PxU32 index)
+	{
+		mNumContacts--;
+		mContactPoints[index] = mContactPoints[mNumContacts];
+	}      
+    
+	bool validContactDistance(const PersistentContact& pt, const Ps::aos::FloatVArg breakingThreshold) const
+	{
+		using namespace Ps::aos;
+		const FloatV dist = V4GetW(pt.mLocalNormalPen);
+		return FAllGrtr(breakingThreshold, dist) != 0;
+	}
+
+	PX_FORCE_INLINE void clearManifold()
+	{
+		mNumWarmStartPoints = 0;
+		mNumContacts = 0;
+		mRelativeTransform.Invalidate();
+	}
+
+	PX_FORCE_INLINE void initialize()
+	{
+		clearManifold();
+	}
+
+	//This function is used to replace the existing contact with the newly created contact if their distance are within some threshold
+	bool replaceManifoldPoint(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen, const Ps::aos::FloatVArg replaceBreakingThreshold);
+
+	//This function is to add a point(in box/convexhull contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
+	PxU32 addManifoldPoint( const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalAPen, const Ps::aos::FloatVArg replaceBreakingThreshold);
+	//This function is to add a point(in capsule contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
+	PxU32 addManifoldPoint2( const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalAPen, const Ps::aos::FloatVArg replaceBreakingThreshold);//max two points of contacts  
+	//This function is used in box-plane contact gen to add the plane contacts to the manifold 
+	void addBatchManifoldContactsCluster( const PersistentContact* manifoldPoints, const PxU32 numPoints);      
+	
+	//This function is used in the capsule full manifold contact genenation(maximum 2 points). 
+	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);
+	//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);
+
+	//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);
+	//This function is used for incremental manifold contact reduction for capsule
+	PxU32 reduceContactSegment(const Ps::aos::Vec3VArg localPointA, const Ps::aos::Vec3VArg localPointB, const Ps::aos::Vec4VArg localNormalPen);
+  
+
+	/*
+		This function recalculate the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
+		we will keep the contacts; Otherwise, we will remove the contacts.
+	*/
+	void refreshContactPoints(const Ps::aos::PsMatTransformV& relTra, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg contactOffset);
+	//This function is just used in boxbox contact gen for fast transform
+	void addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsMatTransformV& transf1);
+	//This function is for adding box/convexhull manifold contacts to the contact buffer
+	void addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsTransformV& transf1, const Ps::aos::FloatVArg contactOffset);
+	//This function is for adding sphere/capsule manifold contacts to the contact buffer
+	void addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::Vec3VArg normal, const Ps::aos::PsTransformV& transf0, const Ps::aos::FloatVArg radius, const Ps::aos::FloatVArg contactOffset);
+
+	//get the average normal in the manifold in world space
+	Ps::aos::Vec3V getWorldNormal(const Ps::aos::PsTransformV& trB);
+	//get the average normal in the manifold in local B object space
+	Ps::aos::Vec3V getLocalNormal();
+	
+	void drawManifold(Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB);
+	void drawManifold(Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, const Ps::aos::FloatVArg radius);
+	void drawManifold(const PersistentContact& m, Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB);
+	static void drawPoint(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p, const PxF32 size, const PxU32 color = 0x0000ff00);
+	static void drawLine(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const PxU32 color = 0xff00ffff);  
+	static void drawTriangle(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const Ps::aos::Vec3VArg p2, const PxU32 color = 0xffff0000);
+	static void drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color = 0xff00ffff);
+	static void drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsMatTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color = 0xff00ffff);
+
+	Ps::aos::PsTransformV mRelativeTransform;//aToB
+	PxU8 mNumContacts;
+	PxU8 mCapacity;
+	PxU8 mNumWarmStartPoints;
+	PxU8 mAIndice[4];
+	PxU8 mBIndice[4]; 
+	PersistentContact* mContactPoints;
+} PX_ALIGN_SUFFIX(16);
+
+PX_ALIGN_PREFIX(16)
+class LargePersistentContactManifold : public PersistentContactManifold
+{
+public:
+	LargePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_MANIFOLD_CACHE_SIZE)
+	{
+	}
+
+	PersistentContact mContactPointsBuff[GU_MANIFOLD_CACHE_SIZE];
+}PX_ALIGN_SUFFIX(16);
+
+PX_ALIGN_PREFIX(16)
+class SpherePersistentContactManifold : public PersistentContactManifold
+{
+public:
+	SpherePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_SPHERE_MANIFOLD_CACHE_SIZE)
+	{
+	}
+
+	PersistentContact mContactPointsBuff[GU_SPHERE_MANIFOLD_CACHE_SIZE];
+}PX_ALIGN_SUFFIX(16);
+
+PX_ALIGN_PREFIX(16)
+class SinglePersistentContactManifold
+{
+public:
+
+	SinglePersistentContactManifold(): mNumContacts(0)
+	{
+	}
+
+	PX_FORCE_INLINE PxU32	getNumContacts() const { return mNumContacts;}
+
+	PX_FORCE_INLINE bool	isEmpty() { return mNumContacts==0; }
+
+	PX_FORCE_INLINE MeshPersistentContact& getContactPoint(const PxU32 index)
+	{
+		PX_ASSERT(index < GU_SINGLE_MANIFOLD_CACHE_SIZE);
+		return mContactPoints[index];
+	}
+
+	PX_FORCE_INLINE void removeContactPoint (PxU32 index)
+	{
+		mNumContacts--;
+		mContactPoints[index] = mContactPoints[mNumContacts];
+	}  
+
+	PX_FORCE_INLINE void clearManifold()
+	{
+		mNumContacts = 0;
+	}
+
+	PX_FORCE_INLINE void initialize()
+	{
+		clearManifold();
+	}
+
+
+	PX_FORCE_INLINE Ps::aos::Vec3V getWorldNormal(const Ps::aos::PsTransformV& trB)
+	{
+		using namespace Ps::aos;
+		Vec4V nPen = mContactPoints[0].mLocalNormalPen;
+		for(PxU32 i =1; i < mNumContacts; ++i)
+		{
+			nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
+		}
+
+		const Vec3V n = Vec3V_From_Vec4V(nPen);
+		return V3Normalize(trB.rotate(n));
+	}
+
+	PX_FORCE_INLINE Ps::aos::Vec3V getLocalNormal()
+	{
+		using namespace Ps::aos;
+		Vec4V nPen = mContactPoints[0].mLocalNormalPen;
+		for(PxU32 i =1; i < mNumContacts; ++i)
+		{
+			nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
+		}
+		return V3Normalize(Vec3V_From_Vec4V(nPen));
+	}
+	
+	//This function reduces the manifold contact list in a patch for box/convexhull vs mesh
+	Ps::aos::FloatV reduceBatchContactsConvex(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch);
+	//This function reduces the manifold contact list in a patch for sphere vs mesh
+	Ps::aos::FloatV reduceBatchContactsSphere(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch);
+	//This function reduces the manifold contact list in a pathc for capsuel vs mesh
+	Ps::aos::FloatV reduceBatchContactsCapsule(const MeshPersistentContact* manifoldContactExt, const PxU32 numContacts, PCMContactPatch& patch);
+
+	//This function adds the manifold contact list in a patch for box/convexhull vs mesh
+	Ps::aos::FloatV addBatchManifoldContactsConvex(const MeshPersistentContact* manifoldContact, const PxU32 numContacts, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold);
+	//This function adds the manifold contact list in a patch for sphere vs mesh
+	Ps::aos::FloatV addBatchManifoldContactsSphere(const MeshPersistentContact* manifoldContact, const PxU32 numContacts, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold);
+	//This function adds the manifold contact list in a patch for capsule vs mesh
+	Ps::aos::FloatV addBatchManifoldContactsCapsule(const MeshPersistentContact* manifoldContact, const PxU32 numContacts, PCMContactPatch& patch, const Ps::aos::FloatVArg replaceBreakingThreshold);
+	
+	//This is used for in the addContactsToPatch for convex mesh contact gen. 
+	static PxU32 reduceContacts(MeshPersistentContact* manifoldContactExt, PxU32 numContacts);
+
+	//This function is to recalculate the contacts based on the relative transform between a pair of objects
+	Ps::aos::FloatV refreshContactPoints(const Ps::aos::PsMatTransformV& relTra, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg contactOffset);
+
+	void drawManifold(Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB);
+
+	MeshPersistentContact mContactPoints[GU_SINGLE_MANIFOLD_CACHE_SIZE];//384 bytes
+	PxU32 mNumContacts;//400 bytes
+	
+} PX_ALIGN_SUFFIX(16);
+
+//This is a structure used to cache a multi-persistent-manifold in the cache stream
+struct MultiPersistentManifoldHeader
+{
+	Ps::aos::PsTransformV mRelativeTransform;//aToB
+	PxU32 mNumManifolds;
+	PxU32 pad[3];
+};
+
+struct SingleManifoldHeader
+{
+	PxU32 mNumContacts;
+	PxU32 pad[3];
+};
+
+#if PX_VC 
+#pragma warning(push)
+#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+
+PX_ALIGN_PREFIX(16)
+class PX_PHYSX_COMMON_API MultiplePersistentContactManifold
+{
+public:
+	MultiplePersistentContactManifold():mNumManifolds(0), mNumTotalContacts(0)
+	{
+		mRelativeTransform.Invalidate();
+	}
+
+	PX_FORCE_INLINE void setRelativeTransform(const Ps::aos::PsTransformV& transform)
+	{
+		mRelativeTransform = transform;
+	}
+
+	PX_FORCE_INLINE Ps::aos::FloatV maxTransformPositionDelta(const Ps::aos::Vec3V& curP)
+	{
+		using namespace Ps::aos;
+	
+		const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
+		const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
+		//need to work out max from a single vector...
+		return V4ExtractMax(delta);
+	}
+
+	PX_FORCE_INLINE Ps::aos::FloatV maxTransformQuatDelta(const Ps::aos::QuatV& curQ)
+	{
+		using namespace Ps::aos;
+	
+		const Vec4V deltaQ = V4Sub(curQ, mRelativeTransform.q);
+		const Vec4V delta = V4Abs(deltaQ);
+		//need to work out max from a single vector...
+		return V4ExtractMax(delta);
+	}
+
+	PX_FORCE_INLINE PxU32 invalidate(const Ps::aos::PsTransformV& curRTrans, const Ps::aos::FloatVArg minMargin, const Ps::aos::FloatVArg ratio)
+	{
+		using namespace Ps::aos;
+		
+		const FloatV thresholdP = FMul(minMargin, ratio);
+		const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
+		const FloatV thresholdQ = FLoad(0.9998f);//about 1 degree
+		const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
+		const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
+
+		return BAllEqTTTT(con);
+	}
+
+	PX_FORCE_INLINE PxU32 invalidate(const Ps::aos::PsTransformV& curRTrans, const Ps::aos::FloatVArg minMargin)
+	{
+		using namespace Ps::aos;
+		return invalidate(curRTrans, minMargin, FLoad(0.2f));
+	}
+
+	/*
+		This function work out the contact patch connectivity. If two patches's normal are within 5 degree, we would link these two patches together and reset the total size.
+	*/
+	PX_FORCE_INLINE void refineContactPatchConnective(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, const Ps::aos::FloatVArg acceptanceEpsilon)
+	{
+		PX_UNUSED(manifoldContacts);
+
+		using namespace Ps::aos;
+	
+		//work out the contact patch connectivity, the patchNormal should be in the local space of mesh
+		for(PxU32 i=0; i<numContactPatch; ++i)
+		{
+			PCMContactPatch* patch = contactPatch[i];
+			patch->mRoot = patch;
+			patch->mEndPatch = patch;
+			patch->mTotalSize = patch->mEndIndex - patch->mStartIndex;
+			patch->mNextPatch = NULL;
+	
+			for(PxU32 j=i; j>0; --j)
+			{
+				PCMContactPatch* other = contactPatch[j-1];
+				const FloatV d = V3Dot(patch->mPatchNormal, other->mRoot->mPatchNormal);
+				if(FAllGrtrOrEq(d, acceptanceEpsilon))//less than 5 degree
+				{
+					
+					other->mNextPatch = patch;
+					other->mRoot->mEndPatch = patch;
+					patch->mRoot = other->mRoot;
+					other->mRoot->mTotalSize += patch->mEndIndex - patch->mStartIndex;
+					break;
+				}
+			}
+		}
+	}
+
+
+	/*
+		This function uses to reduce the manifold contacts which are in different connected patchs but are within replace breaking threshold 
+	*/
+	PX_FORCE_INLINE PxU32 reduceManifoldContactsInDifferentPatches(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, PxU32 numContacts, const Ps::aos::FloatVArg sqReplaceBreaking)
+	{
+		using namespace Ps::aos;
+
+		for(PxU32 i=0; i<numContactPatch; ++i)
+		{
+			PCMContactPatch* currentPatch = contactPatch[i];
+			//this make sure the patch is the root before we do the contact reduction, otherwise, we will do duplicate work
+			if(currentPatch->mRoot == currentPatch)
+			{
+				while(currentPatch)
+				{
+					PCMContactPatch* nextPatch = currentPatch->mNextPatch;
+					if(nextPatch)
+					{
+						for(PxU32 k = currentPatch->mStartIndex; k<currentPatch->mEndIndex; ++k)
+						{
+							for(PxU32 l = nextPatch->mStartIndex; l < nextPatch->mEndIndex; ++l)
+							{
+								Vec3V dif = V3Sub(manifoldContacts[l].mLocalPointB, manifoldContacts[k].mLocalPointB);
+								FloatV d = V3Dot(dif, dif);
+								if(FAllGrtr(sqReplaceBreaking, d))
+								{
+									//if two manifold contacts are within threshold, we will get rid of the manifold contacts in the other contact patch
+									manifoldContacts[l] = manifoldContacts[nextPatch->mEndIndex-1];
+									nextPatch->mEndIndex--;
+									numContacts--;
+									l--;
+								}
+							}
+						}
+					}
+					currentPatch = nextPatch;
+				}
+			}
+		}
+	
+		return numContacts;
+	}
+
+
+	/*
+		This function is for the multiple manifold loop through each individual single manifold to recalculate the contacts based on the relative transform between a pair of objects
+	*/
+	PX_FORCE_INLINE void refreshManifold(const Ps::aos::PsMatTransformV& relTra, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg contactDist)
+	{
+		using namespace Ps::aos;
+	
+		//refresh manifold contacts
+		for(PxU32 i=0; i < mNumManifolds; ++i)
+		{
+			PxU8 ind = mManifoldIndices[i];
+			PX_ASSERT(mManifoldIndices[i] < GU_MAX_MANIFOLD_SIZE);
+			PxU32 nextInd = PxMin(i, mNumManifolds-2u)+1;
+			Ps::prefetchLine(&mManifolds[mManifoldIndices[nextInd]]);
+			Ps::prefetchLine(&mManifolds[mManifoldIndices[nextInd]],128);
+			Ps::prefetchLine(&mManifolds[mManifoldIndices[nextInd]],256);
+			FloatV _maxPen = mManifolds[ind].refreshContactPoints(relTra, projectBreakingThreshold, contactDist);
+			if(mManifolds[ind].isEmpty())
+			{
+				//swap the index with the next manifolds
+				PxU8 index = mManifoldIndices[--mNumManifolds];
+				mManifoldIndices[mNumManifolds] = ind;
+				mManifoldIndices[i] = index;
+				i--;
+			}
+			else
+			{
+				FStore(_maxPen, &mMaxPen[ind]);
+			}
+		}
+	}
+
+
+	PX_FORCE_INLINE void initialize()
+	{
+		mNumManifolds = 0;
+		mNumTotalContacts = 0;
+		mRelativeTransform.Invalidate();
+		for(PxU8 i=0; i<GU_MAX_MANIFOLD_SIZE; ++i)
+		{
+			mManifolds[i].initialize();
+			mManifoldIndices[i] = i;
+		}
+	}
+
+	PX_FORCE_INLINE void clearManifold()
+	{
+		for(PxU8 i=0; i<mNumManifolds; ++i)
+		{
+			mManifolds[i].clearManifold();
+		}
+		mNumManifolds = 0;
+		mNumTotalContacts = 0;
+		mRelativeTransform.Invalidate();
+	}
+
+	PX_FORCE_INLINE SinglePersistentContactManifold* getManifold(const PxU32 index)
+	{
+		PX_ASSERT(index < GU_MAX_MANIFOLD_SIZE);
+		return &mManifolds[mManifoldIndices[index]];
+	}
+
+	PX_FORCE_INLINE SinglePersistentContactManifold* getEmptyManifold()
+	{
+		if(mNumManifolds < GU_MAX_MANIFOLD_SIZE)
+			return  &mManifolds[mManifoldIndices[mNumManifolds]];
+		return NULL;
+	}
+
+	//This function adds the manifold contacts with different patches into the corresponding single persistent contact manifold
+	void addManifoldContactPoints(MeshPersistentContact* manifoldContact, PxU32 numManifoldContacts, PCMContactPatch** contactPatch, const PxU32 numPatch, 
+		const Ps::aos::FloatVArg sqReplaceBreakingThreshold, const Ps::aos::FloatVArg acceptanceEpsilon, PxU8 maxContactsPerManifold);
+	//This function adds the box/convexhull manifold contacts to the contact buffer 
+	bool addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::PsTransformV& transf1);
+	//This function adds the sphere/capsule manifold contacts to the contact buffer
+	bool addManifoldContactsToContactBuffer(Gu::ContactBuffer& contactBuffer, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB, const Ps::aos::FloatVArg radius);
+	void drawManifold(Cm::RenderOutput& out, const Ps::aos::PsTransformV& trA, const Ps::aos::PsTransformV& trB);
+
+	//Code to load from a buffer and store to a buffer.
+	void fromBuffer(PxU8*  PX_RESTRICT buffer);
+	void toBuffer(PxU8*  PX_RESTRICT buffer);
+
+	static void drawLine(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p0, const Ps::aos::Vec3VArg p1, const PxU32 color = 0xff00ffff);
+	static void drawLine(Cm::RenderOutput& out, const PxVec3 p0, const PxVec3 p1, const PxU32 color = 0xff00ffff);
+	static void drawPoint(Cm::RenderOutput& out, const Ps::aos::Vec3VArg p, const PxF32 size, const PxU32 color = 0x00ff0000);
+	static void drawPolygon( Cm::RenderOutput& out, const Ps::aos::PsTransformV& transform,  Ps::aos::Vec3V* points, const PxU32 numVerts, const PxU32 color = 0xff00ffff);
+
+	Ps::aos::PsTransformV mRelativeTransform;//aToB
+	PxF32 mMaxPen[GU_MAX_MANIFOLD_SIZE];
+	PxU8 mManifoldIndices[GU_MAX_MANIFOLD_SIZE];
+	PxU8 mNumManifolds;
+	PxU8 mNumTotalContacts;
+	SinglePersistentContactManifold mManifolds[GU_MAX_MANIFOLD_SIZE];
+	
+	
+} PX_ALIGN_SUFFIX(16);
+
+#if PX_VC 
+#pragma warning(pop)
+#endif
+
+/*
+	This function calculates the average normal in the manifold in world space
+*/
+PX_FORCE_INLINE Ps::aos::Vec3V PersistentContactManifold::getWorldNormal(const Ps::aos::PsTransformV& trB)
+{
+	using namespace Ps::aos;
+	
+	Vec4V nPen = mContactPoints[0].mLocalNormalPen;
+	for(PxU32 i =1; i < mNumContacts; ++i)
+	{
+		nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
+	}
+
+	const Vec3V n = Vec3V_From_Vec4V(nPen);
+	const FloatV sqLength = V3Dot(n, n);
+	const Vec3V nn = V3Sel(FIsGrtr(sqLength, FEps()), n, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen));
+	return V3Normalize(trB.rotate(nn));
+}
+
+/*
+	This function calculates the average normal in the manifold in local B space
+*/
+PX_FORCE_INLINE Ps::aos::Vec3V PersistentContactManifold::getLocalNormal()
+{
+	using namespace Ps::aos;
+	
+	Vec4V nPen = mContactPoints[0].mLocalNormalPen;
+	for(PxU32 i =1; i < mNumContacts; ++i)
+	{
+		nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
+	}
+	return V3Normalize(Vec3V_From_Vec4V(nPen));
+}
+
+/*
+	This function recalculates the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
+	we will keep the contacts; Otherwise, we will remove the contacts.
+*/
+PX_FORCE_INLINE void PersistentContactManifold::refreshContactPoints(const Ps::aos::PsMatTransformV& aToB, const Ps::aos::FloatVArg projectBreakingThreshold, const Ps::aos::FloatVArg /*contactOffset*/)
+{
+	using namespace Ps::aos;
+	const FloatV sqProjectBreakingThreshold =  FMul(projectBreakingThreshold, projectBreakingThreshold); 
+
+	// first refresh worldspace positions and distance
+	for (PxU32 i=mNumContacts; i > 0; --i)
+	{
+		PersistentContact& manifoldPoint = mContactPoints[i-1];
+		const Vec3V localAInB = aToB.transform( manifoldPoint.mLocalPointA ); // from a to b
+		const Vec3V localBInB = manifoldPoint.mLocalPointB;
+		const Vec3V v = V3Sub(localAInB, localBInB); 
+
+		const Vec3V localNormal = Vec3V_From_Vec4V(manifoldPoint.mLocalNormalPen); // normal in b space
+		const FloatV dist= V3Dot(v, localNormal);
+
+		const Vec3V projectedPoint = V3NegScaleSub(localNormal,  dist, localAInB);//manifoldPoint.worldPointA - manifoldPoint.worldPointB * manifoldPoint.m_distance1;
+		const Vec3V projectedDifference = V3Sub(localBInB, projectedPoint);
+
+		const FloatV distance2d = V3Dot(projectedDifference, projectedDifference);
+		//const BoolV con = BOr(FIsGrtr(dist, contactOffset), FIsGrtr(distance2d, sqProjectBreakingThreshold));
+		const BoolV con = FIsGrtr(distance2d, sqProjectBreakingThreshold);
+		if(BAllEqTTTT(con))
+		{
+			removeContactPoint(i-1);
+		} 
+		else
+		{
+			manifoldPoint.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), dist);
+		}
+	}
+}
+
+/*
+	This function copies the mesh persistent contact from the multiple manifold to compress buffer(NpCacheStreamPair in the PxcNpThreadContext)
+*/
+PX_INLINE void MultiplePersistentContactManifold::toBuffer(PxU8*  PX_RESTRICT buffer)
+{
+	using namespace Ps::aos;
+	PxU8* buff = buffer;
+
+	PX_ASSERT(((uintptr_t(buff)) & 0xF) == 0);
+	MultiPersistentManifoldHeader* PX_RESTRICT header = reinterpret_cast<MultiPersistentManifoldHeader*>(buff);
+	buff += sizeof(MultiPersistentManifoldHeader);
+
+	PX_ASSERT(mNumManifolds <= GU_MAX_MANIFOLD_SIZE);
+	header->mNumManifolds = mNumManifolds;
+	header->mRelativeTransform = mRelativeTransform;
+
+	for(PxU32 a = 0; a < mNumManifolds; ++a)
+	{
+		SingleManifoldHeader* manHeader = reinterpret_cast<SingleManifoldHeader*>(buff);
+		buff += sizeof(SingleManifoldHeader);
+		SinglePersistentContactManifold& manifold = *getManifold(a);
+		manHeader->mNumContacts = manifold.mNumContacts;
+		PX_ASSERT((uintptr_t(buff) & 0xf) == 0);
+		CachedMeshPersistentContact* contacts = reinterpret_cast<CachedMeshPersistentContact*>(buff);
+		//convert the mesh persistent contact to cached mesh persistent contact to save 16 byte memory per contact
+		for(PxU32 b = 0; b<manifold.mNumContacts; ++b)
+		{	
+			V4StoreA(Vec4V_From_Vec3V(manifold.mContactPoints[b].mLocalPointA), &contacts[b].mLocalPointA.x);
+			V4StoreA(Vec4V_From_Vec3V(manifold.mContactPoints[b].mLocalPointB), &contacts[b].mLocalPointB.x);
+			V4StoreA(manifold.mContactPoints[b].mLocalNormalPen, &contacts[b].mLocalNormal.x);
+			//Note - this must be written last because we just wrote mLocalPointA to this memory so need to make sure
+			//that face index is written after that.
+			contacts[b].mFaceIndex = manifold.mContactPoints[b].mFaceIndex;
+		}
+		buff += sizeof(CachedMeshPersistentContact) * manifold.mNumContacts;
+	}
+}
+
+#define PX_CP_TO_PCP(contactPoint)				(reinterpret_cast<PersistentContact*>(contactPoint)) //this is used in the normal pcm contact gen
+#define PX_CP_TO_MPCP(contactPoint)				(reinterpret_cast<MeshPersistentContact*>(contactPoint))//this is used in the mesh pcm contact gen
+
+}//Gu
+}//physx
+
+#endif