From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001
From: git perforce import user <a@b>
Date: Tue, 25 Oct 2016 12:29:14 -0600
Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @
 21275617

[CL 21300167]
---
 .../src/pcm/GuPersistentContactManifold.cpp        | 2281 ++++++++++++++++++++
 1 file changed, 2281 insertions(+)
 create mode 100644 PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp

(limited to 'PhysX_3.4/Source/GeomUtils/src/pcm/GuPersistentContactManifold.cpp')

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);
+	}
+}
-- 
cgit v1.2.3