Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

8 files changed, 6544 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp
new file mode 100644
index 00000000..4dd7d182
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp
@@ -0,0 +1,2020 @@
+// 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/PxProfiler.h"
+#include "PxsContactManager.h"
+#include "PxsContext.h"
+#include "PxsRigidBody.h"
+#include "PxcContactMethodImpl.h"
+#include "GuContactPoint.h"
+#include "PxsCCD.h"
+#include "PsSort.h"
+#include "PsAtomic.h"
+#include "CmFlushPool.h"
+#include "PxsMaterialManager.h"
+#include "PxcMaterialMethodImpl.h"
+#include "PxsMaterialManager.h"
+#include "PxsMaterialCombiner.h"
+#include "PxcNpContactPrepShared.h"
+#include "PxvGeometry.h"
+#include "DyThresholdTable.h"
+#include "GuCCDSweepConvexMesh.h"
+#include "PsUtilities.h"
+#include "foundation/PxProfiler.h"
+#include "GuBounds.h"
+
+#if DEBUG_RENDER_CCD
+#include "CmRenderOutput.h"
+#include "CmRenderBuffer.h"
+#include "PxPhysics.h"
+#include "PxScene.h"
+#endif
+
+#define DEBUG_RENDER_CCD_FIRST_PASS_ONLY	1
+#define DEBUG_RENDER_CCD_ATOM_PTR			0
+#define DEBUG_RENDER_CCD_NORMAL				1
+
+
+using namespace physx;
+using namespace physx::shdfnd;
+using namespace physx::Dy;
+using namespace Gu;
+
+
+static PX_FORCE_INLINE void verifyCCDPair(const PxsCCDPair& /*pair*/)
+{
+#if 0
+	if (pair.mBa0)
+		pair.mBa0->getPose();
+	if (pair.mBa1)
+		pair.mBa1->getPose();
+#endif
+}
+
+#if CCD_DEBUG_PRINTS
+namespace physx {
+
+static const char* gGeomTypes[PxGeometryType::eGEOMETRY_COUNT+1] = {
+	"sphere", "plane", "capsule", "box", "convex", "trimesh", "heightfield", "*"
+};
+
+FILE* gCCDLog = NULL;
+
+static inline void openCCDLog()
+{
+	if (gCCDLog)
+	{
+		fclose(gCCDLog);
+		gCCDLog = NULL;
+	}
+	gCCDLog = fopen("c:\\ccd.txt", "wt");
+	fprintf(gCCDLog, ">>>>>>>>>>>>>>>>>>>>>>>>>>> CCD START FRAME <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
+}
+
+static inline void printSeparator(
+	const char* prefix, PxU32 pass, PxsRigidBody* atom0, PxGeometryType::Enum g0, PxsRigidBody* atom1, PxGeometryType::Enum g1)
+{
+	fprintf(gCCDLog, "------- %s pass %d (%s %x) vs (%s %x)\n", prefix, pass, gGeomTypes[g0], atom0, gGeomTypes[g1], atom1);
+	fflush(gCCDLog);
+}
+
+static inline void printCCDToi(
+	const char* header, PxF32 t, const PxVec3& v,
+	PxsRigidBody* atom0, PxGeometryType::Enum g0, PxsRigidBody* atom1, PxGeometryType::Enum g1
+)
+{
+	fprintf(gCCDLog, "%s (%s %x vs %s %x): %.5f, (%.2f, %.2f, %.2f)\n", header, gGeomTypes[g0], atom0, gGeomTypes[g1], atom1, t, v.x, v.y, v.z);
+	fflush(gCCDLog);
+}
+
+static inline void printCCDPair(const char* header, PxsCCDPair& pair)
+{
+	printCCDToi(header, pair.mMinToi, pair.mMinToiNormal, pair.mBa0, pair.mG0, pair.mBa1, pair.mG1);
+}
+
+// also used in PxcSweepConvexMesh.cpp
+void printCCDDebug(const char* msg, const PxsRigidBody* atom0, PxGeometryType::Enum g0, bool printPtr)
+{
+	fprintf(gCCDLog, "  %s (%s %x)\n", msg, gGeomTypes[g0], printPtr ? atom0 : 0);
+	fflush(gCCDLog);
+}
+
+// also used in PxcSweepConvexMesh.cpp
+void printShape(
+	PxsRigidBody* atom0, PxGeometryType::Enum g0, const char* annotation, PxReal dt, PxU32 pass, bool printPtr)
+{
+	fprintf(gCCDLog, "%s (%s %x) atom=(%.2f, %.2f, %.2f)>(%.2f, %.2f, %.2f) v=(%.1f, %.1f, %.1f), mv=(%.1f, %.1f, %.1f)\n",
+		annotation, gGeomTypes[g0], printPtr ? atom0 : 0,
+		atom0->getLastCCDTransform().p.x, atom0->getLastCCDTransform().p.y, atom0->getLastCCDTransform().p.z,
+		atom0->getPose().p.x, atom0->getPose().p.y, atom0->getPose().p.z,
+		atom0->getLinearVelocity().x, atom0->getLinearVelocity().y, atom0->getLinearVelocity().z,
+		atom0->getLinearMotionVelocity(dt).x, atom0->getLinearMotionVelocity(dt).y, atom0->getLinearMotionVelocity(dt).z );
+	fflush(gCCDLog);
+	#if DEBUG_RENDER_CCD && DEBUG_RENDER_CCD_ATOM_PTR
+	if (!DEBUG_RENDER_CCD_FIRST_PASS_ONLY || pass == 0)
+	{
+		PxScene *s; PxGetPhysics()->getScenes(&s, 1, 0);
+		Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer())
+			<< Cm::DebugText(atom0->getPose().p, 0.05f, "%x", atom0);
+	}
+	#endif
+}
+
+static inline void flushCCDLog()
+{
+	fflush(gCCDLog);
+}
+
+} // namespace physx
+
+#else
+
+namespace physx
+{
+
+void printShape(PxsRigidBody* /*atom0*/, PxGeometryType::Enum /*g0*/, const char* /*annotation*/, PxReal /*dt*/, PxU32 /*pass*/, bool printPtr = true)
+{PX_UNUSED(printPtr);}
+
+static inline void openCCDLog() {}
+
+static inline void flushCCDLog() {}
+
+void printCCDDebug(const char* /*msg*/, const PxsRigidBody* /*atom0*/, PxGeometryType::Enum /*g0*/, bool printPtr = true) {PX_UNUSED(printPtr);}
+
+static inline void printSeparator(
+	const char* /*prefix*/, PxU32 /*pass*/, PxsRigidBody* /*atom0*/, PxGeometryType::Enum /*g0*/,
+	PxsRigidBody* /*atom1*/, PxGeometryType::Enum /*g1*/) {}
+} // namespace physx
+#endif
+
+namespace
+{
+	// PT: TODO: refactor with ShapeSim version (SIMD)
+	PX_INLINE PxTransform getShapeAbsPose(const PxsShapeCore* shapeCore, const PxsRigidCore* rigidCore, PxU32 isDynamic)
+	{
+		if(isDynamic)
+		{
+			const PxsBodyCore* PX_RESTRICT bodyCore = static_cast<const PxsBodyCore*>(rigidCore);
+			return bodyCore->body2World * bodyCore->getBody2Actor().getInverse() *shapeCore->transform;
+		}
+		else 
+		{
+			return rigidCore->body2World * shapeCore->transform;
+		}
+	}
+}
+
+namespace physx
+{
+
+	PxsCCDContext::PxsCCDContext(PxsContext* context, Dy::ThresholdStream& thresholdStream, PxvNphaseImplementationContext& nPhaseContext) :
+		mPostCCDSweepTask			(this, "PxsContext.postCCDSweep"),
+		mPostCCDAdvanceTask			(this, "PxsContext.postCCDAdvance"),
+		mPostCCDDepenetrateTask		(this, "PxsContext.postCCDDepenetrate"),
+		mDisableCCDResweep			(false),
+		miCCDPass					(0),
+		mSweepTotalHits				(0),
+		mCCDThreadContext			(NULL),
+		mCCDPairsPerBatch			(0),
+		mCCDMaxPasses				(1),
+		mContext					(context),
+		mThresholdStream			(thresholdStream),
+		mNphaseContext(nPhaseContext)
+{
+}
+
+PxsCCDContext::~PxsCCDContext()
+{
+}
+
+PxsCCDContext* PxsCCDContext::create(PxsContext* context, Dy::ThresholdStream& thresholdStream, PxvNphaseImplementationContext& nPhaseContext)
+{
+	PxsCCDContext* dc = reinterpret_cast<PxsCCDContext*>(
+		PX_ALLOC(sizeof(PxsCCDContext), "PxsCCDContext"));
+
+	if(dc)
+	{
+		new(dc) PxsCCDContext(context, thresholdStream, nPhaseContext);
+	}
+	return dc;
+}
+
+void PxsCCDContext::destroy()
+{
+	this->~PxsCCDContext();
+	PX_FREE(this);
+}
+
+PxTransform PxsCCDShape::getAbsPose(const PxsRigidBody* atom)						const
+{
+	// PT: TODO: refactor with ShapeSim version (SIMD) - or with redundant getShapeAbsPose() above in this same file!
+	if(atom)
+		return atom->getPose() * atom->getCore().getBody2Actor().getInverse() * mShapeCore->transform;
+	else
+		return mRigidCore->body2World * mShapeCore->transform;
+}
+
+PxTransform PxsCCDShape::getLastCCDAbsPose(const PxsRigidBody* atom)						const
+{
+	// PT: TODO: refactor with ShapeSim version (SIMD)
+	return atom->getLastCCDTransform() * atom->getCore().getBody2Actor().getInverse() * mShapeCore->transform;
+}
+
+PxReal PxsCCDPair::sweepFindToi(PxcNpThreadContext& context, PxReal dt, PxU32 pass)
+{
+	printSeparator("findToi", pass, mBa0, mG0, NULL, PxGeometryType::eGEOMETRY_COUNT);
+	//Update shape transforms if necessary
+	updateShapes();
+
+	//Extract the bodies
+	PxsRigidBody* atom0 = mBa0;
+	PxsRigidBody* atom1 = mBa1;
+	PxsCCDShape* ccdShape0 = mCCDShape0;
+	PxsCCDShape* ccdShape1 = mCCDShape1;
+	PxGeometryType::Enum g0 = mG0, g1 = mG1;
+
+	//If necessary, flip the bodies to make sure that g0 <= g1
+	if(mG1 < mG0)
+	{
+		g0 = mG1;
+		g1 = mG0;
+		ccdShape0 = mCCDShape1;
+		ccdShape1 = mCCDShape0;
+		atom0 = mBa1;
+		atom1 = mBa0;
+	}
+	
+	PX_ALIGN(16, PxTransform tm0);
+	PX_ALIGN(16, PxTransform tm1);
+	PX_ALIGN(16, PxTransform lastTm0);
+	PX_ALIGN(16, PxTransform lastTm1);
+
+	tm0 = ccdShape0->mCurrentTransform;
+	lastTm0 = ccdShape0->mPrevTransform;
+
+	tm1 = ccdShape1->mCurrentTransform;
+	lastTm1 = ccdShape1->mPrevTransform;
+
+	const PxVec3 trA = tm0.p - lastTm0.p;
+	const PxVec3 trB = tm1.p - lastTm1.p;
+	const PxVec3 relTr = trA - trB;
+
+	// Do the sweep
+	PxVec3 sweepNormal(0.0f);
+	PxVec3 sweepPoint(0.0f);
+
+	PxReal restDistance = PxMax(mCm->getWorkUnit().restDistance, 0.f);
+
+	context.mDt = dt;
+	context.mCCDFaceIndex = PXC_CONTACT_NO_FACE_INDEX;
+
+	//Cull the sweep hit based on the relative velocity along the normal
+	const PxReal fastMovingThresh0 = ccdShape0->mFastMovingThreshold;
+	const PxReal fastMovingThresh1 = ccdShape1->mFastMovingThreshold;
+
+	const PxReal sumFastMovingThresh = (fastMovingThresh0 + fastMovingThresh1);
+
+
+	PxReal toi = Gu::SweepShapeShape(*ccdShape0, *ccdShape1, tm0, tm1, lastTm0, lastTm1, restDistance,
+		sweepNormal, sweepPoint, mMinToi, context.mCCDFaceIndex, sumFastMovingThresh);
+
+	//If toi is after the end of TOI, return no hit
+	if (toi >= 1.0f)
+	{
+		mToiType				= PxsCCDPair::ePrecise;
+		mPenetration			= 0.f;
+		mPenetrationPostStep	= 0.f;
+		mMinToi					= PX_MAX_REAL; // needs to be reset in case of resweep
+		return toi;
+	}
+
+	PX_ASSERT(PxIsFinite(toi));
+	PX_ASSERT(sweepNormal.isFinite());
+	mFaceIndex = context.mCCDFaceIndex;
+
+	//Work out linear motion (used to cull the sweep hit)
+	const PxReal linearMotion = relTr.dot(-sweepNormal);
+	
+	//If we swapped the shapes, swap them back
+	if(mG1 >= mG0)
+		sweepNormal = -sweepNormal;
+
+
+	mToiType = PxsCCDPair::ePrecise;
+
+	PxReal penetration = 0.f;
+	PxReal penetrationPostStep = 0.f;
+
+	//If linear motion along normal < the CCD threshold, set toi to no-hit.
+	if((linearMotion) < sumFastMovingThresh)
+	{
+		mMinToi					= PX_MAX_REAL; // needs to be reset in case of resweep
+		return PX_MAX_REAL;
+	}
+	else if(toi <= 0.f)
+	{
+		//If the toi <= 0.f, this implies an initial overlap. If the value < 0.f, it implies penetration
+		PxReal stepRatio0 = atom0 ? atom0->mCCD->mTimeLeft : 1.f;
+		PxReal stepRatio1 = atom1 ? atom1->mCCD->mTimeLeft : 1.f;
+
+		PxReal stepRatio = PxMin(stepRatio0, stepRatio1);
+		penetration = -toi;
+
+		toi = 0.f;
+		
+		if(stepRatio == 1.f)
+		{
+			//If stepRatio == 1.f (i.e. neither body has stepped forwards any time at all)
+			//we extract the advance coefficients from the bodies and permit the bodies to step forwards a small amount
+			//to ensure that they won't remain jammed because TOI = 0.0
+			const PxReal advance0 = atom0 ? atom0->mCore->ccdAdvanceCoefficient : 1.f;
+			const PxReal advance1 = atom1 ? atom1->mCore->ccdAdvanceCoefficient : 1.f;
+			const PxReal advance = PxMin(advance0, advance1);
+
+			PxReal fastMoving = PxMin(fastMovingThresh0, atom1 ? fastMovingThresh1 : PX_MAX_REAL);
+			PxReal advanceCoeff = advance * fastMoving;
+			penetrationPostStep = advanceCoeff/linearMotion;
+			
+		}
+		PX_ASSERT(PxIsFinite(toi));
+	}
+
+	//Store TOI, penetration and post-step (how much to step forward in initial overlap conditions)
+	mMinToi			= toi;
+	mPenetration	= penetration;
+	mPenetrationPostStep = penetrationPostStep;
+
+	mMinToiPoint = sweepPoint;
+
+	mMinToiNormal	= sweepNormal;
+
+
+	//Work out the materials for the contact (restitution, friction etc.)
+	context.mContactBuffer.count = 0;
+	context.mContactBuffer.contact(mMinToiPoint, mMinToiNormal, 0.f, g1 == PxGeometryType::eTRIANGLEMESH || g1 == PxGeometryType::eHEIGHTFIELD? mFaceIndex : PXC_CONTACT_NO_FACE_INDEX);
+
+	PxsMaterialInfo materialInfo;
+
+	g_GetSingleMaterialMethodTable[g0](ccdShape0->mShapeCore, 0, context, &materialInfo);
+	g_GetSingleMaterialMethodTable[g1](ccdShape1->mShapeCore, 1, context, &materialInfo);
+
+	const PxsMaterialData& data0 = *context.mMaterialManager->getMaterial(materialInfo.mMaterialIndex0);
+	const PxsMaterialData& data1 = *context.mMaterialManager->getMaterial(materialInfo.mMaterialIndex1);
+
+	const PxReal restitution = PxsMaterialCombiner::combineRestitution(data0, data1);
+	PxsMaterialCombiner combiner(1.0f, 1.0f);
+	PxsMaterialCombiner::PxsCombinedMaterial combinedMat = combiner.combineIsotropicFriction(data0, data1);
+	const PxReal sFriction = combinedMat.staFriction;
+	const PxReal dFriction = combinedMat.dynFriction;
+
+	mMaterialIndex0 = materialInfo.mMaterialIndex0;
+	mMaterialIndex1 = materialInfo.mMaterialIndex1;
+	mDynamicFriction = dFriction;
+	mStaticFriction = sFriction;
+	mRestitution = restitution;
+
+	return toi;
+}
+
+void PxsCCDPair::updateShapes()
+{
+	if(mBa0)
+	{
+		//If the CCD shape's update count doesn't match the body's update count, this shape needs its transforms and bounds re-calculated
+		if(mBa0->mCCD->mUpdateCount != mCCDShape0->mUpdateCount)
+		{
+			const PxTransform tm0 = mCCDShape0->getAbsPose(mBa0);
+			const PxTransform lastTm0 = mCCDShape0->getLastCCDAbsPose(mBa0);
+
+			const PxVec3 trA = tm0.p - lastTm0.p;
+
+			Gu::Vec3p origin, extents;
+			Gu::computeBoundsWithCCDThreshold(origin, extents, mCCDShape0->mShapeCore->geometry.getGeometry(), tm0, NULL);
+
+			mCCDShape0->mCenter = origin - trA;
+			mCCDShape0->mExtents = extents;
+			mCCDShape0->mPrevTransform = lastTm0;
+			mCCDShape0->mCurrentTransform = tm0;
+			mCCDShape0->mUpdateCount = mBa0->mCCD->mUpdateCount;
+		}
+	}
+
+	if(mBa1)
+	{
+		//If the CCD shape's update count doesn't match the body's update count, this shape needs its transforms and bounds re-calculated
+		if(mBa1->mCCD->mUpdateCount != mCCDShape1->mUpdateCount)
+		{
+			const PxTransform tm1 = mCCDShape1->getAbsPose(mBa1);
+			const PxTransform lastTm1 = mCCDShape1->getLastCCDAbsPose(mBa1);
+
+			const PxVec3 trB = tm1.p - lastTm1.p;
+
+			Vec3p origin, extents;
+			computeBoundsWithCCDThreshold(origin, extents, mCCDShape1->mShapeCore->geometry.getGeometry(), tm1, NULL);
+
+			mCCDShape1->mCenter = origin - trB;
+			mCCDShape1->mExtents = extents;
+			mCCDShape1->mPrevTransform = lastTm1;
+			mCCDShape1->mCurrentTransform = tm1;
+			mCCDShape1->mUpdateCount = mBa1->mCCD->mUpdateCount;
+		}
+	}
+}
+
+PxReal PxsCCDPair::sweepEstimateToi()
+{
+	//Update shape transforms if necessary
+	updateShapes();
+
+	//PxsRigidBody* atom1 = mBa1;
+	//PxsRigidBody* atom0 = mBa0;
+	PxsCCDShape* ccdShape0 = mCCDShape0;
+	PxsCCDShape* ccdShape1 = mCCDShape1;
+	PxGeometryType::Enum g0 = mG0, g1 = mG1;
+	PX_UNUSED(g0);
+
+
+	//Flip shapes if necessary
+	if(mG1 < mG0)
+	{
+		g0 = mG1;
+		g1 = mG0;
+		/*atom0 = mBa1;
+		atom1 = mBa0;*/
+		ccdShape0 = mCCDShape1;
+		ccdShape1 = mCCDShape0;
+	}
+
+	//Extract previous/current transforms, translations etc.
+	PxTransform tm0, lastTm0, tm1, lastTm1;
+
+	PxVec3 trA(0.f);
+	PxVec3 trB(0.f);
+	tm0 = ccdShape0->mCurrentTransform;
+	lastTm0 = ccdShape0->mPrevTransform;
+	trA = tm0.p - lastTm0.p;
+
+	tm1 = ccdShape1->mCurrentTransform;
+	lastTm1 = ccdShape1->mPrevTransform;
+	trB = tm1.p - lastTm1.p;
+
+	PxReal restDistance = PxMax(mCm->getWorkUnit().restDistance, 0.f);
+
+	const PxVec3 relTr = trA - trB;
+
+	//Work out the sum of the fast moving thresholds scaled by the step ratio
+	const PxReal fastMovingThresh0 = ccdShape0->mFastMovingThreshold;
+	const PxReal fastMovingThresh1 = ccdShape1->mFastMovingThreshold;
+	const PxReal sumFastMovingThresh = (fastMovingThresh0 + fastMovingThresh1);
+
+	mToiType = eEstimate;
+
+	//If the objects are not moving fast-enough relative to each-other to warrant CCD, set estimated time as PX_MAX_REAL
+	if((relTr.magnitudeSquared()) <= (sumFastMovingThresh * sumFastMovingThresh))
+	{
+		mToiType = eEstimate;
+		mMinToi = PX_MAX_REAL;
+		return PX_MAX_REAL;
+	}
+
+	//Otherwise, the objects *are* moving fast-enough so perform estimation pass
+	if(g1 == PxGeometryType::eTRIANGLEMESH)
+	{
+		//Special-case estimation code for meshes
+		PxF32 toi = Gu::SweepEstimateAnyShapeMesh(*ccdShape0, *ccdShape1, tm0, tm1, lastTm0, lastTm1, restDistance, sumFastMovingThresh);
+									
+		mMinToi	= toi;
+		return toi;
+	}
+	else if (g1 == PxGeometryType::eHEIGHTFIELD)
+	{
+		//Special-case estimation code for heightfields
+		PxF32 toi = Gu::SweepEstimateAnyShapeHeightfield(*ccdShape0, *ccdShape1, tm0, tm1, lastTm0, lastTm1, restDistance, sumFastMovingThresh);
+									
+		mMinToi	= toi;
+		return toi;
+	}
+
+	//Generic estimation code for prim-prim sweeps
+	PxVec3 centreA, extentsA;
+	PxVec3 centreB, extentsB;
+	centreA = ccdShape0->mCenter;
+	extentsA = ccdShape0->mExtents + PxVec3(restDistance);
+
+	centreB = ccdShape1->mCenter;
+	extentsB = ccdShape1->mExtents;
+
+	PxF32 toi = Gu::sweepAABBAABB(centreA, extentsA * 1.1f, centreB, extentsB * 1.1f, trA, trB);
+	mMinToi			= toi;
+	return toi;
+}
+
+bool PxsCCDPair::sweepAdvanceToToi(PxReal dt, bool clipTrajectoryToToi)
+{
+	PxsCCDShape* ccds0 = mCCDShape0;
+	PxsRigidBody* atom0 = mBa0;
+	PxsCCDShape* ccds1 = mCCDShape1;
+	PxsRigidBody* atom1 = mBa1;
+
+	const PxsCCDPair* thisPair = this;
+
+	//Both already had a pass so don't do anything
+	if ((atom0 == NULL || atom0->mCCD->mPassDone) && (atom1 == NULL || atom1->mCCD->mPassDone))
+		return false;
+
+	//Test to validate that they're both infinite mass objects. If so, there can be no response so terminate on a notification-only
+	if((atom0 == NULL || atom0->mCore->inverseMass == 0.f) && (atom1 == NULL || atom1->mCore->inverseMass == 0.f))
+		return false;
+
+	//If the TOI < 1.f. If not, this hit happens after this frame or at the very end of the frame. Either way, next frame can handle it
+	if (thisPair->mMinToi < 1.0f)
+	{
+		if(thisPair->mCm->getWorkUnit().flags & PxcNpWorkUnitFlag::eDISABLE_RESPONSE)
+		{
+			//This is a contact which has response disabled. We just step the bodies and return true but don't respond.
+			if(atom0)
+			{
+				atom0->advancePrevPoseToToi(thisPair->mMinToi);
+				atom0->advanceToToi(thisPair->mMinToi, dt, false);
+			}
+			if(atom1)
+			{
+				atom1->advancePrevPoseToToi(thisPair->mMinToi);
+				atom1->advanceToToi(thisPair->mMinToi, dt, false);
+			}
+			//Don't mark pass as done on either body
+			return true;
+		}
+
+		
+		PxReal minToi = thisPair->mMinToi;
+
+		PxF32 penetration = -mPenetration * 10.f;
+		
+		PxVec3 minToiNormal = thisPair->mMinToiNormal;
+
+		if (!minToiNormal.isNormalized())
+		{
+			// somehow we got zero normal. This can happen for instance if two identical objects spawn exactly on top of one another
+			// abort ccd and clip to current toi
+			if (atom0 && !atom0->mCCD->mPassDone)
+			{
+				atom0->advancePrevPoseToToi(minToi);
+				atom0->advanceToToi(minToi, dt, true);
+				atom0->mCCD->mUpdateCount++;
+			}
+			return true;
+		}
+
+	
+
+		//Get the material indices...
+		const PxReal restitution = mRestitution;
+		const PxReal sFriction = mStaticFriction;
+		const PxReal dFriction = mDynamicFriction;
+
+
+		PxVec3 v0(0.f), v1(0.f);
+
+		PxReal invMass0(0.f), invMass1(0.f);
+#if CCD_ANGULAR_IMPULSE
+		PxMat33 invInertia0(PxVec3(0.f), PxVec3(0.f), PxVec3(0.f)), invInertia1(PxVec3(0.f), PxVec3(0.f), PxVec3(0.f));
+		PxVec3 localPoint0(0.f), localPoint1(0.f);
+#endif
+
+		PxReal dom0 = mCm->getDominance0();
+		PxReal dom1 = mCm->getDominance1();
+
+		//Work out velocity and invMass for body 0
+		if(atom0)
+		{
+			//Put contact point in local space, then find how much point is moving using point velocity...
+			
+#if CCD_ANGULAR_IMPULSE
+			localPoint0 = mMinToiPoint - trA.p;
+			v0 = atom0->mCore->linearVelocity + atom0->mCore->angularVelocity.cross(localPoint0);
+			
+			physx::Cm::transformInertiaTensor(atom0->mCore->inverseInertia, PxMat33(trA.q),invInertia0);
+			invInertia0 *= dom0;
+#else
+			v0 = atom0->mCore->linearVelocity + atom0->mCore->angularVelocity.cross(ccds0->mShapeCore->transform.p);
+#endif
+			invMass0 = atom0->getInvMass() * dom0;
+			
+		}
+
+		//Work out velocity and invMass for body 1
+		if(atom1)
+		{
+			//Put contact point in local space, then find how much point is moving using point velocity...
+#if CCD_ANGULAR_IMPULSE
+			localPoint1 = mMinToiPoint - trB.p;
+			v1 = atom1->mCore->linearVelocity + atom1->mCore->angularVelocity.cross(localPoint1);
+			physx::Cm::transformInertiaTensor(atom1->mCore->inverseInertia, PxMat33(trB.q),invInertia1);
+			invInertia1 *= dom1;
+#else
+			v1 = atom1->mCore->linearVelocity + atom1->mCore->angularVelocity.cross(ccds1->mShapeCore->transform.p);
+#endif
+			invMass1 = atom1->getInvMass() * dom1;
+		}
+
+		PX_ASSERT(v0.isFinite() && v1.isFinite());
+
+		//Work out relative velocity
+		PxVec3 vRel = v1 - v0;
+
+		//Project relative velocity onto contact normal and bias with penetration
+		PxReal relNorVel = vRel.dot(minToiNormal);
+		PxReal relNorVelPlusPen = relNorVel + penetration;
+
+#if CCD_ANGULAR_IMPULSE
+		if(relNorVelPlusPen >= -1e-6f)
+		{
+			//we fall back on linear only parts...
+			localPoint0 = PxVec3(0.f);
+			localPoint1 = PxVec3(0.f);
+			v0 = atom0 ? atom0->getLinearVelocity() : PxVec3(0.f);
+			v1 = atom1 ? atom1->getLinearVelocity() : PxVec3(0.f);
+			vRel = v1 - v0;
+			relNorVel = vRel.dot(minToiNormal);
+			relNorVelPlusPen = relNorVel + penetration;
+		}
+#endif
+
+
+		//If the relative motion is moving towards each-other, respond
+		if(relNorVelPlusPen < -1e-6f)
+		{
+			PxReal sumRecipMass = invMass0 + invMass1;
+
+			const PxReal jLin = relNorVelPlusPen;
+			const PxReal normalResponse = (1.f + restitution) * jLin;
+
+#if CCD_ANGULAR_IMPULSE
+			const PxVec3 angularMom0 = invInertia0 * (localPoint0.cross(mMinToiNormal));
+			const PxVec3 angularMom1 = invInertia1 * (localPoint1.cross(mMinToiNormal));
+
+			const PxReal jAng = minToiNormal.dot(angularMom0.cross(localPoint0) + angularMom1.cross(localPoint1));
+			const PxReal impulseDivisor = sumRecipMass + jAng;
+
+#else
+			const PxReal impulseDivisor = sumRecipMass;
+#endif
+			const PxReal jImp = normalResponse/impulseDivisor;
+
+			PxVec3 j(0.f);
+
+			//If the user requested CCD friction, calculate friction forces.
+			//Note, CCD is *linear* so friction is also linear. The net result is that CCD friction can stop bodies' lateral motion so its better to have it disabled
+			//unless there's a real need for it.
+			if(mHasFriction)
+			{
+			
+				PxVec3 vPerp = vRel - relNorVel * minToiNormal;
+				PxVec3 tDir = vPerp;
+				PxReal length = tDir.normalize();
+				PxReal vPerpImp = length/impulseDivisor;
+				PxF32 fricResponse = 0.f;
+				PxF32 staticResponse = (jImp*sFriction);
+				PxF32 dynamicResponse = (jImp*dFriction);
+				
+				
+				if (PxAbs(staticResponse) >= vPerpImp)
+					fricResponse = vPerpImp;
+				else
+				{
+					fricResponse = -dynamicResponse /* times m0 */;
+				}
+
+				
+				//const PxVec3 fricJ = -vPerp.getNormalized() * (fricResponse/impulseDivisor);
+				const PxVec3 fricJ =  tDir * (fricResponse);
+				j = jImp * mMinToiNormal + fricJ;
+			}
+			else
+			{
+				j = jImp * mMinToiNormal;
+			}
+
+			verifyCCDPair(*this);
+			//If we have a negative impulse value, then we need to apply it. If not, the bodies are separating (no impulse to apply).
+			if(jImp < 0.f)
+			{
+				mAppliedForce = -jImp;
+
+				//Adjust velocities
+				if((atom0 != NULL && atom0->mCCD->mPassDone) || 
+					(atom1 != NULL && atom1->mCCD->mPassDone))
+				{
+					mPenetrationPostStep = 0.f;
+				}
+				else
+				{
+					if (atom0)
+					{
+						//atom0->mAcceleration.linear = atom0->getLinearVelocity();  // to provide pre-"solver" velocity in contact reports
+
+						atom0->setLinearVelocity(atom0->getLinearVelocity() + j * invMass0);
+						atom0->constrainLinearVelocity();
+	#if CCD_ANGULAR_IMPULSE
+						atom0->mAcceleration.angular = atom0->getAngularVelocity();  // to provide pre-"solver" velocity in contact reports
+						atom0->setAngularVelocity(atom0->getAngularVelocity() + invInertia0 * localPoint0.cross(j));
+						atom0->constrainAngularVelocity();
+	#endif
+					}
+					if (atom1)
+					{
+						//atom1->mAcceleration.linear = atom1->getLinearVelocity();  // to provide pre-"solver" velocity in contact reports
+						atom1->setLinearVelocity(atom1->getLinearVelocity() - j * invMass1);
+						atom1->constrainLinearVelocity();
+	#if CCD_ANGULAR_IMPULSE
+						atom1->mAcceleration.angular = atom1->getAngularVelocity();  // to provide pre-"solver" velocity in contact reports
+						atom1->setAngularVelocity(atom1->getAngularVelocity() - invInertia1 * localPoint1.cross(j));
+						atom1->constrainAngularVelocity();
+	#endif
+					}
+				}
+			}
+		}
+
+		//Update poses
+		if (atom0 && !atom0->mCCD->mPassDone)
+		{
+			atom0->advancePrevPoseToToi(minToi);
+			atom0->advanceToToi(minToi, dt, clipTrajectoryToToi && mPenetrationPostStep == 0.f);	
+			atom0->mCCD->mUpdateCount++;
+		}
+		if (atom1 && !atom1->mCCD->mPassDone)
+		{
+			atom1->advancePrevPoseToToi(minToi);
+			atom1->advanceToToi(minToi, dt, clipTrajectoryToToi && mPenetrationPostStep == 0.f);
+			atom1->mCCD->mUpdateCount++;
+		}
+
+		//If we had a penetration post-step (i.e. an initial overlap), step forwards slightly after collision response
+		if(mPenetrationPostStep > 0.f)
+		{
+			if (atom0 && !atom0->mCCD->mPassDone)
+			{
+				atom0->advancePrevPoseToToi(mPenetrationPostStep);
+				if(clipTrajectoryToToi)
+					atom0->advanceToToi(mPenetrationPostStep, dt, clipTrajectoryToToi);
+			}
+			if (atom1 && !atom1->mCCD->mPassDone)
+			{
+				atom1->advancePrevPoseToToi(mPenetrationPostStep);
+				if(clipTrajectoryToToi)
+					atom1->advanceToToi(mPenetrationPostStep, dt, clipTrajectoryToToi);
+			}
+		}
+		//Mark passes as done
+		if (atom0)
+		{
+			atom0->mCCD->mPassDone = true;
+			atom0->mCCD->mHasAnyPassDone = true;
+		}
+		if (atom1)
+		{
+			atom1->mCCD->mPassDone = true;
+			atom1->mCCD->mHasAnyPassDone = true;
+		}
+
+		return true;
+		
+		//return false;
+	} 
+	else
+	{
+		printCCDDebug("advToi: clean sweep", atom0, mG0);
+	}
+
+	return false;
+}
+
+struct IslandCompare
+{
+	bool operator()(PxsCCDPair& a, PxsCCDPair& b) const { return a.mIslandId < b.mIslandId; }
+};
+
+struct IslandPtrCompare
+{
+	bool operator()(PxsCCDPair*& a, PxsCCDPair*& b) const { return a->mIslandId < b->mIslandId; }
+};
+
+struct ToiCompare
+{
+	bool operator()(PxsCCDPair& a, PxsCCDPair& b) const 
+	{ 
+		return (a.mMinToi < b.mMinToi) || 
+		((a.mMinToi == b.mMinToi) && (a.mBa1 != NULL && b.mBa1 == NULL)); 
+	}
+};
+
+struct ToiPtrCompare
+{
+	bool operator()(PxsCCDPair*& a, PxsCCDPair*& b) const 
+	{ 
+		return (a->mMinToi < b->mMinToi) || 
+		((a->mMinToi == b->mMinToi) && (a->mBa1 != NULL && b->mBa1 == NULL)); 
+	}
+};
+
+
+// --------------------------------------------------------------
+/**
+\brief Class to perform a set of sweep estimate tasks
+*/
+class PxsCCDSweepTask : public Cm::Task
+{
+	PxsCCDPair** 					mPairs;
+	PxU32							mNumPairs;
+public:
+	PxsCCDSweepTask(PxsCCDPair** pairs, PxU32 nPairs)
+		:	mPairs(pairs), mNumPairs(nPairs)
+	{
+	}
+
+	virtual void runInternal()
+	{
+		for (PxU32 j = 0; j < mNumPairs; j++)
+		{
+			PxsCCDPair& pair = *mPairs[j];
+			pair.sweepEstimateToi();
+			pair.mEstimatePass = 0;
+		}
+	}
+
+	virtual const char *getName() const
+	{
+		return "PxsContext.CCDSweep";
+	}
+
+private:
+	PxsCCDSweepTask& operator=(const PxsCCDSweepTask&);
+};
+
+#define ENABLE_RESWEEP 1
+
+// --------------------------------------------------------------
+/**
+\brief Class to advance a set of islands
+*/
+class PxsCCDAdvanceTask : public Cm::Task
+{
+	PxsCCDPair** 					mCCDPairs;
+	PxU32							mNumPairs;
+	PxsContext* 					mContext;
+	PxsCCDContext* 					mCCDContext;
+	PxReal							mDt;
+	PxU32							mCCDPass;
+	const PxsCCDBodyArray&			mCCDBodies;
+
+	PxU32							mFirstThreadIsland;
+	PxU32							mIslandsPerThread;
+	PxU32							mTotalIslandCount;
+	PxU32							mFirstIslandPair; // pairs are sorted by island
+	PxsCCDBody**					mIslandBodies;
+	PxU16*							mNumIslandBodies;
+	PxI32*							mSweepTotalHits;
+	bool							mClipTrajectory;
+	bool							mDisableResweep;
+	
+	PxsCCDAdvanceTask& operator=(const PxsCCDAdvanceTask&);
+public:
+	PxsCCDAdvanceTask(PxsCCDPair** pairs, PxU32 nPairs, const PxsCCDBodyArray& ccdBodies,
+				PxsContext* context, PxsCCDContext* ccdContext, PxReal dt, PxU32 ccdPass,
+				PxU32 firstIslandPair, PxU32 firstThreadIsland, PxU32 islandsPerThread, PxU32 totalIslands, 
+				PxsCCDBody** islandBodies, PxU16* numIslandBodies, bool clipTrajectory, bool disableResweep,
+				PxI32* sweepTotalHits)
+		:	mCCDPairs(pairs), mNumPairs(nPairs), mContext(context), mCCDContext(ccdContext), mDt(dt),
+			mCCDPass(ccdPass), mCCDBodies(ccdBodies), mFirstThreadIsland(firstThreadIsland), 
+			mIslandsPerThread(islandsPerThread), mTotalIslandCount(totalIslands), mFirstIslandPair(firstIslandPair),
+			mIslandBodies(islandBodies), mNumIslandBodies(numIslandBodies),	mSweepTotalHits(sweepTotalHits),
+			mClipTrajectory(clipTrajectory), mDisableResweep(disableResweep)
+			
+	{
+		PX_ASSERT(mFirstIslandPair < mNumPairs);
+	}
+
+	virtual void runInternal()
+	{
+
+		PxI32 sweepTotalHits = 0;
+
+		PxcNpThreadContext* threadContext = mContext->getNpThreadContext();
+
+		// --------------------------------------------------------------------------------------
+		// loop over island labels assigned to this thread
+		PxU32 islandStart = mFirstIslandPair;
+		PxU32 lastIsland = PxMin(mFirstThreadIsland + mIslandsPerThread, mTotalIslandCount);
+		for (PxU32 iIsland = mFirstThreadIsland; iIsland < lastIsland; iIsland++)
+		{
+			if (islandStart >= mNumPairs)
+				// this is possible when for instance there are two islands with 0 pairs in the second
+				// since islands are initially segmented using bodies, not pairs, it can happen
+				break;
+
+			// --------------------------------------------------------------------------------------
+			// sort all pairs within current island by toi
+			PxU32 islandEnd = islandStart+1;
+			while (islandEnd < mNumPairs && mCCDPairs[islandEnd]->mIslandId == iIsland) // find first index past the current island id
+				islandEnd++;
+
+			if (islandEnd > islandStart+1)
+				shdfnd::sort(mCCDPairs+islandStart, islandEnd-islandStart, ToiPtrCompare());
+
+			PX_ASSERT(islandEnd <= mNumPairs);
+
+			// --------------------------------------------------------------------------------------
+			// advance all affected pairs within each island to min toi
+			// for each pair (A,B) in toi order, find any later-toi pairs that collide against A or B
+			// and resweep against changed trajectories of either A or B (excluding statics and kinematics)
+			PxReal islandMinToi = PX_MAX_REAL;
+			PxU32 estimatePass = 1;
+
+			PxReal dt = mDt;
+
+			for (PxU32 iFront = islandStart; iFront < islandEnd; iFront++)
+			{
+				PxsCCDPair& pair = *mCCDPairs[iFront];
+
+				verifyCCDPair(pair);
+
+				//If we have reached a pair with a TOI after 1.0, we can terminate this island
+				if(pair.mMinToi > 1.f)
+					break;
+
+				bool needSweep0 = (pair.mBa0 && pair.mBa0->mCCD->mPassDone == false);
+				bool needSweep1 = (pair.mBa1 && pair.mBa1->mCCD->mPassDone == false);
+
+				//If both bodies have been updated (or one has been updated and the other is static), we can skip to the next pair
+				if(!(needSweep0 || needSweep1))
+					continue;
+
+				{
+					//If the pair was an estimate, we must perform an accurate sweep now
+					if(pair.mToiType == PxsCCDPair::eEstimate)
+					{
+						pair.sweepFindToi(*threadContext, dt, mCCDPass);
+
+						//Test to see if the pair is still the earliest pair.
+						if((iFront + 1) < islandEnd && mCCDPairs[iFront+1]->mMinToi < pair.mMinToi)
+						{
+							//If there is an earlier pair, we push this pair into its correct place in the list and return to the start
+							//of this update loop
+							PxsCCDPair* tmp = &pair;
+							PxU32 index = iFront;
+							while((index + 1) < islandEnd && mCCDPairs[index+1]->mMinToi < pair.mMinToi)
+							{
+								mCCDPairs[index] = mCCDPairs[index+1];
+								++index;
+							}
+
+							mCCDPairs[index] = tmp;
+
+							--iFront;
+							continue;
+						}
+					}
+
+					//We now have the earliest contact pair for this island and one/both of the bodies have not been updated. We now perform
+					//contact modification to find out if the user still wants to respond to the collision
+					if(pair.mMinToi <= islandMinToi && 
+						pair.mIsModifiable &&
+						mCCDContext->getCCDContactModifyCallback())
+					{
+						//create a modifiable contact and then 
+						PxModifiableContact point;
+						point.contact = pair.mMinToiPoint;
+						point.normal = pair.mMinToiNormal;
+
+						//KS - todo - reintroduce face indices!!!!
+						//point.internalFaceIndex0 = PXC_CONTACT_NO_FACE_INDEX;
+						//point.internalFaceIndex1 = pair.mFaceIndex;
+						point.materialIndex0 = pair.mMaterialIndex0;
+						point.materialIndex1 = pair.mMaterialIndex1;
+						point.dynamicFriction = pair.mDynamicFriction;
+						point.staticFriction = pair.mStaticFriction;
+						point.restitution = pair.mRestitution;
+						point.separation = 0.f;
+						point.maxImpulse = PX_MAX_REAL;
+						point.materialFlags= 0;
+						point.targetVelocity = PxVec3(0.f);
+
+						mCCDContext->runCCDModifiableContact(&point, 1, pair.mCCDShape0->mShapeCore, pair.mCCDShape1->mShapeCore,
+							pair.mCCDShape0->mRigidCore, pair.mCCDShape1->mRigidCore, pair.mBa0, pair.mBa1);
+
+						//If the maxImpulse is 0, we return to the beginning of the loop, knowing that the application did not want an interaction
+						//between the pair.
+						if(point.maxImpulse == 0.f)
+						{
+							pair.mMinToi = PX_MAX_REAL;
+							continue;
+						}
+						pair.mDynamicFriction = point.dynamicFriction;
+						pair.mStaticFriction = point.staticFriction;
+						pair.mRestitution = point.restitution;
+						pair.mMinToiPoint = point.contact;
+						pair.mMinToiNormal = point.normal;
+
+					}
+
+				}
+
+				// pair.mIsEarliestToiHit is used for contact notification.
+				// only mark as such if this is the first impact for both atoms of this pair (the impacts are sorted)
+				// and there was an actual impact for this pair
+				bool atom0FirstSweep = (pair.mBa0 && pair.mBa0->mCCD->mPassDone == false) || pair.mBa0 == NULL;
+				bool atom1FirstSweep = (pair.mBa1 && pair.mBa1->mCCD->mPassDone == false) || pair.mBa1 == NULL;
+				if (pair.mMinToi <= 1.0f && atom0FirstSweep && atom1FirstSweep)
+					pair.mIsEarliestToiHit = true;
+
+				// sweepAdvanceToToi sets mCCD->mPassDone flags on both atoms, doesn't advance atoms with flag already set
+				// can advance one atom if the other already has the flag set
+				bool advanced = pair.sweepAdvanceToToi( dt, mClipTrajectory);
+				if(pair.mMinToi < 0.f)
+					pair.mMinToi = 0.f;
+				verifyCCDPair(pair);
+
+				if (advanced && pair.mMinToi <= 1.0f)
+				{
+					sweepTotalHits++;
+					PxU32 islandStartIndex = iIsland == 0 ? 0 : PxU32(mNumIslandBodies[iIsland - 1]);
+					PxU32 islandEndIndex = mNumIslandBodies[iIsland];
+
+					if(pair.mMinToi > 0.f)
+					{
+						for(PxU32 a = islandStartIndex; a < islandEndIndex; ++a)
+						{	
+							if(!mIslandBodies[a]->mPassDone)
+							{
+								//If the body has not updated, we advance it to the current time-step that the island has reached.
+								PxsRigidBody* atom = mIslandBodies[a]->mBody;
+								atom->advancePrevPoseToToi(pair.mMinToi);
+								atom->mCCD->mTimeLeft = PxMax(atom->mCCD->mTimeLeft * (1.0f - pair.mMinToi), CCD_MIN_TIME_LEFT);
+								atom->mCCD->mUpdateCount++;
+							}
+						}
+
+						//Adjust remaining dt for the island
+						dt -= dt * pair.mMinToi;
+
+						const PxReal recipOneMinusToi = 1.f/(1.f - pair.mMinToi);
+						for(PxU32 k = iFront+1; k < islandEnd; ++k)
+						{
+							PxsCCDPair& pair1 = *mCCDPairs[k];
+							pair1.mMinToi = (pair1.mMinToi - pair.mMinToi)*recipOneMinusToi;
+						}
+
+					}
+
+					//If we disabled response, we don't need to resweep at all
+					if(!mDisableResweep && !(pair.mCm->getWorkUnit().flags & PxcNpWorkUnitFlag::eDISABLE_RESPONSE))
+					{
+						void* a0 = pair.mBa0 == NULL ? NULL : reinterpret_cast<void*>(pair.mBa0);
+						void* a1 = pair.mBa1 == NULL ? NULL : reinterpret_cast<void*>(pair.mBa1);
+
+						for(PxU32 k = iFront+1; k < islandEnd; ++k)
+						{
+							PxsCCDPair& pair1 = *mCCDPairs[k];
+
+							void* b0 = pair1.mBa0 == NULL ? reinterpret_cast<void*>(pair1.mCCDShape0) : reinterpret_cast<void*>(pair1.mBa0);
+							void* b1 = pair1.mBa1 == NULL ? reinterpret_cast<void*>(pair1.mCCDShape1) : reinterpret_cast<void*>(pair1.mBa1);
+
+							bool containsStatic = pair1.mBa0 == NULL || pair1.mBa1 == NULL;
+
+							PX_ASSERT(b0 != NULL && b1 != NULL);
+
+							if ((!containsStatic) &&
+								((b0 == a0 && b1 != a1) || (b1 == a0 && b0 != a1) ||
+								(b0 == a1 && b1 != a0) || (b1 == a1 && b0 != a0))
+							)
+							{
+								if(estimatePass != pair1.mEstimatePass)
+								{
+									pair1.mEstimatePass = estimatePass;
+									// resweep pair1 since either b0 or b1 trajectory has changed
+									PxReal oldToi = pair1.mMinToi;
+									verifyCCDPair(pair1);
+									PxReal toi1 = pair1.sweepEstimateToi();
+									PX_ASSERT(pair1.mBa0); // this is because mMinToiNormal is the impact point here
+									if (toi1 < oldToi)
+									{
+										// if toi decreased, resort the array backwards
+										PxU32 kk = k;
+										PX_ASSERT(kk > 0);
+										
+										while (kk-1 > iFront && mCCDPairs[kk-1]->mMinToi > toi1)
+										{
+											PxsCCDPair* temp = mCCDPairs[kk-1];
+											mCCDPairs[kk-1] = mCCDPairs[kk];
+											mCCDPairs[kk] = temp;
+											kk--;
+										}
+
+									}
+									else if (toi1 > oldToi)
+									{
+										// if toi increased, resort the array forwards
+										PxU32 kk = k;
+										PX_ASSERT(kk > 0);
+										PxU32 stepped = 0;
+										while (kk+1 < islandEnd && mCCDPairs[kk+1]->mMinToi < toi1)
+										{
+											stepped = 1;
+											PxsCCDPair* temp = mCCDPairs[kk+1];
+											mCCDPairs[kk+1] = mCCDPairs[kk];
+											mCCDPairs[kk] = temp;
+											kk++;
+										}
+										k -= stepped;
+									}
+								}
+							}
+						}
+					}
+					estimatePass++;
+				} // if pair.minToi <= 1.0f
+			} // for iFront
+
+			islandStart = islandEnd;
+		} // for (PxU32 iIsland = mFirstThreadIsland; iIsland < lastIsland; iIsland++)
+
+		Ps::atomicAdd(mSweepTotalHits, sweepTotalHits);
+		mContext->putNpThreadContext(threadContext);
+	}
+
+	virtual const char *getName() const
+	{
+		return "PxsContext.CCDAdvance";
+	}
+};
+
+// --------------------------------------------------------------
+// CCD main function
+// Overall structure:
+/*
+for nPasses (passes are now handled in void Sc::Scene::updateCCDMultiPass)
+
+  update CCD broadphase, generate a list of CMs
+
+  foreach CM
+    create CCDPairs, CCDBodies from CM
+	add shapes, overlappingShapes to CCDBodies
+
+  foreach CCDBody
+    assign island labels per body
+    uses overlappingShapes
+
+  foreach CCDPair 
+    assign island label to pair  
+
+  sort all pairs by islandId
+
+  foreach CCDPair
+    sweep/find toi
+    compute normal:
+        
+  foreach island
+    sort within island by toi
+    foreach pair within island
+      advanceToToi
+      from curPairInIsland to lastPairInIsland
+        resweep if needed
+
+*/
+// --------------------------------------------------------------
+void PxsCCDContext::updateCCDBegin()
+{
+	openCCDLog();
+
+	miCCDPass = 0;
+	mSweepTotalHits = 0;
+}
+
+// --------------------------------------------------------------
+void PxsCCDContext::updateCCDEnd()
+{
+	if (miCCDPass == mCCDMaxPasses - 1 || mSweepTotalHits == 0)
+	{
+		// --------------------------------------------------------------------------------------
+		// At last CCD pass we need to reset mBody pointers back to NULL
+		// so that the next frame we know which ones need to be newly paired with PxsCCDBody objects
+		// also free the CCDBody memory blocks
+
+		mMutex.lock();
+		for (PxU32 j = 0, n = mCCDBodies.size(); j < n; j++)
+		{
+			if (mCCDBodies[j].mBody->mCCD && mCCDBodies[j].mBody->mCCD->mHasAnyPassDone)
+			{
+				//Record this body in the list of bodies that were updated
+				mUpdatedCCDBodies.pushBack(mCCDBodies[j].mBody);
+			}
+			mCCDBodies[j].mBody->mCCD = NULL;
+			mCCDBodies[j].mBody->getCore().isFastMoving = false; //Clear the "isFastMoving" bool
+		}
+		mMutex.unlock();
+
+		mCCDBodies.clear_NoDelete();
+
+	}
+
+	mCCDShapes.clear_NoDelete();
+
+	mMap.clear();
+
+	miCCDPass++;
+}
+
+// --------------------------------------------------------------
+void PxsCCDContext::verifyCCDBegin()
+{
+	#if 0
+	// validate that all bodies have a NULL mCCD pointer
+	if (miCCDPass == 0)
+	{
+		Cm::BitMap::Iterator it(mActiveContactManager);
+		for (PxU32 index = it.getNext(); index != Cm::BitMap::Iterator::DONE; index = it.getNext())
+		{
+			PxsContactManager* cm = mContactManagerPool.findByIndexFast(index);
+			PxsRigidBody* b0 = cm->mBodyShape0->getBodyAtom(), *b1 = cm->mBodyShape1->getBodyAtom();
+			PX_ASSERT(b0 == NULL || b0->mCCD == NULL);
+			PX_ASSERT(b1 == NULL || b1->mCCD == NULL);
+		}
+	}
+	#endif
+}
+
+void PxsCCDContext::resetContactManagers()
+{
+	Cm::BitMap::Iterator it(mContext->mContactManagersWithCCDTouch);
+
+	for (PxU32 index = it.getNext(); index != Cm::BitMap::Iterator::DONE; index = it.getNext())
+	{
+		PxsContactManager* cm = mContext->mContactManagerPool.findByIndexFast(index);
+		cm->clearCCDContactInfo();
+	}
+
+	mContext->mContactManagersWithCCDTouch.clear();
+}
+
+// --------------------------------------------------------------
+void PxsCCDContext::updateCCD(PxReal dt, PxBaseTask* continuation, bool disableResweep, PxI32 numFastMovingShapes)
+{
+	//Flag to run a slightly less-accurate version of CCD that will ensure that objects don't tunnel through the static world but is not as reliable for dynamic-dynamic collisions
+	mDisableCCDResweep = disableResweep;  
+	mThresholdStream.clear();  // clear force threshold report stream
+
+	mContext->clearManagerTouchEvents();
+
+	if (miCCDPass == 0)
+	{
+		resetContactManagers();
+	}
+
+
+	// If we're not in the first pass and the previous pass had no sweeps or the BP didn't generate any fast-moving shapes, we can skip CCD entirely
+	if ((miCCDPass > 0 && mSweepTotalHits == 0) || (numFastMovingShapes == 0)) 
+	{
+		mSweepTotalHits = 0;
+		updateCCDEnd();
+		return;
+	}
+	mSweepTotalHits = 0;
+
+	PX_ASSERT(continuation);
+	PX_ASSERT(continuation->getReference() > 0);
+
+	//printf("CCD 1\n");
+
+	mCCDThreadContext = mContext->getNpThreadContext();
+	mCCDThreadContext->mDt = dt; // doesn't get set anywhere else since it's only used for CCD now
+
+	verifyCCDBegin();
+
+	// --------------------------------------------------------------------------------------
+	// From a list of active CMs, build a temporary array of PxsCCDPair objects (allocated in blocks)
+	// this is done to gather scattered data from memory and also to reduce PxsRidigBody permanent memory footprint
+	// we have to do it every pass since new CMs can become fast moving after each pass (and sometimes cease to be)
+	mCCDPairs.clear_NoDelete();
+	mCCDPtrPairs.forceSize_Unsafe(0);
+
+	mUpdatedCCDBodies.forceSize_Unsafe(0);
+
+	mCCDOverlaps.clear_NoDelete();
+
+
+	bool needsSweep = false;
+
+	{
+		PX_PROFILE_ZONE("Sim.ccdPair", mContext->mContextID);
+	
+		Cm::BitMap::Iterator it(mContext->mActiveContactManagersWithCCD);
+		for (PxU32 index = it.getNext(); index != Cm::BitMap::Iterator::DONE; index = it.getNext())
+		{
+			PxsContactManager* cm = mContext->mContactManagerPool.findByIndexFast(index);
+
+			// skip disabled pairs
+			if(!cm->getCCD())
+				continue;
+
+			bool isJoint0 = (cm->mNpUnit.flags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) == PxcNpWorkUnitFlag::eARTICULATION_BODY0;
+			bool isJoint1 = (cm->mNpUnit.flags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) == PxcNpWorkUnitFlag::eARTICULATION_BODY1;
+			// skip articulation vs articulation ccd
+			//Actually. This is fundamentally wrong also :(. We only want to skip links in the same articulation - not all articulations!!!
+			if (isJoint0 && isJoint1)
+				continue;
+
+			bool isFastMoving0 = static_cast<const PxsBodyCore*>(cm->mNpUnit.rigidCore0)->isFastMoving != 0;
+
+			bool isFastMoving1 = (cm->mNpUnit.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY1 | PxcNpWorkUnitFlag::eDYNAMIC_BODY1)) ? static_cast<const PxsBodyCore*>(cm->mNpUnit.rigidCore1)->isFastMoving != 0: false;
+
+			if (!(isFastMoving0 || isFastMoving1))
+				continue;
+
+			PxcNpWorkUnit& unit = cm->getWorkUnit();
+			const PxsRigidCore* rc0 = unit.rigidCore0;
+			const PxsRigidCore* rc1 = unit.rigidCore1;
+			
+			{
+				const PxsShapeCore* sc0 = unit.shapeCore0;
+				const PxsShapeCore* sc1 = unit.shapeCore1;
+
+				PxsRigidBody* ba0 = cm->mRigidBody0;
+				PxsRigidBody* ba1 = cm->mRigidBody1;
+
+				//Look up the body/shape pair in our CCDShape map
+				const Ps::Pair<const PxsRigidShapePair, PxsCCDShape*>* ccdShapePair0 = mMap.find(PxsRigidShapePair(rc0, sc0));
+				const Ps::Pair<const PxsRigidShapePair, PxsCCDShape*>* ccdShapePair1 = mMap.find(PxsRigidShapePair(rc1, sc1));
+
+				//If the CCD shapes exist, extract them from the map
+				PxsCCDShape* ccdShape0 = ccdShapePair0 ? ccdShapePair0->second : NULL;
+				PxsCCDShape* ccdShape1 = ccdShapePair1 ? ccdShapePair1->second : NULL;
+
+				PxReal threshold0 = 0.f;
+				PxReal threshold1 = 0.f;
+
+				PxVec3 trA(0.f);
+				PxVec3 trB(0.f);
+
+				if(ccdShape0 == NULL)
+				{
+					//If we hadn't already created ccdShape, create one
+					ccdShape0 = &mCCDShapes.pushBack();
+					mMap.insert(PxsRigidShapePair(rc0, sc0), ccdShape0);
+
+					ccdShape0->mRigidCore = rc0;
+					ccdShape0->mShapeCore = sc0;
+					ccdShape0->mGeometry = &sc0->geometry;
+
+					const PxTransform tm0 = ccdShape0->getAbsPose(ba0);
+					const PxTransform oldTm0 = ba0 ? ccdShape0->getLastCCDAbsPose(ba0) : tm0;
+
+					trA = tm0.p - oldTm0.p;
+
+					Vec3p origin, extents;
+
+					//Compute the shape's bounds and CCD threshold
+					threshold0 = computeBoundsWithCCDThreshold(origin, extents, sc0->geometry.getGeometry(), tm0, NULL);
+
+					//Set up the CCD shape
+					ccdShape0->mCenter = origin - trA;
+					ccdShape0->mExtents = extents;
+					ccdShape0->mFastMovingThreshold = threshold0;
+					ccdShape0->mPrevTransform = oldTm0;
+					ccdShape0->mCurrentTransform = tm0;
+					ccdShape0->mUpdateCount = 0;
+				}
+				else
+				{
+					//We had already created the shape, so extract the threshold and translation components
+					threshold0 = ccdShape0->mFastMovingThreshold;
+					trA = ccdShape0->mCurrentTransform.p - ccdShape0->mPrevTransform.p;
+				}
+
+				if(ccdShape1 == NULL)
+				{
+					//If the CCD shape was not already constructed, create it
+					ccdShape1 = &mCCDShapes.pushBack();
+					ccdShape1->mRigidCore = rc1;
+					ccdShape1->mShapeCore = sc1;
+					ccdShape1->mGeometry = &sc1->geometry;
+
+					mMap.insert(PxsRigidShapePair(rc1, sc1), ccdShape1);
+
+					const PxTransform tm1 = ccdShape1->getAbsPose(ba1);
+					const PxTransform oldTm1 = ba1 ? ccdShape1->getLastCCDAbsPose(ba1) : tm1;
+
+					trB = tm1.p - oldTm1.p;
+
+					Vec3p origin, extents;
+					//Compute the shape's bounds and CCD threshold
+					threshold1 = computeBoundsWithCCDThreshold(origin, extents, sc1->geometry.getGeometry(), tm1, NULL);
+
+					//Set up the CCD shape
+					ccdShape1->mCenter = origin - trB;
+					ccdShape1->mExtents = extents;
+					ccdShape1->mFastMovingThreshold = threshold1;
+					ccdShape1->mPrevTransform = oldTm1;
+					ccdShape1->mCurrentTransform = tm1;
+					ccdShape1->mUpdateCount = 0;
+				}
+				else
+				{
+					//CCD shape already constructed so just extract thresholds and trB components
+					threshold1 = ccdShape1->mFastMovingThreshold;
+					trB = ccdShape1->mCurrentTransform.p - ccdShape1->mPrevTransform.p;
+				}
+
+				{
+					//Initialize the CCD bodies
+					PxsRigidBody* atoms[2] = {ba0, ba1};
+					for (int k = 0; k < 2; k++)
+					{
+						PxsRigidBody* b = atoms[k];
+						//If there isn't a body (i.e. it's a static), no need to create a CCD body
+						if (!b)
+							continue;
+						if (b->mCCD == NULL)
+						{
+							// this rigid body has no CCD body created for it yet. Create and initialize one.
+							PxsCCDBody& newB = mCCDBodies.pushBack();
+							b->mCCD = &newB;
+							b->mCCD->mIndex = Ps::to16(mCCDBodies.size()-1);
+							b->mCCD->mBody = b;
+							b->mCCD->mTimeLeft = 1.0f;
+							b->mCCD->mOverlappingObjects = NULL;
+							b->mCCD->mUpdateCount = 0;
+							b->mCCD->mHasAnyPassDone = false;
+							
+						}
+						b->mCCD->mPassDone = 0;
+					}
+					if(ba0 && ba1)
+					{
+						//If both bodies exist (i.e. this is dynamic-dynamic collision), we create an
+						//overlap between the 2 bodies used for island detection.
+						if(!(ba0->isKinematic() || ba1->isKinematic()))
+						{
+							if(!ba0->mCCD->overlaps(ba1->mCCD))
+							{
+								PxsCCDOverlap* overlapA = &mCCDOverlaps.pushBack();
+								PxsCCDOverlap* overlapB = &mCCDOverlaps.pushBack();
+
+								overlapA->mBody = ba1->mCCD;
+								overlapB->mBody = ba0->mCCD;
+
+								ba0->mCCD->addOverlap(overlapA);
+								ba1->mCCD->addOverlap(overlapB);
+							}
+						}
+					}
+				}
+				
+				//We now create the CCD pair. These are used in the CCD sweep and update phases
+				{
+					PxsCCDPair& p = mCCDPairs.pushBack();
+					p.mBa0 = ba0;
+					p.mBa1 = ba1;
+					p.mCCDShape0 = ccdShape0;
+					p.mCCDShape1 = ccdShape1;
+					p.mHasFriction = rc0->hasCCDFriction() || rc1->hasCCDFriction();
+					p.mMinToi = PX_MAX_REAL;
+					p.mG0 = cm->mNpUnit.shapeCore0->geometry.getType();
+					p.mG1 = cm->mNpUnit.shapeCore1->geometry.getType();
+					p.mCm = cm;
+					p.mIslandId = 0xFFFFffff;
+					p.mIsEarliestToiHit = false;
+					p.mFaceIndex = PXC_CONTACT_NO_FACE_INDEX;
+					p.mIsModifiable = cm->isChangeable() != 0;
+					p.mAppliedForce = 0.f;
+
+#if PX_ENABLE_SIM_STATS
+					mContext->mSimStats.mNbCCDPairs[PxMin(p.mG0, p.mG1)][PxMax(p.mG0, p.mG1)] ++;
+#endif
+
+					//Calculate the sum of the thresholds and work out if we need to perform a sweep.
+					
+					const PxReal thresh = threshold0 + threshold1;
+					//If no shape pairs in the entire scene are fast-moving, we can bypass the entire of the CCD.
+					needsSweep = needsSweep || (trA - trB).magnitudeSquared() >= (thresh * thresh);
+				}
+				
+			}
+		}
+		//There are no fast-moving pairs in this scene, so we can terminate right now without proceeding any further
+		if(!needsSweep)
+		{
+			updateCCDEnd();
+			mContext->putNpThreadContext(mCCDThreadContext);
+			return;
+		}
+	}
+
+	//Create the pair pointer buffer. This is a flattened array of pointers to pairs. It is used to sort the pairs
+	//into islands and is also used to prioritize the pairs into their TOIs
+	{
+		const PxU32 size = mCCDPairs.size();
+		mCCDPtrPairs.reserve(size);
+		for(PxU32 a = 0; a < size; ++a)
+		{
+			mCCDPtrPairs.pushBack(&mCCDPairs[a]);
+		}
+
+		mThresholdStream.reserve(Ps::nextPowerOfTwo(size));
+
+		for (PxU32 a = 0; a < mCCDBodies.size(); ++a)
+		{
+			mCCDBodies[a].mPreSolverVelocity.linear = mCCDBodies[a].mBody->getLinearVelocity();
+			mCCDBodies[a].mPreSolverVelocity.angular = mCCDBodies[a].mBody->getAngularVelocity();
+		}
+	}
+
+
+	PxU32 ccdBodyCount = mCCDBodies.size();
+
+	// --------------------------------------------------------------------------------------
+	// assign island labels
+	const PxU16 noLabelYet = 0xFFFF;
+	
+	//Temporary array allocations. Ideally, we should use the scratch pad for there
+	Array<PxU16> islandLabels; 
+	islandLabels.resize(ccdBodyCount);
+	Array<const PxsCCDBody*> stack; 
+	stack.reserve(ccdBodyCount);
+	stack.forceSize_Unsafe(ccdBodyCount);
+
+
+	//Initialize all islands labels (for each body) to be unitialized
+	mIslandSizes.forceSize_Unsafe(0);
+	mIslandSizes.reserve(ccdBodyCount + 1);
+	mIslandSizes.forceSize_Unsafe(ccdBodyCount + 1);
+	for (PxU32 j = 0; j < ccdBodyCount; j++)
+		islandLabels[j] = noLabelYet;
+
+	PxU16 islandCount = 0;
+	PxU32 stackSize = 0;
+	const PxsCCDBody* top = NULL;
+	
+	for (PxU32 j = 0; j < ccdBodyCount; j++)
+	{
+		//If the body has already been labelled, continue
+		if (islandLabels[j] != noLabelYet)
+			continue;
+
+		top = &mCCDBodies[j];
+		//Otherwise push it back into the queue and proceed
+		islandLabels[j] = islandCount;
+		
+		stack[stackSize++] = top;		
+		// assign new label to unlabeled atom
+		// assign the same label to all connected nodes using stack traversal
+		PxU16 islandSize = 0;
+		while (stackSize > 0)
+		{
+			--stackSize;
+			const PxsCCDBody* ccdb = top;
+			top = stack[PxMax(1u, stackSize)-1];
+
+			PxsCCDOverlap* overlaps = ccdb->mOverlappingObjects;
+			while(overlaps)
+			{
+				if (islandLabels[overlaps->mBody->mIndex] == noLabelYet) // non-static & unlabeled?
+				{
+					islandLabels[overlaps->mBody->mIndex] = islandCount;
+					stack[stackSize++] = overlaps->mBody; // push adjacent node to the top of the stack
+					top = overlaps->mBody;
+					islandSize++;
+				}
+				overlaps = overlaps->mNext;
+			}
+		}
+		//Record island size
+		mIslandSizes[islandCount] = PxU16(islandSize + 1);
+		islandCount++;
+	}
+
+		// --------------------------------------------------------------------------------------
+	// label pairs with island ids
+	// (need an extra loop since we don't maintain a mapping from atom to all of it's pairs)
+	mCCDIslandHistogram.clear(); // number of pairs per island
+	mCCDIslandHistogram.resize(islandCount);
+
+	PxU32 totalActivePairs = 0;
+	for (PxU32 j = 0, n = mCCDPtrPairs.size(); j < n; j++)
+	{
+		const PxU32 staticLabel = 0xFFFFffff;
+		PxsCCDPair& p = *mCCDPtrPairs[j];
+		PxU32 id0 = p.mBa0 && !p.mBa0->isKinematic()? islandLabels[p.mBa0->mCCD->getIndex()] : staticLabel;
+		PxU32 id1 = p.mBa1 && !p.mBa1->isKinematic()? islandLabels[p.mBa1->mCCD->getIndex()] : staticLabel;
+		PX_ASSERT(id0 != 0xFFFF || id1 != 0xFFFF);
+
+		PX_ASSERT(id0 == staticLabel || id1 == staticLabel || (id0 == id1));
+
+		p.mIslandId = PxMin(id0, id1);
+		mCCDIslandHistogram[p.mIslandId] ++;
+		PX_ASSERT(p.mIslandId != staticLabel);
+		totalActivePairs++;
+	}
+
+	PxU16 count = 0;
+	for(PxU16 a = 0; a < islandCount+1; ++a)
+	{
+		PxU16 islandSize = mIslandSizes[a];
+		mIslandSizes[a] = count;
+		count += islandSize;
+	}
+
+	mIslandBodies.forceSize_Unsafe(0);
+	mIslandBodies.reserve(ccdBodyCount);
+	mIslandBodies.forceSize_Unsafe(ccdBodyCount);
+	for(PxU32 a = 0; a < mCCDBodies.size(); ++a)
+	{
+		const PxU32 island = islandLabels[mCCDBodies[a].mIndex];
+		PxU16 writeIndex = mIslandSizes[island];
+		mIslandSizes[island] = PxU16(writeIndex + 1);
+		mIslandBodies[writeIndex] = &mCCDBodies[a];
+	}
+
+	// --------------------------------------------------------------------------------------
+	// setup tasks
+	mPostCCDDepenetrateTask.setContinuation(continuation);
+	mPostCCDAdvanceTask.setContinuation(&mPostCCDDepenetrateTask);
+	mPostCCDSweepTask.setContinuation(&mPostCCDAdvanceTask);
+
+	// --------------------------------------------------------------------------------------
+	// sort all pairs by islands
+	shdfnd::sort(mCCDPtrPairs.begin(), mCCDPtrPairs.size(), IslandPtrCompare());
+
+	// --------------------------------------------------------------------------------------
+	// sweep all CCD pairs
+	const PxU32 nPairs = mCCDPtrPairs.size();
+	const PxU32 numThreads = PxMax(1u, mContext->mTaskManager->getCpuDispatcher()->getWorkerCount()); PX_ASSERT(numThreads > 0);
+	mCCDPairsPerBatch = PxMax<PxU32>((nPairs)/numThreads, 1);
+
+	for (PxU32 batchBegin = 0; batchBegin < nPairs; batchBegin += mCCDPairsPerBatch)
+	{
+		void* ptr = mContext->mTaskPool.allocate(sizeof(PxsCCDSweepTask));
+		PX_ASSERT_WITH_MESSAGE(ptr, "Failed to allocate PxsCCDSweepTask");
+		const PxU32 batchEnd = PxMin(nPairs, batchBegin + mCCDPairsPerBatch);
+		PX_ASSERT(batchEnd >= batchBegin);
+		PxsCCDSweepTask* task = PX_PLACEMENT_NEW(ptr, PxsCCDSweepTask)(
+			mCCDPtrPairs.begin() + batchBegin, batchEnd - batchBegin);
+		task->setContinuation(*mContext->mTaskManager, &mPostCCDSweepTask);
+		task->removeReference();
+	}
+
+	mPostCCDSweepTask.removeReference();
+	mPostCCDAdvanceTask.removeReference();
+	mPostCCDDepenetrateTask.removeReference();
+}
+
+void PxsCCDContext::postCCDSweep(PxBaseTask* continuation)
+{
+	// --------------------------------------------------------------------------------------
+	// batch up the islands and send them over to worker threads
+	PxU32 firstIslandPair = 0;
+	PxU32 islandCount = mCCDIslandHistogram.size();
+	for (PxU32 firstIslandInBatch = 0; firstIslandInBatch < islandCount;)
+	{
+		PxU32 pairSum = 0;
+		PxU32 lastIslandInBatch = firstIslandInBatch+1;
+		PxU32 j;
+		// add up the numbers in the histogram until we reach target pairsPerBatch
+		for (j = firstIslandInBatch; j < islandCount; j++)
+		{
+			pairSum += mCCDIslandHistogram[j];
+			if (pairSum > mCCDPairsPerBatch)
+			{
+				lastIslandInBatch = j+1;
+				break;
+			}
+		}
+		if (j == islandCount) // j is islandCount if not enough pairs were left to fill up to pairsPerBatch
+		{
+			if (pairSum == 0)
+				break; // we are done and there are no islands in this batch
+			lastIslandInBatch = islandCount;
+		}
+
+		void* ptr = mContext->mTaskPool.allocate(sizeof(PxsCCDAdvanceTask));
+		PX_ASSERT_WITH_MESSAGE(ptr , "Failed to allocate PxsCCDSweepTask");
+		bool clipTrajectory = (miCCDPass == mCCDMaxPasses-1);
+		PxsCCDAdvanceTask* task = PX_PLACEMENT_NEW(ptr, PxsCCDAdvanceTask) (
+			mCCDPtrPairs.begin(), mCCDPtrPairs.size(), mCCDBodies, mContext, this, mCCDThreadContext->mDt, miCCDPass, 
+			firstIslandPair, firstIslandInBatch, lastIslandInBatch-firstIslandInBatch, islandCount, 
+			mIslandBodies.begin(), mIslandSizes.begin(), clipTrajectory, mDisableCCDResweep,
+			&mSweepTotalHits);
+		firstIslandInBatch = lastIslandInBatch;
+		firstIslandPair += pairSum;		
+		task->setContinuation(*mContext->mTaskManager, continuation);
+		task->removeReference();
+	} // for iIsland
+}
+
+void PxsCCDContext::postCCDAdvance(PxBaseTask* /*continuation*/)
+{	
+	// --------------------------------------------------------------------------------------
+	// contact notifications: update touch status (multi-threading this section would probably slow it down but might be worth a try)
+	PxU32 countLost = 0, countFound = 0, countRetouch = 0;
+
+	PxU32 islandCount = mCCDIslandHistogram.size();
+	PxU32 index = 0;
+
+	for (PxU32 island = 0; island < islandCount; ++island)
+	{
+		PxU32 islandEnd = mCCDIslandHistogram[island] + index;
+		for(PxU32 j = index; j < islandEnd; ++j)
+		{
+			PxsCCDPair& p = *mCCDPtrPairs[j];
+			//The CCD pairs are ordered by TOI. If we reach a TOI > 1, we can terminate
+			if(p.mMinToi > 1.f)
+				break;
+		
+			//If this was the earliest touch for the pair of bodies, we can notify the user about it. If not, it's a future collision that we haven't stepped to yet
+			if(p.mIsEarliestToiHit)
+			{
+				//Flag that we had a CCD contact
+				p.mCm->setHadCCDContact();
+
+				//Test/set the changed touch map
+				PxU16 oldTouch = p.mCm->getTouchStatus();
+				if (!oldTouch)
+				{
+					mContext->mContactManagerTouchEvent.growAndSet(p.mCm->getIndex());
+					p.mCm->mNpUnit.statusFlags = PxU16((p.mCm->mNpUnit.statusFlags & (~PxcNpWorkUnitStatusFlag::eHAS_NO_TOUCH)) | PxcNpWorkUnitStatusFlag::eHAS_TOUCH);
+					//Also need to write it in the CmOutput structure!!!!!
+					
+					//The achieve this, we need to unregister the CM from the Nphase, then re-register it with the status set. This is the only way to force a push to the GPU
+					mNphaseContext.unregisterContactManager(p.mCm);
+					mNphaseContext.registerContactManager(p.mCm, 1, 0);
+					countFound++;
+				}
+				else
+				{
+					mContext->mContactManagerTouchEvent.growAndSet(p.mCm->getIndex());
+					p.mCm->raiseCCDRetouch();
+					countRetouch++;
+				}
+
+				//Do we want to create reports?
+				const bool createReports = 
+					p.mCm->mNpUnit.flags & PxcNpWorkUnitFlag::eOUTPUT_CONTACTS
+					|| (p.mCm->mNpUnit.flags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD
+					&& ((p.mCm->mNpUnit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY0 && static_cast<const PxsBodyCore*>(p.mCm->mNpUnit.rigidCore0)->shouldCreateContactReports())
+					|| (p.mCm->mNpUnit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1 && static_cast<const PxsBodyCore*>(p.mCm->mNpUnit.rigidCore1)->shouldCreateContactReports())));
+
+				if(createReports)
+				{
+					mContext->mContactManagersWithCCDTouch.growAndSet(p.mCm->getIndex());
+
+					const PxU32 numContacts = 1;
+					PxsMaterialInfo matInfo;
+					Gu::ContactBuffer& buffer = mCCDThreadContext->mContactBuffer;
+
+					Gu::ContactPoint& cp = buffer.contacts[0];
+					cp.point = p.mMinToiPoint;
+					cp.normal = -p.mMinToiNormal;						//KS - discrete contact gen produces contacts pointing in the opposite direction to CCD sweeps
+					cp.internalFaceIndex1 = p.mFaceIndex;
+					cp.separation = 0.0f;
+					cp.restitution = p.mRestitution;
+					cp.dynamicFriction = p.mDynamicFriction;
+					cp.staticFriction = p.mStaticFriction;
+					cp.targetVel = PxVec3(0.f);
+					cp.maxImpulse = PX_MAX_REAL;
+					
+					matInfo.mMaterialIndex0 = p.mMaterialIndex0;
+					matInfo.mMaterialIndex1 = p.mMaterialIndex1;
+
+					//Write contact stream for the contact. This will allocate memory for the contacts and forces
+					PxReal* contactForces;
+					//PxU8* contactStream;
+					PxU8* contactPatches;
+					PxU8* contactPoints;
+					PxU16 contactStreamSize;
+					PxU8 contactCount;
+					PxU8 nbPatches;
+					PxsCCDContactHeader* ccdHeader = reinterpret_cast<PxsCCDContactHeader*>(p.mCm->mNpUnit.ccdContacts);
+					if (writeCompressedContact(buffer.contacts, numContacts, mCCDThreadContext, contactCount, contactPatches,
+						contactPoints, contactStreamSize, contactForces, numContacts*sizeof(PxReal), mCCDThreadContext->mMaterialManager,
+												((p.mCm->mNpUnit.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT) != 0), true, &matInfo, nbPatches, sizeof(PxsCCDContactHeader),NULL, NULL,
+												false, NULL, NULL, NULL, p.mFaceIndex != PXC_CONTACT_NO_FACE_INDEX))
+					{
+						PxsCCDContactHeader* newCCDHeader = reinterpret_cast<PxsCCDContactHeader*>(contactPatches);
+						newCCDHeader->contactStreamSize = Ps::to16(contactStreamSize);
+						newCCDHeader->isFromPreviousPass = 0;
+
+						p.mCm->mNpUnit.ccdContacts = contactPatches;	// put the latest stream at the head of the linked list since it needs to get accessed every CCD pass
+																	// to prepare the reports
+
+						if (!ccdHeader)
+							newCCDHeader->nextStream = NULL;
+						else
+						{
+							newCCDHeader->nextStream = ccdHeader;
+							ccdHeader->isFromPreviousPass = 1;
+						}
+
+						//And write the force and contact count
+						PX_ASSERT(contactForces != NULL);
+						contactForces[0] = p.mAppliedForce;
+					}
+					else if (!ccdHeader)
+					{
+						p.mCm->mNpUnit.ccdContacts = NULL;
+						// we do not set the status flag on failure because the pair might have written
+						// a contact stream sucessfully during discrete collision this frame.
+					}
+					else
+						ccdHeader->isFromPreviousPass = 1;
+
+					//If the touch event already existed, the solver would have already configured the threshold stream
+					if((p.mCm->mNpUnit.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0 | PxcNpWorkUnitFlag::eARTICULATION_BODY1)) == 0 && p.mAppliedForce)
+					{
+#if 0
+						ThresholdStreamElement elt;
+						elt.normalForce = p.mAppliedForce;
+						elt.threshold = PxMin<float>(p.mBa0 == NULL ? PX_MAX_REAL : p.mBa0->mCore->contactReportThreshold, p.mBa1 == NULL ? PX_MAX_REAL : 
+							p.mBa1->mCore->contactReportThreshold);
+						elt.body0 = p.mBa0;
+						elt.body1 = p.mBa1;
+						Ps::order(elt.body0,elt.body1);
+						PX_ASSERT(elt.body0 < elt.body1);
+						mThresholdStream.pushBack(elt);
+#endif
+					}
+				}
+			}
+		}
+		index = islandEnd;
+	}
+
+	mContext->mCMTouchEventCount[PXS_LOST_TOUCH_COUNT] += countLost;
+	mContext->mCMTouchEventCount[PXS_NEW_TOUCH_COUNT] += countFound;
+	mContext->mCMTouchEventCount[PXS_CCD_RETOUCH_COUNT] += countRetouch;
+}
+
+void PxsCCDContext::postCCDDepenetrate(PxBaseTask* /*continuation*/)
+{
+	// --------------------------------------------------------------------------------------
+	// reset mOverlappingShapes array for all bodies
+	// we do it each pass because this set can change due to movement as well as new objects
+	// becoming fast moving due to intra-frame impacts
+
+	for (PxU32 j = 0; j < mCCDBodies.size(); j ++)
+	{
+		mCCDBodies[j].mOverlappingObjects = NULL;
+	}
+
+	mCCDOverlaps.clear_NoDelete();
+
+	updateCCDEnd();
+
+	mContext->putNpThreadContext(mCCDThreadContext);
+
+	flushCCDLog();
+}
+
+Cm::SpatialVector PxsRigidBody::getPreSolverVelocities() const
+{
+	if (mCCD)
+		return mCCD->mPreSolverVelocity;
+	return Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f));
+}
+
+
+PxTransform PxsRigidBody::getAdvancedTransform(PxReal toi) const
+{
+	//If it is kinematic, just return identity. We don't fully support kinematics yet
+	if (isKinematic())
+		return PxTransform(PxIdentity);
+
+	//Otherwise we interpolate the pose between the current and previous pose and return that pose
+	PxVec3 newLastP = mLastTransform.p*(1.0f-toi) + mCore->body2World.p*toi; // advance mLastTransform position to toi
+	PxQuat newLastQ = slerp(toi, getLastCCDTransform().q, mCore->body2World.q); // advance mLastTransform rotation to toi
+	return PxTransform(newLastP, newLastQ);
+}
+
+void PxsRigidBody::advancePrevPoseToToi(PxReal toi)
+{
+	//If this is kinematic, just return
+	if (isKinematic())
+		return;
+
+	//update latest pose
+	PxVec3 newLastP = mLastTransform.p*(1.0f-toi) + mCore->body2World.p*toi; // advance mLastTransform position to toi
+	mLastTransform.p = newLastP;
+#if CCD_ROTATION_LOCKING
+	mCore->body2World.q = getLastCCDTransform().q;
+#else
+	// slerp from last transform to current transform with ratio of toi
+	PxQuat newLastQ = slerp(toi, getLastCCDTransform().q, mCore->body2World.q); // advance mLastTransform rotation to toi
+	mLastTransform.q = newLastQ;
+#endif
+
+	
+
+}
+
+
+void PxsRigidBody::advanceToToi(PxReal toi, PxReal dt, bool clip)
+{
+	if (isKinematic())
+		return;
+
+	
+	if (clip)
+	{
+		//If clip is true, we set the previous and current pose to be the same. This basically makes the object appear stationary in the CCD
+		mCore->body2World.p = getLastCCDTransform().p;
+#if !CCD_ROTATION_LOCKING
+		mCore->body2World.q = getLastCCDTransform().q;
+#endif
+	}
+	else
+	{
+		// advance new CCD target after impact to remaining toi using post-impact velocities
+		mCore->body2World.p = getLastCCDTransform().p + getLinearVelocity() * dt * (1.0f - toi);
+#if !CCD_ROTATION_LOCKING
+		PxVec3 angularDelta = getAngularVelocity() * dt * (1.0f - toi);
+		PxReal deltaMag = angularDelta.magnitude();
+		PxVec3 deltaAng = deltaMag > 1e-20f ? angularDelta / deltaMag : PxVec3(1.0f, 0.0f, 0.0f);
+		PxQuat angularQuat(deltaMag, deltaAng);
+		mCore->body2World.q = getLastCCDTransform().q * angularQuat;
+#endif
+		PX_ASSERT(mCore->body2World.isSane());
+	}
+
+	// rescale total time left to elapse this frame
+	mCCD->mTimeLeft = PxMax(mCCD->mTimeLeft * (1.0f - toi), CCD_MIN_TIME_LEFT);
+}
+
+
+void PxsCCDContext::runCCDModifiableContact(PxModifiableContact* PX_RESTRICT contacts, PxU32 contactCount, const PxsShapeCore* PX_RESTRICT shapeCore0, 
+											const PxsShapeCore* PX_RESTRICT shapeCore1, const PxsRigidCore* PX_RESTRICT rigidCore0, const PxsRigidCore* PX_RESTRICT rigidCore1,
+											const PxsRigidBody* PX_RESTRICT rigid0, const PxsRigidBody* PX_RESTRICT rigid1)
+{
+	if(!mCCDContactModifyCallback)
+		return;
+
+	class PxcContactSet: public PxContactSet
+	{
+	public:
+		PxcContactSet(PxU32 count, PxModifiableContact* contacts_)
+		{
+			mContacts = contacts_;
+			mCount = count;
+		}
+	};
+	{
+			PxContactModifyPair p;
+
+			p.shape[0] = gPxvOffsetTable.convertPxsShape2Px(shapeCore0);
+			p.shape[1] = gPxvOffsetTable.convertPxsShape2Px(shapeCore1);
+
+			p.actor[0] = rigid0 != NULL ? gPxvOffsetTable.convertPxsRigidCore2PxRigidBody(rigidCore0) 
+										: gPxvOffsetTable.convertPxsRigidCore2PxRigidStatic(rigidCore0);
+
+			p.actor[1] = rigid1 != NULL ? gPxvOffsetTable.convertPxsRigidCore2PxRigidBody(rigidCore1) 
+										: gPxvOffsetTable.convertPxsRigidCore2PxRigidStatic(rigidCore1);
+
+			p.transform[0] = getShapeAbsPose(shapeCore0, rigidCore0, PxU32(rigid0 != NULL));
+			p.transform[1] = getShapeAbsPose(shapeCore1, rigidCore1, PxU32(rigid1 != NULL));
+
+			static_cast<PxcContactSet&>(p.contacts) = 
+				PxcContactSet(contactCount, contacts);
+
+			mCCDContactModifyCallback->onCCDContactModify(&p, 1);
+	}
+}
+
+
+} //namespace physx
+
+
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsContactManager.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsContactManager.cpp
new file mode 100644
index 00000000..97e9fe85
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsContactManager.cpp
@@ -0,0 +1,87 @@
+// 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 "PxsContactManager.h"
+#include "PxsRigidBody.h"
+#include "PxcContactMethodImpl.h"
+#include "PxvManager.h"
+#include "PxsIslandSim.h"
+
+using namespace physx;
+
+PxsContactManager::PxsContactManager(PxsContext*, PxU32 index) /*:
+	mUserData	(NULL)*/
+{
+	mFlags = 0;
+
+	// PT: TODO: any reason why we don't initialize all members here, e.g. shapeCore pointers?
+	mNpUnit.index				= index;
+	mNpUnit.rigidCore0			= NULL;
+	mNpUnit.rigidCore1			= NULL;
+	mNpUnit.restDistance		= 0;
+	mNpUnit.dominance0			= 1u;
+	mNpUnit.dominance1			= 1u;
+	mNpUnit.frictionDataPtr		= NULL;
+	mNpUnit.frictionPatchCount	= 0;
+}
+
+PxsContactManager::~PxsContactManager()
+{
+}
+
+
+void PxsContactManager::setCCD(bool enable)
+{
+	PxU32 flags = mFlags & (~PXS_CM_CCD_CONTACT);
+	if (enable)
+		flags |= PXS_CM_CCD_LINEAR;
+	else
+		flags &= ~PXS_CM_CCD_LINEAR;
+
+	mFlags = flags;
+}
+
+
+
+void PxsContactManager::resetCachedState()
+{ 
+	// happens when the body transform or shape relative transform changes.
+
+	PxcNpWorkUnitClearCachedState(mNpUnit);
+}
+
+void PxsContactManager::resetFrictionCachedState()
+{ 
+	// happens when the body transform or shape relative transform changes.
+
+	PxcNpWorkUnitClearFrictionCachedState(mNpUnit);
+}
+
+
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsContext.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsContext.cpp
new file mode 100644
index 00000000..0e18355b
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsContext.cpp
@@ -0,0 +1,641 @@
+// 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/PxProfiler.h"
+#include "PxvConfig.h"
+#include "PxcContactCache.h"
+#include "PxsRigidBody.h"
+#include "PxsContactManager.h"
+#include "PxsContext.h"
+#include "PxPhysXConfig.h"
+
+#include "CmBitMap.h"
+#include "CmFlushPool.h"
+
+#include "PxsMaterialManager.h"
+#include "PxSceneDesc.h"
+#include "PxsCCD.h"
+#include "PxvGeometry.h"
+#include "PxvManager.h"
+#include "PxsSimpleIslandManager.h"
+
+#if PX_SUPPORT_GPU_PHYSX
+#include "task/PxGpuDispatcher.h"
+#include "PxPhysXGpu.h"
+#endif
+
+#include "PxcNpContactPrepShared.h"
+#include "PxcNpCache.h"
+
+
+using namespace physx;
+using namespace physx::shdfnd;
+
+#define PXS_CONTACTMANAGER_SLABSIZE 1024
+#define PXS_MAX_CONTACTMANAGER_SLABS 64
+
+#define PXS_BODYSHAPE_SLABSIZE 1024
+#define PXS_MAX_BODYSHAPE_SLABS 16
+
+
+void PxsCMUpdateTask::release()
+{
+	// We used to do Task::release(); here before fixing DE1106 (xbox pure virtual crash)
+	// Release in turn causes the dependent tasks to start running
+	// The problem was that between the time release was called and by the time we got to the destructor
+	// The task chain would get all the way to scene finalization code which would reset the allocation pool
+	// And a new task would get allocated at the same address, then we would invoke the destructor on that freshly created task
+	// This could potentially cause any number of other problems, it is suprising that it only manifested itself
+	// as a pure virtual crash
+	PxBaseTask* saveContinuation = mCont;
+	this->~PxsCMUpdateTask();
+	if (saveContinuation)
+		saveContinuation->removeReference();
+}
+
+
+
+PxsContext::PxsContext(const PxSceneDesc& desc, PxTaskManager* taskManager, Cm::FlushPool& taskPool, PxU64 contextID) :
+	mNpThreadContextPool		(this),
+	mContactManagerPool			("mContactManagerPool", this, 256, 8192),
+	mManifoldPool				("mManifoldPool", 256),
+	mSphereManifoldPool			("mSphereManifoldPool", 256),
+	mContactModifyCallback		(NULL),
+	mNpImplementationContext	(NULL),
+	mNpFallbackImplementationContext(NULL),
+	mTaskManager				(taskManager),
+	mTaskPool					(taskPool),
+	mPCM						(desc.flags & PxSceneFlag::eENABLE_PCM),
+	mContactCache				(false),
+	mCreateAveragePoint			(desc.flags & PxSceneFlag::eENABLE_AVERAGE_POINT),
+	mContextID					(contextID)
+{
+	clearManagerTouchEvents();
+	mVisualizationCullingBox.setMaximal();
+
+	PxMemZero(mVisualizationParams, sizeof(PxReal) * PxVisualizationParameter::eNUM_VALUES);
+
+	mNpMemBlockPool.init(desc.nbContactDataBlocks, desc.maxNbContactDataBlocks);
+}
+
+PxsContext::~PxsContext()
+{
+	if(mTransformCache)
+	{
+		mTransformCache->~PxsTransformCache();
+		PX_FREE(mTransformCache);
+	} 
+	mTransformCache = NULL;
+
+	mContactManagerPool.destroy(); //manually destroy the contact manager pool, otherwise pool deletion order is random and we can get into trouble with references into other pools needed during destruction.
+}
+
+// =========================== Create methods
+namespace physx
+{
+	bool gEnablePCMCaching[PxGeometryType::eGEOMETRY_COUNT][PxGeometryType::eGEOMETRY_COUNT] =
+	{
+		//eSPHERE,
+		{
+			false,				//eSPHERE
+			false,				//ePLANE
+			false,				//eCAPSULE
+			false,				//eBOX
+			true,				//eCONVEXMESH
+			true,				//eTRIANGLEMESH
+			true				//eHEIGHTFIELD
+		},
+
+		//ePLANE
+		{
+			false,				//eSPHERE
+			false,				//ePLANE
+			true,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			false,				//eTRIANGLEMESH
+			false				//eHEIGHTFIELD
+		},
+
+		//eCAPSULE,
+		{
+			false,				//eSPHERE
+			true,				//ePLANE
+			false,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			true,				//eTRIANGLEMESH
+			true				//eHEIGHTFIELD
+		},
+
+		//eBOX,
+		{
+			false,				//eSPHERE
+			true,				//ePLANE
+			true,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			true,				//eTRIANGLEMESH
+			true				//eHEIGHTFIELD
+		},
+
+		//eCONVEXMESH,
+		{
+			true,				//eSPHERE
+			true,				//ePLANE
+			true,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			true,				//eTRIANGLEMESH
+			true				//eHEIGHTFIELD
+		},
+		//eTRIANGLEMESH,
+		{
+			true,				//eSPHERE
+			false,				//ePLANE
+			true,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			false,				//eTRIANGLEMESH
+			false				//eHEIGHTFIELD
+		},
+
+		//eHEIGHTFIELD,
+		{
+			true,				//eSPHERE
+			false,				//ePLANE
+			true,				//eCAPSULE
+			true,				//eBOX
+			true,				//eCONVEXMESH
+			false,				//eTRIANGLEMESH
+			false				//eHEIGHTFIELD
+		}
+	};
+}
+
+void PxsContext::createTransformCache(Ps::VirtualAllocatorCallback& allocatorCallback)
+{
+	mTransformCache = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(PxsTransformCache), PX_DEBUG_EXP("PxsTransformCache")), PxsTransformCache(allocatorCallback));
+}
+
+PxsContactManager* PxsContext::createContactManager(PxsContactManager* contactManager, const bool useCCD)
+{
+	PxsContactManager* cm = contactManager? contactManager : mContactManagerPool.get();
+
+	if(cm)
+	{
+		PxcNpWorkUnitClearContactState(cm->getWorkUnit());
+		PxcNpWorkUnitClearCachedState(cm->getWorkUnit());
+
+		if (contactManager == NULL)
+		{
+			if (cm->getIndex() >= mActiveContactManager.size())
+			{
+				PxU32 newSize = (2 * cm->getIndex() + 256)&~255;
+				mActiveContactManager.resize(newSize);
+			}
+			mActiveContactManager.set(cm->getIndex());
+
+			if (useCCD)
+			{
+				if (cm->getIndex() >= mActiveContactManagersWithCCD.size())
+				{
+					PxU32 newSize = (2 * cm->getIndex() + 256)&~255;
+					mActiveContactManagersWithCCD.resize(newSize);
+				}
+				mActiveContactManagersWithCCD.set(cm->getIndex());
+			}
+		}
+	}
+	else
+	{
+		PX_WARN_ONCE("Reached limit of contact pairs.");
+	}
+
+	return cm;
+}
+
+void PxsContext::createCache(Gu::Cache& cache, PxsContactManager* cm, PxU8 geomType0, PxU8 geomType1)
+{
+	if(cm)
+	{
+		if(mPCM)
+		{
+			if(gEnablePCMCaching[geomType0][geomType1])
+			{
+				if(geomType0 <= PxGeometryType::eCONVEXMESH && 
+				   geomType1 <= PxGeometryType::eCONVEXMESH)
+				{
+					if(geomType0 == PxGeometryType::eSPHERE || geomType1 == PxGeometryType::eSPHERE)
+					{
+						Gu::PersistentContactManifold* manifold = mSphereManifoldPool.allocate();
+						new(manifold) Gu::SpherePersistentContactManifold();
+						cache.setManifold(manifold);
+					}
+					else
+					{
+						Gu::PersistentContactManifold* manifold = mManifoldPool.allocate();
+						new(manifold) Gu::LargePersistentContactManifold();
+						cache.setManifold(manifold);
+
+					}
+					cache.getManifold().clearManifold();
+
+				}
+				else
+				{
+					//ML: raised 1 to indicate the manifold is multiManifold which is for contact gen in mesh/height field
+					//cache.manifold = 1;
+					cache.setMultiManifold(NULL);
+				}
+			}
+			else
+			{
+				//cache.manifold =  0;
+				cache.mCachedData = NULL;
+				cache.mManifoldFlags = 0;
+			}			
+		}
+	}
+}
+
+void PxsContext::destroyContactManager(PxsContactManager* cm)
+{
+	const PxU32 idx = cm->getIndex();
+	if (cm->getCCD())
+		mActiveContactManagersWithCCD.growAndReset(idx);
+	mActiveContactManager.growAndReset(idx);
+	mContactManagerTouchEvent.growAndReset(idx);
+	mContactManagerPatchChangeEvent.growAndReset(idx);
+	mContactManagerPool.put(cm);
+}
+
+void PxsContext::destroyCache(Gu::Cache& cache)
+{
+	if(cache.isManifold())
+	{
+		if(!cache.isMultiManifold())
+		{
+			Gu::PersistentContactManifold& manifold = cache.getManifold();
+			if (manifold.mCapacity == GU_SPHERE_MANIFOLD_CACHE_SIZE)
+			{
+				mSphereManifoldPool.deallocate(static_cast<Gu::SpherePersistentContactManifold*>(&manifold));
+			}
+			else
+			{
+				mManifoldPool.deallocate(static_cast<Gu::LargePersistentContactManifold*>(&manifold));
+			}
+		}
+		cache.mCachedData = NULL;
+		cache.mManifoldFlags = 0;
+	}
+}
+
+void PxsContext::setScratchBlock(void* addr, PxU32 size)
+{
+	mScratchAllocator.setBlock(addr, size);
+}
+
+void PxsContext::setContactDistance(Ps::Array<PxReal, Ps::VirtualAllocator>* contactDistance)
+{
+	mContactDistance = contactDistance;
+}
+
+
+void PxsContext::shiftOrigin(const PxVec3& shift)
+{
+	// transform cache
+	mTransformCache->shiftTransforms(-shift);
+
+#if 0
+	if (getContactCacheFlag())
+	{
+		//Iterate all active contact managers
+		Cm::BitMap::Iterator it(mActiveContactManager);
+		PxU32 index = it.getNext();
+		while(index != Cm::BitMap::Iterator::DONE)
+		{
+			PxsContactManager* cm = mContactManagerPool.findByIndexFast(index);
+
+			PxcNpWorkUnit& npwUnit = cm->getWorkUnit();
+
+			// contact cache
+			if(!npwUnit.pairCache.isManifold())
+			{
+				PxU8* contactCachePtr = npwUnit.pairCache.mCachedData;
+				if (contactCachePtr)
+				{
+					PxcLocalContactsCache* lcc;
+					PxU8* contacts = PxcNpCacheRead(npwUnit.pairCache, lcc);
+#ifdef _DEBUG
+					PxcLocalContactsCache testCache;
+					PxU32 testBytes;
+					const PxU8* testPtr = PxcNpCacheRead2(npwUnit.pairCache, testCache, testBytes);
+#endif
+					lcc->mTransform0.p -= shift;
+					lcc->mTransform1.p -= shift;
+				
+					const PxU32 nbContacts = lcc->mNbCachedContacts;
+					const bool sameNormal = lcc->mSameNormal;
+					const bool useFaceIndices = lcc->mUseFaceIndices;
+				
+					for(PxU32 i=0; i < nbContacts; i++)
+					{
+						if (i != nbContacts-1)
+							Ps::prefetchLine(contacts, 128);
+
+						if(!i || !sameNormal)
+							contacts += sizeof(PxVec3);
+
+						PxVec3* cachedPoint	= reinterpret_cast<PxVec3*>(contacts);
+						*cachedPoint -= shift;
+						contacts += sizeof(PxVec3);
+						contacts += sizeof(PxReal);
+
+						if(useFaceIndices)
+							contacts += 2 * sizeof(PxU32);
+					}
+#ifdef _DEBUG
+					PX_ASSERT(contacts == (testPtr + testBytes));
+#endif
+				}
+			}
+
+			index = it.getNext();
+		}
+
+	}
+#endif
+
+	//
+	// adjust visualization culling box
+	//
+	PxBounds3 maximalBounds;
+	maximalBounds.setMaximal();
+	if ((mVisualizationCullingBox.minimum != maximalBounds.minimum) || (mVisualizationCullingBox.maximum != maximalBounds.maximum))
+	{
+		mVisualizationCullingBox.minimum -= shift;
+		mVisualizationCullingBox.maximum -= shift;
+	}
+}
+
+void PxsContext::swapStreams()
+{
+	mNpMemBlockPool.swapNpCacheStreams();
+}
+
+void PxsContext::mergeCMDiscreteUpdateResults(PxBaseTask* /*continuation*/)
+{
+	PX_PROFILE_ZONE("Sim.narrowPhaseMerge", mContextID);
+
+	this->mNpImplementationContext->appendContactManagers();
+
+	//Note: the iterator extracts all the items and returns them to the cache on destruction(for thread safety).
+	PxcThreadCoherentCacheIterator<PxcNpThreadContext, PxcNpContext> threadContextIt(mNpThreadContextPool);
+
+	for(PxcNpThreadContext* threadContext = threadContextIt.getNext(); threadContext; threadContext = threadContextIt.getNext())
+	{
+		mCMTouchEventCount[PXS_LOST_TOUCH_COUNT] += threadContext->getLocalLostTouchCount();
+		mCMTouchEventCount[PXS_NEW_TOUCH_COUNT] += threadContext->getLocalNewTouchCount();
+		mCMTouchEventCount[PXS_PATCH_FOUND_COUNT] += threadContext->getLocalFoundPatchCount();
+		mCMTouchEventCount[PXS_PATCH_LOST_COUNT] += threadContext->getLocalLostPatchCount();
+
+#if PX_ENABLE_SIM_STATS
+		for(PxU32 i=0;i<PxGeometryType::eGEOMETRY_COUNT;i++)
+		{
+	#if PX_DEBUG
+			for(PxU32 j=0; j<i; j++)
+				PX_ASSERT(!threadContext->mDiscreteContactPairs[i][j]);
+	#endif
+			for(PxU32 j=i; j<PxGeometryType::eGEOMETRY_COUNT; j++)
+			{
+				const PxU32 nb = threadContext->mDiscreteContactPairs[i][j];
+				const PxU32 nbModified = threadContext->mModifiedContactPairs[i][j];
+				mSimStats.mNbDiscreteContactPairs[i][j] += nb;
+				mSimStats.mNbModifiedContactPairs[i][j] += nbModified;
+				mSimStats.mNbDiscreteContactPairsTotal += nb;
+			}
+		}
+
+		mSimStats.mNbDiscreteContactPairsWithCacheHits += threadContext->mNbDiscreteContactPairsWithCacheHits;
+		mSimStats.mNbDiscreteContactPairsWithContacts += threadContext->mNbDiscreteContactPairsWithContacts;
+
+		mSimStats.mTotalCompressedContactSize += threadContext->mCompressedCacheSize;
+		//KS - this data is not available yet
+		//mSimStats.mTotalConstraintSize += threadContext->mConstraintSize;
+		threadContext->clearStats();
+#endif
+		mContactManagerTouchEvent.combineInPlace<Cm::BitMap::OR>(threadContext->getLocalChangeTouch());
+		mContactManagerPatchChangeEvent.combineInPlace<Cm::BitMap::OR>(threadContext->getLocalPatchChangeMap());
+		mTotalCompressedCacheSize += threadContext->mTotalCompressedCacheSize;
+		mMaxPatches = PxMax(mMaxPatches, threadContext->mMaxPatches);
+
+		threadContext->mTotalCompressedCacheSize = threadContext->mMaxPatches = 0;
+	}
+}
+
+void PxsContext::setCreateContactStream(bool to)
+{ 
+	mCreateContactStream = to; 
+	PxcThreadCoherentCacheIterator<PxcNpThreadContext, PxcNpContext> threadContextIt(mNpThreadContextPool);
+	for(PxcNpThreadContext* threadContext = threadContextIt.getNext(); threadContext; threadContext = threadContextIt.getNext())
+	{
+		threadContext->setCreateContactStream(to);
+	}
+}
+
+void PxsContext::updateContactManager(PxReal dt, bool hasBoundsArrayChanged, bool hasContactDistanceChanged, PxBaseTask* continuation, PxBaseTask* firstPassContinuation)
+{
+	PX_ASSERT(mNpImplementationContext);
+	mNpImplementationContext->updateContactManager(dt, hasBoundsArrayChanged, hasContactDistanceChanged, continuation, firstPassContinuation);
+}
+
+void PxsContext::secondPassUpdateContactManager(PxReal dt, PxBaseTask* continuation)
+{
+	PX_ASSERT(mNpImplementationContext);
+	mNpImplementationContext->secondPassUpdateContactManager(dt, continuation);
+}
+
+void PxsContext::fetchUpdateContactManager()
+{
+	PX_ASSERT(mNpImplementationContext);
+	mNpImplementationContext->fetchUpdateContactManager();
+	mergeCMDiscreteUpdateResults(NULL);
+}
+
+void PxsContext::resetThreadContexts()
+{
+	//Note: the iterator extracts all the items and returns them to the cache on destruction(for thread safety).
+	PxcThreadCoherentCacheIterator<PxcNpThreadContext, PxcNpContext> threadContextIt(mNpThreadContextPool);
+	PxcNpThreadContext* threadContext = threadContextIt.getNext();
+
+	while(threadContext != NULL)
+	{
+		threadContext->reset(mContactManagerTouchEvent.size());
+		threadContext = threadContextIt.getNext();
+	}
+}
+
+bool PxsContext::getManagerTouchEventCount(int* newTouch, int* lostTouch, int* ccdTouch) const
+{
+	if(newTouch)
+		*newTouch = int(mCMTouchEventCount[PXS_NEW_TOUCH_COUNT]);
+
+	if(lostTouch)
+		*lostTouch = int(mCMTouchEventCount[PXS_LOST_TOUCH_COUNT]);
+
+	if(ccdTouch)
+		*ccdTouch = int(mCMTouchEventCount[PXS_CCD_RETOUCH_COUNT]);
+
+	return true;
+}
+
+bool PxsContext::fillManagerTouchEvents(PxvContactManagerTouchEvent* newTouch, PxI32& newTouchCount, PxvContactManagerTouchEvent* lostTouch, PxI32& lostTouchCount,
+										 PxvContactManagerTouchEvent* ccdTouch, PxI32& ccdTouchCount)
+{
+	PxU32 index;
+
+	const PxvContactManagerTouchEvent* newTouchStart = newTouch;
+	const PxvContactManagerTouchEvent* lostTouchStart = lostTouch;
+	const PxvContactManagerTouchEvent* ccdTouchStart = ccdTouch;
+
+	const PxvContactManagerTouchEvent* newTouchEnd = newTouch + newTouchCount;
+	const PxvContactManagerTouchEvent* lostTouchEnd = lostTouch + lostTouchCount;
+	const PxvContactManagerTouchEvent* ccdTouchEnd = ccdTouch + ccdTouchCount;
+
+	PX_UNUSED(newTouchEnd);
+	PX_UNUSED(lostTouchEnd);
+	PX_UNUSED(ccdTouchEnd);
+
+	Cm::BitMap::Iterator it(mContactManagerTouchEvent);
+
+	while((index = it.getNext()) != Cm::BitMap::Iterator::DONE)
+	{
+		PxsContactManager* cm = mContactManagerPool.findByIndexFast(index);
+
+		if(cm->getTouchStatus())
+		{
+			if (!cm->getHasCCDRetouch())
+			{
+				PX_ASSERT(newTouch < newTouchEnd);
+				newTouch->manager	= cm;
+				newTouch->userData	= cm->getUserData();
+				newTouch++;
+			}
+			else
+			{
+				PX_ASSERT(ccdTouch);
+				PX_ASSERT(ccdTouch < ccdTouchEnd);
+				ccdTouch->manager	= cm;
+				ccdTouch->userData	= cm->getUserData();
+				cm->clearCCDRetouch();
+				ccdTouch++;
+			}
+		}
+		else
+		{
+			PX_ASSERT(lostTouch < lostTouchEnd);
+			lostTouch->manager	= cm;
+			lostTouch->userData	= cm->getUserData();
+			lostTouch++;
+		}
+	}
+	newTouchCount = PxI32(newTouch - newTouchStart);
+	lostTouchCount = PxI32(lostTouch - lostTouchStart);
+	ccdTouchCount = PxI32(ccdTouch - ccdTouchStart);
+	return true;
+}
+
+
+
+bool PxsContext::fillManagerPatchChangedEvents(PxsContactManager** foundPatch, PxU32& foundPatchCount,
+							PxsContactManager** lostPatch, PxU32& lostPatchCount)
+{
+	Cm::BitMap::Iterator it(mContactManagerPatchChangeEvent);
+
+	PxsContactManagerOutputIterator outputs = mNpImplementationContext->getContactManagerOutputs();
+
+	PxU32 index;
+	PxsContactManager** currFoundPatch = foundPatch;
+	PxsContactManager** currLostPatch = lostPatch;
+	while((index = it.getNext()) != Cm::BitMap::Iterator::DONE)
+	{
+		PxsContactManager* cm = mContactManagerPool.findByIndexFast(index);
+		PxcNpWorkUnit& workUnit = cm->getWorkUnit();
+		PxsContactManagerOutput& output = outputs.getContactManager(workUnit.mNpIndex);
+		if(output.nbPatches > output.prevPatches)
+		{
+			PX_ASSERT(PxU32(currFoundPatch - foundPatch) < foundPatchCount);
+			*currFoundPatch = cm;
+			currFoundPatch++;
+		}
+		else if(output.nbPatches < output.prevPatches)
+		{
+			PX_ASSERT(PxU32(currLostPatch - lostPatch) < lostPatchCount);
+			*currLostPatch = cm;
+			currLostPatch++;
+		}
+	}
+
+	foundPatchCount = PxU32(currFoundPatch - foundPatch);
+	lostPatchCount = PxU32(currLostPatch - lostPatch);
+	return true;
+}
+
+
+void PxsContext::beginUpdate()
+{
+#if PX_ENABLE_SIM_STATS
+	mSimStats.clearAll();
+#endif
+}
+
+
+// Contact manager related
+
+PxReal PxsContext::getVisualizationParameter(PxVisualizationParameter::Enum param) const
+{
+	PX_ASSERT(param < PxVisualizationParameter::eNUM_VALUES);
+
+	return mVisualizationParams[param];
+}
+
+void PxsContext::setVisualizationParameter(PxVisualizationParameter::Enum param, PxReal value)
+{
+	PX_ASSERT(param < PxVisualizationParameter::eNUM_VALUES);
+	PX_ASSERT(value >= 0.0f);
+
+	mVisualizationParams[param] = value;
+}
+
+
+
+
+
+
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsDefaultMemoryManager.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsDefaultMemoryManager.cpp
new file mode 100644
index 00000000..f84efdd5
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsDefaultMemoryManager.cpp
@@ -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.  
+
+#include "PxsDefaultMemoryManager.h"
+
+namespace physx
+{
+
+	PxsDefaultMemoryManager::~PxsDefaultMemoryManager()
+	{
+		for (PxU32 i = 0; i < mAllocators.size(); ++i)
+		{
+			mAllocators[i]->~VirtualAllocatorCallback();
+			PX_FREE(mAllocators[i]);
+		}
+	}
+
+	Ps::VirtualAllocatorCallback* PxsDefaultMemoryManager::createHostMemoryAllocator(const PxU32 gpuComputeVersion)
+	{
+		PX_UNUSED(gpuComputeVersion);
+		Ps::VirtualAllocatorCallback* allocator = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(PxsDefaultMemoryAllocator), "PxsDefaultMemoryAllocator"), PxsDefaultMemoryAllocator());
+		mAllocators.pushBack(allocator);
+		return allocator;
+	}
+
+	//this is an empty stub
+	Ps::VirtualAllocatorCallback* PxsDefaultMemoryManager::createDeviceMemoryAllocator(const PxU32 gpuComputeVersion)
+	{
+		PX_UNUSED(gpuComputeVersion);
+		return NULL;
+	}
+
+	void PxsDefaultMemoryManager::destroyMemoryAllocator()
+	{
+		for (PxU32 i = 0; i < mAllocators.size(); ++i)
+		{
+			mAllocators[i]->~VirtualAllocatorCallback();
+			PX_FREE(mAllocators[i]);
+		}
+	}
+
+
+	PxsMemoryManager* createMemoryManager()
+	{
+		return PX_PLACEMENT_NEW(PX_ALLOC(sizeof(PxsDefaultMemoryManager), PX_DEBUG_EXP("PxsDefaultMemoryManager")), PxsDefaultMemoryManager());
+	}
+
+}
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsIslandSim.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsIslandSim.cpp
new file mode 100644
index 00000000..867e291f
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsIslandSim.cpp
@@ -0,0 +1,2299 @@
+// 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 "PxsIslandSim.h"
+#include "PsSort.h"
+#include "PsUtilities.h"
+#include "foundation/PxProfiler.h"
+
+#define IG_SANITY_CHECKS 0
+
+namespace physx
+{
+namespace IG
+{
+
+	IslandSim::IslandSim(Ps::Array<PartitionEdge*>* firstPartitionEdges, Ps::Array<NodeIndex>& edgeNodeIndices, 
+		Ps::Array<PartitionEdge*>* destroyedPartitionEdges, PxU64 contextID) :
+		mNodes(PX_DEBUG_EXP("IslandSim::mNodes")),
+		mActiveNodeIndex(PX_DEBUG_EXP("IslandSim::mActiveNodeIndex")),
+		mEdges(PX_DEBUG_EXP("IslandSim::mEdges")),
+		mEdgeInstances(PX_DEBUG_EXP("IslandSim::mEdgeInstances")),
+		mIslands(PX_DEBUG_EXP("IslandSim::mIslands")),
+		mIslandStaticTouchCount(PX_DEBUG_EXP("IslandSim.activeStaticTouchCount")),
+		mActiveKinematicNodes(PX_DEBUG_EXP("IslandSim::mActiveKinematicNodes")),
+		//Ps::Array<EdgeIndex> mActiveEdges[Edge::eEDGE_TYPE_COUNT];	//! An array of active edges
+		mHopCounts(PX_DEBUG_EXP("IslandSim::mHopCounts")),
+		mFastRoute(PX_DEBUG_EXP("IslandSim::,FastRoute")),
+		mIslandIds(PX_DEBUG_EXP("IslandSim::mIslandIds")),
+		//mIslandAwake(PX_DEBUG_EXP("IslandSim::mIslandAwake")),
+		//mActiveContactEdges(PX_DEBUG_EXP("IslandSim::mActiveContactEdges")),
+		mActiveIslands(PX_DEBUG_EXP("IslandSim::mActiveIslands")),
+		mLastMapIndex(0),
+		mActivatingNodes(PX_DEBUG_EXP("IslandSim::mActivatingNodes")),
+		mDestroyedEdges(PX_DEBUG_EXP("IslandSim::mDestroyedEdges")),
+		mTempIslandIds(PX_DEBUG_EXP("IslandSim::mTempIslandIds")),
+		//mPriorityQueue(PX_DEBUG_EXP("IslandSim::mPriorityQueue")),
+		mVisitedNodes(PX_DEBUG_EXP("IslandSim::mVisitedNodes")),
+		//mVisitedState(PX_DEBUG_EXP("IslandSim::mVisitedState"))
+		mFirstPartitionEdges(firstPartitionEdges),
+		mEdgeNodeIndices(edgeNodeIndices),
+		mDestroyedPartitionEdges(destroyedPartitionEdges),
+		mContextId(contextID)
+	{
+		mInitialActiveNodeCount[0] = 0; mInitialActiveNodeCount[1] = 0; mNpIndexPtr = NULL; 
+		mActiveEdgeCount[0] = mActiveEdgeCount[1] = 0;
+	}
+
+template <typename Thing>
+static bool contains(Ps::Array<Thing>& arr, const Thing& thing)
+{
+	for(PxU32 a = 0; a < arr.size(); ++a)
+	{
+		if(thing == arr[a])
+			return true;
+	}
+	return false;
+}
+
+void IslandSim::resize(const PxU32 nbNodes, const PxU32 nbContactManagers, const PxU32 nbConstraints)
+{
+	PxU32 totalEdges = nbContactManagers + nbConstraints;
+	mNodes.reserve(nbNodes);
+	mIslandIds.reserve(nbNodes);
+	mEdges.reserve(totalEdges);
+	mActiveContactEdges.resize(totalEdges);
+	mEdgeInstances.reserve(totalEdges*2);
+}
+
+void IslandSim::addNode(bool isActive, bool isKinematic, Node::NodeType type, NodeIndex nodeIndex)
+{
+	PxU32 handle = nodeIndex.index();
+	if(handle == mNodes.capacity())
+	{
+		const PxU32 newCapacity = PxMax(2*mNodes.capacity(), 256u);
+		mNodes.reserve(newCapacity);
+		mIslandIds.reserve(newCapacity);
+		mFastRoute.reserve(newCapacity);
+		mHopCounts.reserve(newCapacity);
+		mActiveNodeIndex.reserve(newCapacity);
+	}
+
+	const PxU32 newSize = PxMax(handle+1, mNodes.size());
+	mNodes.resize(newSize);
+	mIslandIds.resize(newSize);
+	mFastRoute.resize(newSize);
+	mHopCounts.resize(newSize);
+	mActiveNodeIndex.resize(newSize);
+
+	mActiveNodeIndex[handle] = IG_INVALID_NODE;
+
+	
+	Node& node = mNodes[handle];
+	node.mType = Ps::to8(type);
+	//Ensure that the node is not currently being used.
+	PX_ASSERT(node.isDeleted());
+
+	PxU8 flags = PxU16(isActive ? 0 : Node::eREADY_FOR_SLEEPING);
+	if(isKinematic)
+		flags |= Node::eKINEMATIC;
+	node.mFlags = flags;
+	mIslandIds[handle] = IG_INVALID_ISLAND;
+	mFastRoute[handle].setIndices(IG_INVALID_NODE, 0);
+	mHopCounts[handle] = 0;
+
+	if(!isKinematic)
+	{
+		IslandId islandHandle = mIslandHandles.getHandle();
+		
+		if(islandHandle == mIslands.capacity())
+		{
+			const PxU32 newCapacity = PxMax(2*mIslands.capacity(), 256u);
+			mIslands.reserve(newCapacity);
+			mIslandAwake.resize(newCapacity);
+			mIslandStaticTouchCount.reserve(newCapacity);
+		}
+		mIslands.resize(PxMax(islandHandle+1, mIslands.size()));
+		mIslandStaticTouchCount.resize(PxMax(islandHandle+1, mIslands.size()));
+		mIslandAwake.growAndReset(PxMax(islandHandle+1, mIslands.size()));
+		Island& island = mIslands[islandHandle];
+		island.mLastNode = island.mRootNode = nodeIndex;
+		island.mSize[type] = 1;
+		mIslandIds[handle] = islandHandle;
+		mIslandStaticTouchCount[islandHandle] = 0;
+	}
+
+	if(isActive)
+	{
+		activateNode(nodeIndex);
+	}	
+}
+
+void IslandSim::addRigidBody(PxsRigidBody* body, bool isKinematic, bool isActive, NodeIndex nodeIndex)
+{
+	addNode(isActive, isKinematic, Node::eRIGID_BODY_TYPE, nodeIndex);
+	Node& node = mNodes[nodeIndex.index()];
+	node.mRigidBody = body;
+}
+
+void IslandSim::addArticulation(Sc::ArticulationSim* /*articulation*/, Dy::Articulation* llArtic, bool isActive, NodeIndex nodeIndex)
+{
+	addNode(isActive, false, Node::eARTICULATION_TYPE, nodeIndex);
+	Node& node = mNodes[nodeIndex.index()];
+	node.mLLArticulation = llArtic;
+}
+
+void IslandSim::connectEdge(EdgeInstance& instance, EdgeInstanceIndex edgeIndex, Node& source, NodeIndex /*destination*/)
+{
+	PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE);
+	PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE);
+
+	instance.mNextEdge = source.mFirstEdgeIndex;
+	if(source.mFirstEdgeIndex != IG_INVALID_EDGE)
+	{
+		EdgeInstance& firstEdge = mEdgeInstances[source.mFirstEdgeIndex];
+		firstEdge.mPrevEdge = edgeIndex;
+	}
+
+	source.mFirstEdgeIndex = edgeIndex;
+	instance.mPrevEdge = IG_INVALID_EDGE;
+}
+
+void IslandSim::addConnection(NodeIndex nodeHandle1, NodeIndex nodeHandle2, Edge::EdgeType edgeType, EdgeIndex handle)
+{	
+	PX_UNUSED(nodeHandle1);
+	PX_UNUSED(nodeHandle2);
+	if(handle >= mEdges.capacity())
+	{
+		const PxU32 newSize = PxMax(2*(handle+1), 256u);
+		mEdges.reserve(newSize);
+		mActiveContactEdges.resize(newSize);
+	}
+	mEdges.resize(PxMax(mEdges.size(), handle+1));
+	mActiveContactEdges.reset(handle);
+
+	Edge& edge = mEdges[handle];
+
+	if(edge.isPendingDestroyed())
+	{
+		//If it's in this state, then the edge has been tagged for destruction but actually is now not needed to be destroyed
+		edge.clearPendingDestroyed();
+		return;
+	}
+
+	if(edge.isInDirtyList())
+	{
+		PX_ASSERT(mEdgeNodeIndices[handle * 2].index() == nodeHandle1.index());
+		PX_ASSERT(mEdgeNodeIndices[handle * 2 + 1].index() == nodeHandle2.index());
+		PX_ASSERT(edge.mEdgeType == edgeType);
+		return;
+	}
+
+	PX_ASSERT(!edge.isInserted());
+
+	PX_ASSERT(edge.isDestroyed());
+	edge.clearDestroyed();
+
+	PX_ASSERT(edge.mNextIslandEdge == IG_INVALID_ISLAND);
+	PX_ASSERT(edge.mPrevIslandEdge == IG_INVALID_ISLAND);
+
+	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle].mNextEdge == IG_INVALID_EDGE);
+	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle+1].mNextEdge == IG_INVALID_EDGE);
+	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle].mPrevEdge == IG_INVALID_EDGE);
+	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle+1].mPrevEdge == IG_INVALID_EDGE);
+
+	edge.mEdgeType = edgeType;
+
+	PX_ASSERT(handle*2 >= mEdgeInstances.size() || mEdgeInstances[handle*2].mNextEdge == IG_INVALID_EDGE);
+	PX_ASSERT(handle*2+1 >= mEdgeInstances.size() || mEdgeInstances[handle*2+1].mNextEdge == IG_INVALID_EDGE);
+	PX_ASSERT(handle*2 >= mEdgeInstances.size() || mEdgeInstances[handle*2].mPrevEdge == IG_INVALID_EDGE);
+	PX_ASSERT(handle*2+1 >= mEdgeInstances.size() || mEdgeInstances[handle*2+1].mPrevEdge == IG_INVALID_EDGE);
+	
+	//Add the new handle
+	if(!edge.isInDirtyList())
+	{
+		PX_ASSERT(!contains(mDirtyEdges[edgeType], handle));
+		mDirtyEdges[edgeType].pushBack(handle);
+		edge.markInDirtyList();
+	}
+	edge.mEdgeState &= ~(Edge::eACTIVATING);
+}
+
+void IslandSim::addConnectionToGraph(EdgeIndex handle)
+{
+	EdgeInstanceIndex instanceHandle = 2*handle;
+	PX_ASSERT(instanceHandle < mEdgeInstances.capacity());
+	/*if(instanceHandle == mEdgeInstances.capacity())
+	{
+		mEdgeInstances.reserve(2*mEdgeInstances.capacity() + 2);
+	}*/
+	mEdgeInstances.resize(PxMax(instanceHandle+2, mEdgeInstances.size()));
+
+	Edge& edge = mEdges[handle];
+	
+	bool activeEdge = false;
+	bool kinematicKinematicEdge = true;
+
+	NodeIndex nodeIndex1 = mEdgeNodeIndices[instanceHandle];
+	NodeIndex nodeIndex2 = mEdgeNodeIndices[instanceHandle+1];
+
+	if(nodeIndex1.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex1.index()];
+		connectEdge(mEdgeInstances[instanceHandle], instanceHandle, node, nodeIndex2);
+		activeEdge = node.isActive() || node.isActivating();
+		kinematicKinematicEdge = node.isKinematic();
+	}
+
+	if (nodeIndex1.index() != nodeIndex2.index() && nodeIndex2.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex2.index()];
+		connectEdge(mEdgeInstances[instanceHandle + 1], instanceHandle + 1, node, nodeIndex1);
+		activeEdge = activeEdge || node.isActive() || node.isActivating();
+		kinematicKinematicEdge = kinematicKinematicEdge && node.isKinematic();
+	}
+
+	if(activeEdge && (!kinematicKinematicEdge || edge.getEdgeType() == IG::Edge::eCONTACT_MANAGER))
+	{				
+		markEdgeActive(handle);
+		edge.activateEdge();
+	}
+}
+
+void IslandSim::removeConnectionFromGraph(EdgeIndex edgeIndex)
+{
+	NodeIndex nodeIndex1 = mEdgeNodeIndices[2 * edgeIndex];
+	NodeIndex nodeIndex2 = mEdgeNodeIndices[2 * edgeIndex+1];
+	if (nodeIndex1.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex1.index()];
+		if (nodeIndex2.index() == mFastRoute[nodeIndex1.index()].index())
+			mFastRoute[nodeIndex1.index()].setIndices(IG_INVALID_NODE, 0);
+		if(!node.isDirty())
+		{
+			//mDirtyNodes.pushBack(nodeIndex1);
+			mDirtyMap.growAndSet(nodeIndex1.index());
+			node.markDirty();
+		}
+	}
+
+	if (nodeIndex2.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex2.index()];
+		if (nodeIndex1.index() == mFastRoute[nodeIndex2.index()].index())
+			mFastRoute[nodeIndex2.index()].setIndices(IG_INVALID_NODE, 0);
+		if(!node.isDirty())
+		{
+			mDirtyMap.growAndSet(nodeIndex2.index());
+			node.markDirty();
+		}
+	}
+}
+
+void IslandSim::disconnectEdge(EdgeInstance& instance, EdgeInstanceIndex edgeIndex, Node& node)
+{
+
+	PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE || mEdgeInstances[instance.mNextEdge].mPrevEdge == edgeIndex);
+	PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE || mEdgeInstances[instance.mPrevEdge].mNextEdge == edgeIndex);
+
+	if(node.mFirstEdgeIndex == edgeIndex)
+	{
+		PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE);
+		node.mFirstEdgeIndex = instance.mNextEdge;
+	}
+	else
+	{
+		EdgeInstance& prev = mEdgeInstances[instance.mPrevEdge];
+		PX_ASSERT(prev.mNextEdge == edgeIndex);
+		prev.mNextEdge = instance.mNextEdge;
+	}
+
+	if(instance.mNextEdge != IG_INVALID_EDGE)
+	{
+		EdgeInstance& next = mEdgeInstances[instance.mNextEdge];
+		PX_ASSERT(next.mPrevEdge == edgeIndex);
+		next.mPrevEdge = instance.mPrevEdge;
+	}
+
+	PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE || mEdgeInstances[instance.mNextEdge].mPrevEdge == instance.mPrevEdge);
+	PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE || mEdgeInstances[instance.mPrevEdge].mNextEdge == instance.mNextEdge);
+
+	instance.mNextEdge = IG_INVALID_EDGE;
+	instance.mPrevEdge = IG_INVALID_EDGE;
+}
+
+void IslandSim::removeConnection(EdgeIndex edgeIndex)
+{
+	Edge& edge = mEdges[edgeIndex];
+	if(!edge.isPendingDestroyed())// && edge.isInserted())
+	{
+		mDestroyedEdges.pushBack(edgeIndex);
+		/*if(!edge.isInserted())
+			edge.setReportOnlyDestroy();*/
+	}
+	edge.setPendingDestroyed();
+}
+
+void IslandSim::removeConnectionInternal(EdgeIndex edgeIndex)
+{
+	PX_ASSERT(edgeIndex != IG_INVALID_EDGE);
+	EdgeInstanceIndex edgeInstanceBase = edgeIndex*2;
+
+
+	NodeIndex nodeIndex1 = mEdgeNodeIndices[edgeIndex * 2];
+
+	if (nodeIndex1.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex1.index()];
+		disconnectEdge(mEdgeInstances[edgeInstanceBase], edgeInstanceBase, node);
+	}
+
+	NodeIndex nodeIndex2 = mEdgeNodeIndices[edgeIndex * 2 + 1];
+
+	if (nodeIndex2.index() != IG_INVALID_NODE && nodeIndex1.index() != nodeIndex2.index())
+	{
+		Node& node = mNodes[nodeIndex2.index()];
+		disconnectEdge(mEdgeInstances[edgeInstanceBase+1], edgeInstanceBase+1, node);
+	}
+}
+
+
+void IslandSim::addContactManager(PxsContactManager* /*manager*/, NodeIndex nodeHandle1, NodeIndex nodeHandle2, EdgeIndex handle)
+{
+	addConnection(nodeHandle1, nodeHandle2, Edge::eCONTACT_MANAGER, handle);
+}
+
+void IslandSim::addConstraint(Dy::Constraint* /*constraint*/, NodeIndex nodeHandle1, NodeIndex nodeHandle2, EdgeIndex handle)
+{
+	addConnection(nodeHandle1, nodeHandle2, Edge::eCONSTRAINT, handle);
+}
+
+void IslandSim::activateNode(NodeIndex nodeIndex)
+{
+	if(nodeIndex.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex.index()];
+
+		if(!(node.isActive() || 
+			node.isActivating()))
+		{
+
+			//If the node is kinematic and already in the active node list, then we need to remove it
+			//from the active kinematic node list, then re-add it after the wake-up. It's a bit stupid
+			//but it means that we don't need another index
+
+			if(node.isKinematic() && mActiveNodeIndex[nodeIndex.index()] != IG_INVALID_NODE)
+			{
+				//node.setActive();
+				//node.clearIsReadyForSleeping(); //Clear the "isReadyForSleeping" flag. Just in case it was set
+				//return;
+
+				PxU32 activeRefCount = node.mActiveRefCount;
+				node.mActiveRefCount = 0;
+				node.clearActive();
+				markKinematicInactive(nodeIndex);
+				node.mActiveRefCount = activeRefCount;
+			}
+			
+			node.setActivating(); //Tag it as activating
+			PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == IG_INVALID_NODE);
+			mActiveNodeIndex[nodeIndex.index()] = mActivatingNodes.size();	
+			//Add to waking list
+			mActivatingNodes.pushBack(nodeIndex);
+		}
+		node.clearIsReadyForSleeping(); //Clear the "isReadyForSleeping" flag. Just in case it was set
+		node.clearDeactivating();
+	}
+}
+
+void IslandSim::deactivateNode(NodeIndex nodeIndex)
+{
+	if(nodeIndex.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[nodeIndex.index()];
+
+		//If the node is activating, clear its activating state and remove it from the activating list. 
+		//If it wasn't already activating, then it's probably already in the active list
+
+		bool wasActivating = node.isActivating();
+
+		if(wasActivating)
+		{
+			//Already activating, so remove it from the activating list
+			node.clearActivating();
+			PX_ASSERT(mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]].index() == nodeIndex.index());
+			NodeIndex replaceIndex = mActivatingNodes[mActivatingNodes.size()-1];
+			mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[nodeIndex.index()];
+			mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]] = replaceIndex;
+			mActivatingNodes.forceSize_Unsafe(mActivatingNodes.size()-1);
+			mActiveNodeIndex[nodeIndex.index()] = IG_INVALID_NODE;
+
+			if(node.isKinematic())
+			{
+				//If we were temporarily removed from the active kinematic list to be put in the waking kinematic list
+				//then add the node back in before deactivating the node. This is a bit counter-intuitive but the active
+				//kinematic list contains all active kinematics and all kinematics that are referenced by an active constraint
+				PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == IG_INVALID_NODE);
+				mActiveNodeIndex[nodeIndex.index()] = mActiveKinematicNodes.size();
+				mActiveKinematicNodes.pushBack(nodeIndex);
+			}
+		}
+
+		//Raise the "ready for sleeping" flag so that island gen can put this node to sleep
+		node.setIsReadyForSleeping();
+	}
+}
+
+void IslandSim::putNodeToSleep(NodeIndex nodeIndex)
+{
+	if(nodeIndex.index() != IG_INVALID_NODE)
+	{
+		deactivateNode(nodeIndex);
+	}
+}
+
+
+void IslandSim::activateNodeInternal(NodeIndex nodeIndex)
+{
+	//This method should activate the node, then activate all the connections involving this node
+	Node& node = mNodes[nodeIndex.index()];
+
+	if(!node.isActive())
+	{
+		PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == IG_INVALID_NODE);
+
+		//Activate all the edges + nodes...
+
+		EdgeInstanceIndex index = node.mFirstEdgeIndex;
+
+		while(index != IG_INVALID_EDGE)
+		{
+			EdgeIndex idx = index/2;
+			Edge& edge = mEdges[idx]; //InstanceIndex/2 = edgeIndex
+			if(!edge.isActive())
+			{
+				//Make the edge active...
+				PX_ASSERT(mEdgeNodeIndices[idx * 2].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2].index()].isKinematic());
+				PX_ASSERT(mEdgeNodeIndices[idx * 2 + 1].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isKinematic());
+
+				markEdgeActive(idx);
+				edge.activateEdge();
+
+			}
+			index = mEdgeInstances[index].mNextEdge;
+		}
+
+		if(node.isKinematic())
+		{
+			markKinematicActive(nodeIndex);
+		}
+		else
+		{
+			markActive(nodeIndex);
+		}
+		node.setActive();
+	}
+
+}
+
+void IslandSim::deactivateNodeInternal(NodeIndex nodeIndex)
+{
+	//We deactivate a node, we need to loop through all the edges and deactivate them *if* both bodies are asleep
+
+	Node& node = mNodes[nodeIndex.index()];
+
+	if(node.isActive())
+	{
+		if(node.isKinematic())
+		{
+			markKinematicInactive(nodeIndex);
+		}
+		else
+		{
+			markInactive(nodeIndex);
+		}
+
+		//Clear the active status flag
+		node.clearActive();
+		node.clearActivating();
+
+		EdgeInstanceIndex index = node.mFirstEdgeIndex;
+
+		while(index != IG_INVALID_EDGE)
+		{
+			EdgeInstance& instance = mEdgeInstances[index];
+		
+
+			NodeIndex outboundNode = mEdgeNodeIndices[index ^ 1];
+			if(outboundNode.index() == IG_INVALID_NODE || 
+				!mNodes[outboundNode.index()].isActive())
+			{
+				EdgeIndex idx = index/2;
+				Edge& edge = mEdges[idx]; //InstanceIndex/2 = edgeIndex
+				//PX_ASSERT(edge.isActive()); //The edge must currently be inactive because the node was active
+				//Deactivate the edge if both nodes connected are inactive OR if one node is static/kinematic and the other is inactive...
+				PX_ASSERT(mEdgeNodeIndices[index & (~1)].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[index & (~1)].index()].isActive());
+				PX_ASSERT(mEdgeNodeIndices[index | 1].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[index | 1].index()].isActive());
+				if(edge.isActive())
+				{
+					edge.deactivateEdge();
+					mActiveEdgeCount[edge.mEdgeType]--;
+					removeEdgeFromActivatingList(idx);
+					mDeactivatingEdges[edge.mEdgeType].pushBack(idx);
+				}
+			}
+			index = instance.mNextEdge;
+		}		
+	}
+
+}
+
+bool IslandSim::canFindRoot(NodeIndex startNode, NodeIndex targetNode, Ps::Array<NodeIndex>* visitedNodes)
+{
+	if(visitedNodes)
+		visitedNodes->pushBack(startNode);
+	if(startNode.index() == targetNode.index())
+		return true;
+	Cm::BitMap visitedState;
+	visitedState.resizeAndClear(mNodes.size());
+
+	Ps::Array<NodeIndex> stack;
+
+	stack.pushBack(startNode);
+
+	visitedState.set(startNode.index());
+
+	do
+	{
+		NodeIndex currentIndex = stack.popBack();
+		Node& currentNode = mNodes[currentIndex.index()];
+
+		EdgeInstanceIndex currentEdge = currentNode.mFirstEdgeIndex;
+
+		while(currentEdge != IG_INVALID_EDGE)
+		{
+			EdgeInstance& edge = mEdgeInstances[currentEdge];
+			NodeIndex outboundNode = mEdgeNodeIndices[currentEdge ^ 1];
+			if(outboundNode.index() != IG_INVALID_NODE && !mNodes[outboundNode.index()].isKinematic() && !visitedState.test(outboundNode.index()))
+			{
+				if(outboundNode.index() == targetNode.index())
+				{
+					return true;
+				}
+
+				visitedState.set(outboundNode.index());
+				stack.pushBack(outboundNode);
+				if(visitedNodes)
+					visitedNodes->pushBack(outboundNode);
+			}
+
+			currentEdge = edge.mNextEdge;
+		}
+
+	}
+	while(stack.size());
+
+	return false;
+
+}
+
+
+
+void IslandSim::unwindRoute(PxU32 traversalIndex, NodeIndex lastNode, PxU32 hopCount, IslandId id)
+{
+	//We have found either a witness *or* the root node with this traversal. In the event of finding the root node, hopCount will be 0. In the event of finding
+	//a witness, hopCount will be the hopCount that witness reported as being the distance to the root.
+
+	PxU32 currIndex = traversalIndex;
+	PxU32 hc = hopCount+1; //Add on 1 for the hop to the witness/root node.
+	do
+	{
+		TraversalState& state = mVisitedNodes[currIndex];
+		mHopCounts[state.mNodeIndex.index()] = hc++;
+		mIslandIds[state.mNodeIndex.index()] = id;
+		mFastRoute[state.mNodeIndex.index()] = lastNode;
+		currIndex = state.mPrevIndex;
+		lastNode = state.mNodeIndex;
+	}
+	while(currIndex != IG_INVALID_NODE);
+}
+
+void IslandSim::activateIsland(IslandId islandId)
+{
+	Island& island = mIslands[islandId];
+	PX_ASSERT(!mIslandAwake.test(islandId));
+	PX_ASSERT(island.mActiveIndex == IG_INVALID_ISLAND);
+	
+	NodeIndex currentNode = island.mRootNode;
+	while(currentNode.index() != IG_INVALID_NODE)
+	{
+		activateNodeInternal(currentNode);
+		currentNode = mNodes[currentNode.index()].mNextNode;
+	}
+	markIslandActive(islandId);
+}
+
+void IslandSim::deactivateIsland(IslandId islandId)
+{
+	PX_ASSERT(mIslandAwake.test(islandId));
+	Island& island = mIslands[islandId];
+	
+	NodeIndex currentNode = island.mRootNode;
+	while(currentNode.index() != IG_INVALID_NODE)
+	{
+		Node& node = mNodes[currentNode.index()];
+		//if(mActiveNodeIndex[currentNode.index()] < mInitialActiveNodeCount[node.mType])
+			mNodesToPutToSleep[node.mType].pushBack(currentNode); //If this node was previously active, then push it to the list of nodes to deactivate
+		deactivateNodeInternal(currentNode);
+		currentNode = node.mNextNode;
+	}
+	markIslandInactive(islandId);
+}
+
+
+void IslandSim::wakeIslands()
+{
+	PX_PROFILE_ZONE("Basic.wakeIslands", getContextId());
+
+	//(1) Iterate over activating nodes and activate them
+
+
+	PxU32 originalActiveIslands = mActiveIslands.size();
+
+	for (PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
+	{
+		for (PxU32 i = 0, count = mActivatedEdges[a].size(); i < count; ++i)
+		{
+			IG::Edge& edge = mEdges[mActivatedEdges[a][i]];
+			edge.mEdgeState &= (~Edge::eACTIVATING);
+		}
+
+	}
+
+	mActivatedEdges[0].forceSize_Unsafe(0);
+	mActivatedEdges[1].forceSize_Unsafe(0);
+	/*mInitialActiveEdgeCount[0] = mActiveEdges[0].size();
+	mInitialActiveEdgeCount[1] = mActiveEdges[1].size();*/
+
+	for(PxU32 a = 0; a < mActivatingNodes.size(); ++a)
+	{
+		NodeIndex wakeNode = mActivatingNodes[a];
+
+		IslandId islandId = mIslandIds[wakeNode.index()];
+
+		Node& node = mNodes[wakeNode.index()];
+		node.clearActivating();
+		if(islandId != IG_INVALID_ISLAND)
+		{
+			if(!mIslandAwake.test(islandId))
+			{
+				markIslandActive(islandId);						
+			}
+			mActiveNodeIndex[wakeNode.index()] = IG_INVALID_NODE; //Mark active node as invalid.
+			activateNodeInternal(wakeNode);
+		}
+		else
+		{
+			PX_ASSERT(node.isKinematic());
+			node.setActive();
+			PX_ASSERT(mActiveNodeIndex[wakeNode.index()] == a);
+			mActiveNodeIndex[wakeNode.index()] = mActiveKinematicNodes.size();
+			mActiveKinematicNodes.pushBack(wakeNode);
+
+			//Wake up the islands connected to this waking kinematic!
+			EdgeInstanceIndex index = node.mFirstEdgeIndex;
+			while(index != IG_INVALID_EDGE)
+			{
+				EdgeInstance& edgeInstance = mEdgeInstances[index];
+
+				NodeIndex outboundNode = mEdgeNodeIndices[index ^ 1];
+				//Edge& edge = mEdges[index/2];
+				//if(edge.isConnected()) //Only wake up if the edge is not connected...
+				NodeIndex nodeIndex = outboundNode;
+
+				if (nodeIndex.isStaticBody() || mIslandIds[nodeIndex.index()] == IG_INVALID_ISLAND)
+				{
+					//If the edge connects to a static body *or* it connects to a node which is not part of an island (i.e. a kinematic), then activate the edge
+					EdgeIndex idx = index / 2;
+					Edge& edge = mEdges[idx];
+					if (!edge.isActive() && edge.getEdgeType() != IG::Edge::eCONSTRAINT)
+					{
+						//Make the edge active...
+						PX_ASSERT(mEdgeNodeIndices[idx * 2].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2].index()].isKinematic());
+						PX_ASSERT(mEdgeNodeIndices[idx * 2 + 1].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isKinematic());
+
+						markEdgeActive(idx);
+						edge.activateEdge();
+
+					}
+				}
+				else
+				{
+					IslandId connectedIslandId = mIslandIds[nodeIndex.index()];
+					if(!mIslandAwake.test(connectedIslandId))
+					{
+						//Wake up that island
+						markIslandActive(connectedIslandId);
+					}
+				}
+
+				index = edgeInstance.mNextEdge;
+			}
+		}
+	}
+
+	mInitialActiveNodeCount[0] = mActiveNodes[0].size();
+	mInitialActiveNodeCount[1] = mActiveNodes[1].size();
+
+	mActivatingNodes.forceSize_Unsafe(0);
+
+	for(PxU32 a = originalActiveIslands; a < mActiveIslands.size(); ++a)
+	{
+		Island& island = mIslands[mActiveIslands[a]];
+	
+		NodeIndex currentNode = island.mRootNode;
+		while(currentNode.index() != IG_INVALID_NODE)
+		{
+			activateNodeInternal(currentNode);
+			currentNode = mNodes[currentNode.index()].mNextNode;
+		}
+	}
+}
+
+void IslandSim::wakeIslands2()
+{
+	PxU32 originalActiveIslands = mActiveIslands.size();
+
+	for (PxU32 a = 0; a < mActivatingNodes.size(); ++a)
+	{
+		NodeIndex wakeNode = mActivatingNodes[a];
+
+		IslandId islandId = mIslandIds[wakeNode.index()];
+
+		Node& node = mNodes[wakeNode.index()];
+		node.clearActivating();
+		if (islandId != IG_INVALID_ISLAND)
+		{
+			if (!mIslandAwake.test(islandId))
+			{
+				markIslandActive(islandId);
+			}
+			mActiveNodeIndex[wakeNode.index()] = IG_INVALID_NODE; //Mark active node as invalid.
+			activateNodeInternal(wakeNode);
+		}
+		else
+		{
+			PX_ASSERT(node.isKinematic());
+			node.setActive();
+			PX_ASSERT(mActiveNodeIndex[wakeNode.index()] == a);
+			mActiveNodeIndex[wakeNode.index()] = mActiveKinematicNodes.size();
+			mActiveKinematicNodes.pushBack(wakeNode);
+
+			//Wake up the islands connected to this waking kinematic!
+			EdgeInstanceIndex index = node.mFirstEdgeIndex;
+			while (index != IG_INVALID_EDGE)
+			{
+				EdgeInstance& edgeInstance = mEdgeInstances[index];
+
+				NodeIndex outboundNode = mEdgeNodeIndices[index ^ 1];
+				//Edge& edge = mEdges[index/2];
+				//if(edge.isConnected()) //Only wake up if the edge is not connected...
+				NodeIndex nodeIndex = outboundNode;
+
+				if (nodeIndex.isStaticBody() || mIslandIds[nodeIndex.index()] == IG_INVALID_ISLAND)
+				{
+					//If the edge connects to a static body *or* it connects to a node which is not part of an island (i.e. a kinematic), then activate the edge
+					EdgeIndex idx = index / 2;
+					Edge& edge = mEdges[idx];
+					if (!edge.isActive() && edge.getEdgeType() != IG::Edge::eCONSTRAINT)
+					{
+						//Make the edge active...
+						PX_ASSERT(mEdgeNodeIndices[idx * 2].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2].index()].isKinematic());
+						PX_ASSERT(mEdgeNodeIndices[idx * 2 + 1].index() == IG_INVALID_NODE || !mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isActive() || mNodes[mEdgeNodeIndices[idx * 2 + 1].index()].isKinematic());
+
+						markEdgeActive(idx);
+						edge.activateEdge();
+
+					}
+				}
+				else
+				{
+					IslandId connectedIslandId = mIslandIds[nodeIndex.index()];
+					if (!mIslandAwake.test(connectedIslandId))
+					{
+						//Wake up that island
+						markIslandActive(connectedIslandId);
+					}
+				}
+
+				index = edgeInstance.mNextEdge;
+			}
+		}
+	}
+
+	mActivatingNodes.forceSize_Unsafe(0);
+
+	for (PxU32 a = originalActiveIslands; a < mActiveIslands.size(); ++a)
+	{
+		Island& island = mIslands[mActiveIslands[a]];
+
+		NodeIndex currentNode = island.mRootNode;
+		while (currentNode.index() != IG_INVALID_NODE)
+		{
+			activateNodeInternal(currentNode);
+			currentNode = mNodes[currentNode.index()].mNextNode;
+		}
+	}
+}
+
+void IslandSim::insertNewEdges()
+{
+	PX_PROFILE_ZONE("Basic.insertNewEdges", getContextId());
+
+	mEdgeInstances.reserve(mEdges.capacity()*2);
+	
+	for(PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
+	{
+		for(PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
+		{
+			EdgeIndex edgeIndex = mDirtyEdges[i][a];
+
+			Edge& edge = mEdges[edgeIndex];
+
+			if(!edge.isPendingDestroyed())
+			{
+				//PX_ASSERT(!edge.isInserted());
+				if(!edge.isInserted())
+				{
+					addConnectionToGraph(edgeIndex);
+					edge.setInserted();
+				}
+			}
+		}
+	}
+}
+
+void IslandSim::removeDestroyedEdges()
+{
+	PX_PROFILE_ZONE("Basic.removeDestroyedEdges", getContextId());
+
+	for(PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
+	{
+		EdgeIndex edgeIndex = mDestroyedEdges[a];
+
+		Edge& edge = mEdges[edgeIndex];
+		
+		if(edge.isPendingDestroyed())
+		{
+			if(!edge.isInDirtyList() && edge.isInserted())
+			{
+				PX_ASSERT(edge.isInserted());
+				removeConnectionInternal(edgeIndex);
+				removeConnectionFromGraph(edgeIndex);
+				//edge.clearInserted();
+			}
+			//edge.clearDestroyed();
+		}
+	}		
+}
+
+void IslandSim::processNewEdges()
+{
+	PX_PROFILE_ZONE("Basic.processNewEdges", getContextId());
+	//Stage 1: we process the list of new pairs. To do this, we need to first sort them based on a predicate...
+
+	insertNewEdges();
+
+	mHopCounts.resize(mNodes.size()); //Make sure we have enough space for hop counts for all nodes
+	mFastRoute.resize(mNodes.size());
+
+
+	for(PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
+	{
+		for(PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
+		{
+			EdgeIndex edgeIndex = mDirtyEdges[i][a];
+			Edge& edge = mEdges[edgeIndex];
+			
+			/*PX_ASSERT(edge.mState != Edge::eDESTROYED || ((edge.mNode1.index() == IG_INVALID_NODE || mNodes[edge.mNode1.index()].isKinematic() || mNodes[edge.mNode1.index()].isActive() == false) &&
+				(edge.mNode2.index() == IG_INVALID_NODE || mNodes[edge.mNode2.index()].isKinematic() || mNodes[edge.mNode2.index()].isActive() == false)));*/
+
+			//edge.clearInDirtyList();
+			
+
+			//We do not process either destroyed or disconnected edges
+			if(/*edge.isConnected() && */!edge.isPendingDestroyed())
+			{
+				//Conditions:
+				//(1)	Neither body is in an island (static/kinematics are never in islands) so we need to create a new island containing these bodies 
+				//		or just 1 body if the other is kinematic/static
+				//(2)	Both bodies are already in the same island. Update root node hop count estimates for the bodies if a route through the new connection
+				//		is shorter for either body
+				//(3)	One body is already in an island and the other isn't, so we just add the new body to the existing island.
+				//(4)	Both bodies are in different islands. In that case, we merge the islands
+
+				NodeIndex nodeIndex1 = mEdgeNodeIndices[2 * edgeIndex];
+				NodeIndex nodeIndex2 = mEdgeNodeIndices[2 * edgeIndex+1];
+
+				IslandId islandId1 = nodeIndex1.index() == IG_INVALID_NODE ? IG_INVALID_ISLAND : mIslandIds[nodeIndex1.index()];
+				IslandId islandId2 = nodeIndex2.index() == IG_INVALID_NODE ? IG_INVALID_ISLAND : mIslandIds[nodeIndex2.index()];
+
+				//TODO - wake ups!!!!
+				//If one of the nodes is awake and the other is asleep, we need to wake 'em up
+
+				//When a node is activated, the island must also be activated...
+
+				bool active1 = nodeIndex1.index() != IG_INVALID_NODE && mNodes[nodeIndex1.index()].isActive();
+				bool active2 = nodeIndex2.index() != IG_INVALID_NODE && mNodes[nodeIndex2.index()].isActive();
+
+				IslandId islandId = IG_INVALID_ISLAND;
+
+				if(islandId1 == IG_INVALID_ISLAND && islandId2 == IG_INVALID_ISLAND)
+				{
+					//All nodes should be introduced in an island now unless they are static or kinematic. Therefore, if we get here, we have an edge
+					//between 2 kinematic nodes or a kinematic and static node. These should not influence island management so we should just ignore
+					//these edges.
+				}
+				else if(islandId1 == islandId2)
+				{
+					islandId = islandId1;
+					if(active1 || active2)
+					{
+						PX_ASSERT(mIslandAwake.test(islandId1)); //If we got here, where the 2 were already in an island, if 1 node is awake, the whole island must be awake
+					}
+					//Both bodies in the same island. Nothing major to do already but we should see if this creates a shorter path to root for either node
+					PxU32 hopCount1 = mHopCounts[nodeIndex1.index()];
+					PxU32 hopCount2 = mHopCounts[nodeIndex2.index()];
+					if((hopCount1+1) < hopCount2)
+					{
+						//It would be faster for node 2 to go through node 1
+						mHopCounts[nodeIndex2.index()] = hopCount1 + 1;
+						mFastRoute[nodeIndex2.index()] = nodeIndex1;
+					}
+					else if((hopCount2+1) < hopCount1)
+					{
+						//It would be faster for node 1 to go through node 2
+						mHopCounts[nodeIndex1.index()] = hopCount2 + 1;
+						mFastRoute[nodeIndex1.index()] = nodeIndex2;
+					}
+
+					//No need to activate/deactivate the island. Its state won't have changed
+
+				}
+				else if(islandId1 == IG_INVALID_ISLAND)
+				{
+					islandId = islandId2;
+					if (nodeIndex1.index() != IG_INVALID_NODE)
+					{
+						if (!mNodes[nodeIndex1.index()].isKinematic())
+						{
+							PX_ASSERT(islandId2 != IG_INVALID_ISLAND);
+							//We need to add node 1 to island2
+							PX_ASSERT(mNodes[nodeIndex1.index()].mNextNode.index() == IG_INVALID_NODE); //Ensure that this node is not in any other island
+							PX_ASSERT(mNodes[nodeIndex1.index()].mPrevNode.index() == IG_INVALID_NODE); //Ensure that this node is not in any other island
+							
+							Island& island = mIslands[islandId2];
+
+							Node& lastNode = mNodes[island.mLastNode.index()];
+
+							PX_ASSERT(lastNode.mNextNode.index() == IG_INVALID_NODE);
+
+							Node& node = mNodes[nodeIndex1.index()];
+							lastNode.mNextNode = nodeIndex1;
+							node.mPrevNode = island.mLastNode;
+							island.mLastNode = nodeIndex1;
+							island.mSize[node.mType]++;
+							mIslandIds[nodeIndex1.index()] = islandId2;
+							mHopCounts[nodeIndex1.index()] = mHopCounts[nodeIndex2.index()] + 1;
+							mFastRoute[nodeIndex1.index()] = nodeIndex2;
+
+							if(active1 || active2)
+							{
+								if(!mIslandAwake.test(islandId2))
+								{
+									//This island wasn't already awake, so need to wake the whole island up
+									activateIsland(islandId2);
+								}
+								if(!active1)
+								{
+									//Wake up this node...
+									activateNodeInternal(nodeIndex1);
+								}
+							}
+						}
+						else if(active1 && !active2)
+						{
+							//Active kinematic object -> wake island!
+							activateIsland(islandId2);
+						}
+					}
+					else
+					{
+						//A new touch with a static body...
+						Node& node = mNodes[nodeIndex2.index()];
+						node.mStaticTouchCount++; //Increment static touch counter on the body
+						//Island& island = mIslands[islandId2];
+						//island.mStaticTouchCount++; //Increment static touch counter on the island
+						mIslandStaticTouchCount[islandId2]++;
+
+					}
+				}
+				else if (islandId2 == IG_INVALID_ISLAND)
+				{
+					islandId = islandId1;
+					if (nodeIndex2.index() != IG_INVALID_NODE)
+					{ 
+						if (!mNodes[nodeIndex2.index()].isKinematic())
+						{
+							PX_ASSERT(islandId1 != IG_INVALID_NODE);
+							//We need to add node 1 to island2
+							PX_ASSERT(mNodes[nodeIndex2.index()].mNextNode.index() == IG_INVALID_NODE); //Ensure that this node is not in any other island
+							PX_ASSERT(mNodes[nodeIndex2.index()].mPrevNode.index() == IG_INVALID_NODE); //Ensure that this node is not in any other island
+							
+							Island& island = mIslands[islandId1];
+
+							Node& lastNode = mNodes[island.mLastNode.index()];
+							PX_ASSERT(lastNode.mNextNode.index() == IG_INVALID_NODE);
+							Node& node = mNodes[nodeIndex2.index()];
+							lastNode.mNextNode = nodeIndex2;
+							node.mPrevNode = island.mLastNode;
+							island.mLastNode = nodeIndex2;
+							island.mSize[node.mType]++;
+							mIslandIds[nodeIndex2.index()] = islandId1;
+							mHopCounts[nodeIndex2.index()] = mHopCounts[nodeIndex1.index()] + 1;
+							mFastRoute[nodeIndex2.index()] = nodeIndex1;
+
+							if(active1 || active2)
+							{
+								if(!mIslandAwake.test(islandId1))
+								{
+									//This island wasn't already awake, so need to wake the whole island up
+									activateIsland(islandId1);
+								}
+								if(!active1)
+								{
+									//Wake up this node...
+									activateNodeInternal(nodeIndex2);
+								}
+							}
+						}
+						else if(active2 && !active1)
+						{
+							//Active kinematic object -> wake island!
+							activateIsland(islandId1);
+						}
+					}
+					else
+					{
+						//New static touch 
+						//A new touch with a static body...
+						Node& node = mNodes[nodeIndex1.index()];
+						node.mStaticTouchCount++; //Increment static touch counter on the body
+						//Island& island = mIslands[islandId1];
+						mIslandStaticTouchCount[islandId1]++;
+						//island.mStaticTouchCount++; //Increment static touch counter on the island
+					}
+
+				}
+				else
+				{
+					PX_ASSERT(islandId1 != islandId2);
+					PX_ASSERT(islandId1 != IG_INVALID_ISLAND && islandId2 != IG_INVALID_ISLAND);
+
+					if(active1 || active2)
+					{
+						//One of the 2 islands was awake, so need to wake the other one! We do this now, before we merge the islands, to ensure that all 
+						//the bodies are activated
+						if(!mIslandAwake.test(islandId1))
+						{
+							//This island wasn't already awake, so need to wake the whole island up
+							activateIsland(islandId1);
+						}
+						if(!mIslandAwake.test(islandId2))
+						{
+							//This island wasn't already awake, so need to wake the whole island up
+							activateIsland(islandId2);
+						}
+					}
+
+					//OK. We need to merge these islands together...
+					islandId = mergeIslands(islandId1, islandId2, nodeIndex1, nodeIndex2);
+				}
+
+				if(islandId != IG_INVALID_ISLAND)
+				{
+					//Add new edge to existing island
+					Island& island = mIslands[islandId];
+					addEdgeToIsland(island, edgeIndex);
+				}
+			}
+		}
+
+	}
+
+}
+
+bool IslandSim::isPathTo(NodeIndex startNode, NodeIndex targetNode)
+{
+	Node& node = mNodes[startNode.index()];
+
+	EdgeInstanceIndex index = node.mFirstEdgeIndex;
+	while(index != IG_INVALID_EDGE)
+	{
+		EdgeInstance& instance = mEdgeInstances[index];
+		if(/*mEdges[index/2].isConnected() &&*/ mEdgeNodeIndices[index^1].index() == targetNode.index())
+			return true;
+		index = instance.mNextEdge;
+	}
+	return false;
+}
+
+bool IslandSim::tryFastPath(NodeIndex startNode, NodeIndex targetNode, IslandId islandId)
+{
+	PX_UNUSED(startNode);
+	PX_UNUSED(targetNode);
+
+	NodeIndex currentNode = startNode;
+
+	PxU32 currentVisitedNodes = mVisitedNodes.size();
+
+	PxU32 depth = 0;
+	
+	bool found = false;
+	do
+	{
+		//Get the fast path from this node...
+		
+		if(mVisitedState.test(currentNode.index()))
+		{
+			found = mIslandIds[currentNode.index()] != IG_INVALID_ISLAND; //Already visited and not tagged with invalid island == a witness!
+			break;
+		}
+		if( currentNode.index() == targetNode.index())
+		{
+			found = true;
+			break;
+		}
+
+		mVisitedNodes.pushBack(TraversalState(currentNode, mVisitedNodes.size(), mVisitedNodes.size()-1, depth++));
+
+		PX_ASSERT(mFastRoute[currentNode.index()].index() == IG_INVALID_NODE || isPathTo(currentNode, mFastRoute[currentNode.index()]));
+
+		mIslandIds[currentNode.index()] = IG_INVALID_ISLAND;
+		mVisitedState.set(currentNode.index());
+
+		currentNode = mFastRoute[currentNode.index()];
+	}
+	while(currentNode.index() != IG_INVALID_NODE);
+
+	for(PxU32 a = currentVisitedNodes; a < mVisitedNodes.size(); ++a)
+	{
+		TraversalState& state = mVisitedNodes[a];
+		mIslandIds[state.mNodeIndex.index()] = islandId;
+	}
+
+	if(!found)
+	{
+		for(PxU32 a = currentVisitedNodes; a < mVisitedNodes.size(); ++a)
+		{
+			TraversalState& state = mVisitedNodes[a];
+			mVisitedState.reset(state.mNodeIndex.index());
+		}
+
+		mVisitedNodes.forceSize_Unsafe(currentVisitedNodes);
+	}
+	return found;
+
+}
+
+bool IslandSim::findRoute(NodeIndex startNode, NodeIndex targetNode, IslandId islandId)
+{
+
+	//Firstly, traverse the fast path and tag up witnesses. TryFastPath can fail. In that case, no witnesses are left but this node is permitted to report
+	//that it is still part of the island. Whichever node lost its fast path will be tagged as dirty and will be responsible for recovering the fast path
+	//and tagging up the visited nodes
+	if(mFastRoute[startNode.index()].index() != IG_INVALID_NODE)
+	{
+		if(tryFastPath(startNode, targetNode, islandId))
+			return true;
+
+		//Try fast path can either be successful or not. If it was successful, then we had a valid fast path cached and all nodes on that fast path were tagged
+		//as witness nodes (visited and with a valid island ID). If the fast path was not successful, then no nodes were tagged as witnesses. 
+		//Technically, we need to find a route to the root node but, as an optimization, we can simply return true from here with no witnesses added. 
+		//Whichever node actually broke the "fast path" will also be on the list of dirty nodes and will be processed later. 
+		//If that broken edge triggered an island separation, this node will be re-visited and added to that island, otherwise
+		//the path to the root node will be re-established. The end result is the same - the island state is computed - this just saves us some work.
+		//return true;
+	}
+
+	{
+		//If we got here, there was no fast path. Therefore, we need to fall back on searching for the root node. This is optimized by using "hop counts".
+		//These are per-node counts that indicate the expected number of hops from this node to the root node. These are lazily evaluated and updated
+		//as new edges are formed or when traversals occur to re-establish islands. As a result, they may be inaccurate but they still serve the purpose
+		//of guiding our search to minimize the chances of us doing an exhaustive search to find the root node.
+		mIslandIds[startNode.index()] = IG_INVALID_ISLAND;
+		TraversalState* startTraversal = &mVisitedNodes.pushBack(TraversalState(startNode, mVisitedNodes.size(), IG_INVALID_NODE, 0));
+		mVisitedState.set(startNode.index());
+		QueueElement element(startTraversal, mHopCounts[startNode.index()]);
+		mPriorityQueue.push(element);
+
+		do
+		{
+			QueueElement currentQE = mPriorityQueue.pop();
+
+			TraversalState& currentState = *currentQE.mState;
+
+			Node& currentNode = mNodes[currentState.mNodeIndex.index()];
+
+			EdgeInstanceIndex edge = currentNode.mFirstEdgeIndex;
+
+			while(edge != IG_INVALID_EDGE)
+			{
+				EdgeInstance& instance = mEdgeInstances[edge];
+				{
+					NodeIndex nextIndex = mEdgeNodeIndices[edge ^ 1];
+
+					//Static or kinematic nodes don't connect islands.
+					if(nextIndex.index() != IG_INVALID_NODE && !mNodes[nextIndex.index()].isKinematic())
+					{
+						if(nextIndex.index() == targetNode.index())
+						{
+							unwindRoute(currentState.mCurrentIndex, nextIndex, 0, islandId);
+							return true;
+						}
+
+						if(mVisitedState.test(nextIndex.index()))
+						{
+							//We already visited this node. This means that it's either in the priority queue already or we 
+							//visited in on a previous pass. If it was visited on a previous pass, then it already knows what island it's in. 
+							//We now need to test the island id to find out if this node knows the root.
+							//If it has a valid root id, that id *is* our new root. We can guesstimate our hop count based on the node's properties
+							
+							IslandId visitedIslandId = mIslandIds[nextIndex.index()];
+							if(visitedIslandId != IG_INVALID_ISLAND)
+							{
+								//If we get here, we must have found a node that knows a route to our root node. It must not be a different island
+								//because that would caused me to have been visited already because totally separate islands trigger a full traversal on 
+								//the orphaned side.
+								PX_ASSERT(visitedIslandId == islandId);
+								unwindRoute(currentState.mCurrentIndex, nextIndex, mHopCounts[nextIndex.index()], islandId);
+								return true;
+							}
+						}
+						else
+						{
+							//This node has not been visited yet, so we need to push it into the stack and continue traversing
+							TraversalState* state = &mVisitedNodes.pushBack(TraversalState(nextIndex, mVisitedNodes.size(), currentState.mCurrentIndex, currentState.mDepth+1));
+							QueueElement qe(state, mHopCounts[nextIndex.index()]);
+							mPriorityQueue.push(qe);
+							mVisitedState.set(nextIndex.index());
+							PX_ASSERT(mIslandIds[nextIndex.index()] == islandId);
+							mIslandIds[nextIndex.index()] = IG_INVALID_ISLAND; //Flag as invalid island until we know whether we can find root or an island id.
+						}
+					}
+				}
+
+				edge = instance.mNextEdge;
+			}
+		}
+		while(mPriorityQueue.size());
+
+		return false;
+	}
+}
+
+
+void IslandSim::processLostEdges(Ps::Array<NodeIndex>& destroyedNodes, bool allowDeactivation, bool permitKinematicDeactivation,
+	PxU32 dirtyNodeLimit)
+{
+	PX_PROFILE_ZONE("Basic.processLostEdges", getContextId());
+	//At this point, all nodes and edges are activated. 
+
+	//Bit map for visited
+	mVisitedState.resizeAndClear(mNodes.size());
+	
+	//Reserve space on priority queue for at least 1024 nodes. It will resize if more memory is required during traversal.
+	mPriorityQueue.reserve(1024);
+
+	mIslandSplitEdges[0].reserve(1024);
+	mIslandSplitEdges[1].reserve(1024);
+
+	mVisitedNodes.reserve(mNodes.size()); //Make sure we have enough space for all nodes!
+
+	const PxU32 nbDestroyedEdges = mDestroyedEdges.size();
+	PX_UNUSED(nbDestroyedEdges);
+	{
+		PX_PROFILE_ZONE("Basic.removeEdgesFromIslands", getContextId());
+		for(PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
+		{
+			EdgeIndex lostIndex = mDestroyedEdges[a];
+			Edge& lostEdge = mEdges[lostIndex];
+
+			if(lostEdge.isPendingDestroyed() && !lostEdge.isInDirtyList() )
+			{
+				//Process this edge...
+				if(!lostEdge.isReportOnlyDestroy() && lostEdge.isInserted())
+				{
+					PxU32 index1 = mEdgeNodeIndices[mDestroyedEdges[a] * 2].index();
+					PxU32 index2 = mEdgeNodeIndices[mDestroyedEdges[a] * 2 + 1].index();
+			
+					IslandId islandId = IG_INVALID_ISLAND;
+					if(index1 != IG_INVALID_NODE && index2 != IG_INVALID_NODE)
+					{
+						PX_ASSERT(mIslandIds[index1] == IG_INVALID_ISLAND || mIslandIds[index2] == IG_INVALID_ISLAND || 
+							mIslandIds[index1] == mIslandIds[index2]);
+						islandId = mIslandIds[index1] != IG_INVALID_ISLAND ? mIslandIds[index1] : mIslandIds[index2];
+					}
+					else if(index1 != IG_INVALID_NODE)
+					{
+						PX_ASSERT(index2 == IG_INVALID_NODE);
+						Node& node = mNodes[index1];
+						if(!node.isKinematic())
+						{
+							islandId = mIslandIds[index1];
+							node.mStaticTouchCount--;
+							//Island& island = mIslands[islandId];
+							mIslandStaticTouchCount[islandId]--;
+							//island.mStaticTouchCount--;
+						}
+					}
+					else if(index2 != IG_INVALID_NODE)
+					{
+						PX_ASSERT(index1 == IG_INVALID_NODE);
+						Node& node = mNodes[index2];
+						if(!node.isKinematic())
+						{
+							islandId = mIslandIds[index2];
+							node.mStaticTouchCount--;
+							//Island& island = mIslands[islandId];
+							mIslandStaticTouchCount[islandId]--;
+							//island.mStaticTouchCount--;
+						}
+					}
+
+					if(islandId != IG_INVALID_ISLAND)
+					{
+						//We need to remove this edge from the island
+						Island& island = mIslands[islandId];
+						removeEdgeFromIsland(island, lostIndex);
+					}
+				}
+
+				lostEdge.clearInserted();
+
+			}
+		}
+	}
+
+	if (allowDeactivation)
+	{
+		PX_PROFILE_ZONE("Basic.findPathsAndBreakIslands", getContextId());
+		Cm::BitMap::CircularIterator iter(mDirtyMap, mLastMapIndex);
+
+		//KS - process only this many dirty nodes, deferring future dirty nodes to subsequent frames. 
+		//This means that it may take several frames for broken edges to trigger islands to completely break but this is better
+		//than triggering large performance spikes.
+		const PxU32 MaxCount = dirtyNodeLimit;
+
+		PxU32 count = 0;
+		PxU32 dirtyIdx;
+		while ((dirtyIdx = iter.getNext()) != Cm::BitMap::CircularIterator::DONE && (count++ < MaxCount))
+		{
+			mLastMapIndex = dirtyIdx + 1;
+			//Process dirty nodes. Figure out if we can make our way from the dirty node to the root.
+
+			mPriorityQueue.clear(); //Clear the queue used for traversal
+			mVisitedNodes.forceSize_Unsafe(0); //Clear the list of nodes in this island
+			NodeIndex dirtyNodeIndex(dirtyIdx);
+			Node& dirtyNode = mNodes[dirtyNodeIndex.index()];
+		
+			//Check whether this node has already been touched. If it has been touched this frame, then its island state is reliable 
+			//and we can just unclear the dirty flag on the body. If we were already visited, then the state should have already been confirmed in a 
+			//previous pass.
+			if(!dirtyNode.isKinematic() && !dirtyNode.isDeleted() && !mVisitedState.test(dirtyNodeIndex.index()))
+			{
+				//We haven't visited this node in our island repair passes yet, so we still need to process until we've hit a visited node or found
+				//our root node. Note that, as soon as we hit a visited node that has already been processed in a previous pass, we know that we can rely
+				//on its island information although the hop counts may not be optimal. It also indicates that this island was not broken immediately because
+				//otherwise, the entire new sub-island would already have been visited and this node would have already had its new island state assigned.
+
+				//Indicate that I've been visited
+
+				IslandId islandId = mIslandIds[dirtyNodeIndex.index()];
+				Island& findIsland = mIslands[islandId];
+
+				NodeIndex searchNode = findIsland.mRootNode;//The node that we're searching for!
+
+				if(searchNode.index() != dirtyNodeIndex.index()) //If we are the root node, we don't need to do anything!
+				{
+					if(findRoute(dirtyNodeIndex, searchNode, islandId))
+					{
+						//We found the root node so let's let every visited node know that we found its root
+						//and we can also update our hop counts because we recorded how many hops it took to reach this
+						//node
+
+						//We already filled in the path to the root/witness with accurate hop counts. Now we just need to fill in the estimates
+						//for the remaining nodes and re-define their islandIds. We approximate their path to the root by just routing them through
+						//the route we already found.
+
+						//This loop works because mVisitedNodes are recorded in the order they were visited and we already filled in the critical path
+						//so the remainder of the paths will just fork from that path.
+
+						//Verify state (that we can see the root from this node)...
+
+	#if IG_SANITY_CHECKS
+						PX_ASSERT(canFindRoot(dirtyNode, searchNode, NULL)); //Verify that we found the connection
+	#endif
+
+						for(PxU32 b = 0; b < mVisitedNodes.size(); ++b)
+						{
+							TraversalState& state = mVisitedNodes[b];
+							if(mIslandIds[state.mNodeIndex.index()] == IG_INVALID_ISLAND)
+							{
+								mHopCounts[state.mNodeIndex.index()] = mHopCounts[mVisitedNodes[state.mPrevIndex].mNodeIndex.index()]+1; 
+								mFastRoute[state.mNodeIndex.index()] = mVisitedNodes[state.mPrevIndex].mNodeIndex;
+								mIslandIds[state.mNodeIndex.index()] = islandId;
+							}
+						}
+					}
+					else
+					{
+						//If I traversed and could not find the root node, then I have established a new island. In this island, I am the root node
+						//and I will point all my nodes towards me. Furthermore, I have established how many steps it took to reach all nodes in my island
+
+						//OK. We need to separate the islands. We have a list of nodes that are part of the new island (mVisitedNodes) and we know that the 
+						//first node in that list is the root node.
+
+
+						//OK, we need to remove all these actors from their current island, then add them to the new island...
+
+						Island& oldIsland = mIslands[islandId];
+						//We can just unpick these nodes from the island because they do not contain the root node (if they did, then we wouldn't be
+						//removing this node from the island at all). The only challenge is if we need to remove the last node. In that case
+						//we need to re-establish the new last node in the island but perhaps the simplest way to do that would be to traverse
+						//the island to establish the last node again
+
+	#if IG_SANITY_CHECKS
+						PX_ASSERT(!canFindRoot(dirtyNode, searchNode, NULL));
+	#endif
+
+						PxU32 totalStaticTouchCount = 0;
+						mIslandSplitEdges[0].forceSize_Unsafe(0);
+						mIslandSplitEdges[1].forceSize_Unsafe(0);
+						PxU32 size[2] = {0,0};
+
+						//NodeIndex lastIndex = oldIsland.mLastNode;
+
+						//size[node.mType] = 1;
+
+						for(PxU32 a = 0; a < mVisitedNodes.size(); ++a)
+						{
+							NodeIndex index = mVisitedNodes[a].mNodeIndex;
+							Node& node = mNodes[index.index()];
+
+							if(node.mNextNode.index() != IG_INVALID_NODE)
+								mNodes[node.mNextNode.index()].mPrevNode = node.mPrevNode;
+							else
+								oldIsland.mLastNode = node.mPrevNode;
+							if(node.mPrevNode.index() != IG_INVALID_NODE)
+								mNodes[node.mPrevNode.index()].mNextNode = node.mNextNode;
+
+							size[node.mType]++;
+
+							node.mNextNode.setIndices(IG_INVALID_NODE, 0);
+							node.mPrevNode.setIndices(IG_INVALID_NODE, 0);
+
+							PX_ASSERT(mNodes[oldIsland.mLastNode.index()].mNextNode.index() == IG_INVALID_NODE);
+
+							totalStaticTouchCount += node.mStaticTouchCount;
+
+							EdgeInstanceIndex idx = node.mFirstEdgeIndex;
+
+							while(idx != IG_INVALID_EDGE)
+							{
+								EdgeInstance& instance = mEdgeInstances[idx];
+								const EdgeIndex edgeIndex = idx/2;
+								Edge& edge = mEdges[edgeIndex];
+								
+								//Only split the island if we're processing the first node or if the first node is infinte-mass
+								if (!(idx & 1) || (mEdgeNodeIndices[idx & (~1)].index() == IG_INVALID_NODE || mNodes[mEdgeNodeIndices[idx & (~1)].index()].isKinematic()))
+								{
+									//We will remove this edge from the island...
+									mIslandSplitEdges[edge.mEdgeType].pushBack(edgeIndex);
+
+									removeEdgeFromIsland(oldIsland, edgeIndex);	
+
+								}
+								idx = instance.mNextEdge;
+							}
+
+						}
+
+						//oldIsland.mStaticTouchCount -= totalStaticTouchCount;
+						mIslandStaticTouchCount[islandId] -= totalStaticTouchCount;
+
+						oldIsland.mSize[0] -= size[0];
+						oldIsland.mSize[1] -= size[1];
+
+						//Now add all these nodes to the new island
+
+						//(1) Create the new island...
+						IslandId newIslandHandle = mIslandHandles.getHandle();
+						/*if(newIslandHandle == mIslands.capacity())
+						{
+							mIslands.reserve(2*mIslands.capacity() + 1);
+						}*/
+						mIslands.resize(PxMax(newIslandHandle+1, mIslands.size()));
+						mIslandStaticTouchCount.resize(PxMax(newIslandHandle+1, mIslandStaticTouchCount.size()));
+						Island& newIsland = mIslands[newIslandHandle];
+					
+						if(mIslandAwake.test(islandId))
+						{
+							newIsland.mActiveIndex = mActiveIslands.size();
+							mActiveIslands.pushBack(newIslandHandle);
+							mIslandAwake.growAndSet(newIslandHandle); //Separated island, so it should be awake
+						}
+						else
+						{
+							mIslandAwake.growAndReset(newIslandHandle);
+						}
+
+						newIsland.mRootNode = dirtyNodeIndex;
+						mHopCounts[dirtyNodeIndex.index()] = 0;
+						mIslandIds[dirtyNodeIndex.index()] = newIslandHandle;
+						//newIsland.mTotalSize = mVisitedNodes.size();
+
+						mNodes[dirtyNodeIndex.index()].mPrevNode.setIndices(IG_INVALID_NODE, 0); //First node so doesn't have a preceding node
+						mFastRoute[dirtyNodeIndex.index()].setIndices(IG_INVALID_NODE, 0);
+
+						size[0] = 0; size[1] = 0;
+
+						size[dirtyNode.mType] = 1;
+					
+						for(PxU32 a = 1; a < mVisitedNodes.size(); ++a)
+						{
+							NodeIndex index = mVisitedNodes[a].mNodeIndex;
+							Node& thisNode = mNodes[index.index()];
+							NodeIndex prevNodeIndex = mVisitedNodes[a-1].mNodeIndex;
+							thisNode.mPrevNode = prevNodeIndex;
+							mNodes[prevNodeIndex.index()].mNextNode = index;
+							size[thisNode.mType]++;
+							mIslandIds[index.index()] = newIslandHandle;
+							mHopCounts[index.index()] = mVisitedNodes[a].mDepth; //How many hops to root
+							mFastRoute[index.index()] = mVisitedNodes[mVisitedNodes[a].mPrevIndex].mNodeIndex;
+						}
+
+						newIsland.mSize[0] = size[0];
+						newIsland.mSize[1] = size[1];
+						//Last node in the island
+						NodeIndex lastIndex = mVisitedNodes[mVisitedNodes.size()-1].mNodeIndex;
+						mNodes[lastIndex.index()].mNextNode.setIndices(IG_INVALID_NODE, 0);
+						newIsland.mLastNode = lastIndex;
+						//newIsland.mStaticTouchCount = totalStaticTouchCount;
+						mIslandStaticTouchCount[newIslandHandle] = totalStaticTouchCount;
+						newIsland.mSize[0] = size[0];
+						newIsland.mSize[1] = size[1];
+
+						PX_ASSERT(mNodes[newIsland.mLastNode.index()].mNextNode.index() == IG_INVALID_NODE);
+
+						for(PxU32 j = 0; j < 2; ++j)
+						{
+							Ps::Array<EdgeIndex>& splitEdges = mIslandSplitEdges[j];
+							const PxU32 splitEdgeSize = splitEdges.size();
+							if(splitEdgeSize)
+							{
+								splitEdges.pushBack(IG_INVALID_EDGE); //Push in a dummy invalid edge to complete the connectivity
+								mEdges[splitEdges[0]].mNextIslandEdge = splitEdges[1];
+								for(PxU32 a = 1; a < splitEdgeSize; ++a)
+								{
+									EdgeIndex edgeIndex = splitEdges[a];
+									Edge& edge = mEdges[edgeIndex];
+									edge.mNextIslandEdge = splitEdges[a+1];
+									edge.mPrevIslandEdge = splitEdges[a-1];
+								}
+
+								newIsland.mFirstEdge[j] = splitEdges[0];
+								newIsland.mLastEdge[j] = splitEdges[splitEdgeSize-1];
+								newIsland.mEdgeCount[j] = splitEdgeSize;
+							}
+						}
+
+					}
+				}
+
+			}
+
+			dirtyNode.clearDirty();
+			mDirtyMap.reset(dirtyIdx);
+		}
+
+		if (count < MaxCount)
+			mLastMapIndex = 0;
+		//mDirtyNodes.forceSize_Unsafe(0);
+	}
+
+
+	{
+		PX_PROFILE_ZONE("Basic.clearDestroyedEdges", getContextId());
+		//Now process the lost edges...
+		for (PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
+		{
+			//Process these destroyed edges. Recompute island information. Update the islands and hop counters accordingly
+			EdgeIndex index = mDestroyedEdges[a];
+
+			Edge& edge = mEdges[index];
+			if (edge.isPendingDestroyed())
+			{
+				//if(edge.mFirstPartitionEdge)
+				PartitionEdge* pEdge = mFirstPartitionEdges ? (*mFirstPartitionEdges)[index] : NULL;
+				if (pEdge)
+				{
+					mDestroyedPartitionEdges->pushBack(pEdge);
+					(*mFirstPartitionEdges)[index] = NULL; //Force first partition edge to NULL to ensure we don't have a clash
+				}
+				if (edge.isActive())
+				{
+					removeEdgeFromActivatingList(index); //TODO - can we remove this call? Can we handle this elsewhere, e.g. when destroying the nodes...
+					mActiveEdgeCount[edge.mEdgeType]--;
+				}
+
+				edge = Edge(); //Reset edge
+				mActiveContactEdges.growAndReset(index);
+			}
+		}
+
+		mDestroyedEdges.forceSize_Unsafe(0);
+	}
+
+	{
+		PX_PROFILE_ZONE("Basic.clearDestroyedNodes", getContextId());
+
+		for(PxU32 a = 0; a < destroyedNodes.size(); ++a)
+		{
+			NodeIndex nodeIndex = destroyedNodes[a];
+			IslandId islandId = mIslandIds[nodeIndex.index()];
+			Node& node = mNodes[nodeIndex.index()];
+			if(islandId != IG_INVALID_ISLAND)
+			{
+				Island& island = mIslands[islandId];
+
+				removeNodeFromIsland(island, nodeIndex);
+
+				mIslandIds[nodeIndex.index()] = IG_INVALID_ISLAND;
+
+				if((island.mSize[0] + island.mSize[1]) == 0)
+				{
+					mIslandHandles.freeHandle(islandId);
+					if(island.mActiveIndex != IG_INVALID_ISLAND)
+					{
+						IslandId replaceId = mActiveIslands[mActiveIslands.size()-1];
+						Island& replaceIsland = mIslands[replaceId];
+						replaceIsland.mActiveIndex = island.mActiveIndex;
+						mActiveIslands[island.mActiveIndex] = replaceId;
+						mActiveIslands.forceSize_Unsafe(mActiveIslands.size()-1);
+						island.mActiveIndex = IG_INVALID_ISLAND;
+						//island.mStaticTouchCount -= node.mStaticTouchCount; //Remove the static touch count from the island
+						mIslandStaticTouchCount[islandId] -= node.mStaticTouchCount;
+					}
+					mIslandAwake.reset(islandId);
+					island.mLastNode.setIndices(IG_INVALID_NODE, 0);
+					island.mRootNode.setIndices(IG_INVALID_NODE, 0);
+					island.mActiveIndex = IG_INVALID_ISLAND;
+				}
+			}
+		
+			if(node.isKinematic())
+			{
+				if(mActiveNodeIndex[nodeIndex.index()] != IG_INVALID_NODE)
+				{
+					//Remove from the active kinematics list...
+					markKinematicInactive(nodeIndex);
+				}
+			}
+			else
+			{
+				if(mActiveNodeIndex[nodeIndex.index()] != IG_INVALID_NODE)
+				{
+					markInactive(nodeIndex);
+				}
+			}
+
+			node.reset();
+		}
+	}
+	//Now we need to produce the list of active edges and nodes!!!
+
+	//If we get here, we have a list of active islands. From this, we need to iterate over all active islands and establish if that island
+	//can, in fact, go to sleep. In order to become deactivated, all nodes in the island must be ready for sleeping...
+
+	if(allowDeactivation)
+	{
+		PX_PROFILE_ZONE("Basic.deactivation", getContextId());
+		for(PxU32 a = 0; a < mActiveIslands.size(); a++)
+		{
+			IslandId islandId = mActiveIslands[a];
+
+			mIslandAwake.reset(islandId);
+		}
+
+		//Loop over the active kinematic nodes and tag all islands touched by active kinematics as awake
+		for(PxU32 a = mActiveKinematicNodes.size(); a > 0; --a)
+		{
+			NodeIndex kinematicIndex = mActiveKinematicNodes[a-1];
+
+			Node& kinematicNode = mNodes[kinematicIndex.index()];
+
+			if(kinematicNode.isReadyForSleeping())
+			{
+				if(permitKinematicDeactivation)
+				{
+					kinematicNode.clearActive();
+					markKinematicInactive(kinematicIndex);
+				}
+			}
+			else //if(!kinematicNode.isReadyForSleeping())
+			{
+				//KS - if kinematic is active, then wake up all islands the kinematic is touching
+				EdgeInstanceIndex edgeId = kinematicNode.mFirstEdgeIndex;
+				while(edgeId != IG_INVALID_EDGE)
+				{
+					EdgeInstance& instance = mEdgeInstances[edgeId];
+					//Edge& edge = mEdges[edgeId/2];
+					//Only wake up islands if a connection was present
+					//if(edge.isConnected())
+					{
+						NodeIndex outNode = mEdgeNodeIndices[edgeId^1];
+						if(outNode.index() != IG_INVALID_NODE)
+						{
+							IslandId islandId = mIslandIds[outNode.index()];
+							if(islandId != IG_INVALID_ISLAND)
+							{
+								mIslandAwake.set(islandId);
+								PX_ASSERT(mIslands[islandId].mActiveIndex != IG_INVALID_ISLAND);
+							}
+						}
+					}
+					edgeId = instance.mNextEdge;
+				}
+			}
+		}
+
+		for(PxU32 a = mActiveIslands.size(); a > 0; --a)
+		{
+			IslandId islandId = mActiveIslands[a-1];
+
+			Island& island = mIslands[islandId];				
+
+			bool canDeactivate = !mIslandAwake.test(islandId);
+			mIslandAwake.set(islandId);			
+
+			//If it was touched by an active kinematic in the above loop, we can't deactivate it.
+			//Therefore, no point in testing the nodes in the island. They must remain awake
+			if(canDeactivate) 
+			{
+				NodeIndex nodeId = island.mRootNode;
+				while(nodeId.index() != IG_INVALID_NODE)
+				{
+					Node& node = mNodes[nodeId.index()];
+					if(!node.isReadyForSleeping())
+					{
+						canDeactivate = false;
+						break;
+					}
+					nodeId = node.mNextNode;
+				}
+				if(canDeactivate)
+				{
+					//If all nodes in this island are ready for sleeping and there were no active 
+					//kinematics interacting with the any bodies in the island, we can deactivate the island.
+					deactivateIsland(islandId);
+				}
+			}
+		}
+	}
+
+	{
+		PX_PROFILE_ZONE("Basic.resetDirtyEdges", getContextId());
+		for (PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
+		{
+			for (PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
+			{
+				Edge& edge = mEdges[mDirtyEdges[i][a]];
+				edge.clearInDirtyList();
+			}
+			mDirtyEdges[i].clear(); //All new edges processed
+		}
+	}
+
+}
+
+
+IslandId IslandSim::mergeIslands(IslandId island0, IslandId island1, NodeIndex node0, NodeIndex node1)
+{
+	Island& is0 = mIslands[island0];
+	Island& is1 = mIslands[island1];
+
+	//We defer this process and do it later instead. That way, if we have some pathalogical 
+	//case where multiple islands get merged repeatedly, we don't end up repeatedly remapping all the nodes in those islands 
+	//to their new island. Instead, we just choose the largest island and remap the smaller island to that.
+
+	PxU32 totalSize0 = is0.mSize[0] + is0.mSize[1];
+	PxU32 totalSize1 = is1.mSize[0] + is1.mSize[1];
+	if(totalSize0 > totalSize1)
+	{
+		mergeIslandsInternal(is0, is1, island0, island1, node0, node1);
+		mIslandAwake.reset(island1);
+		mIslandHandles.freeHandle(island1);
+		mFastRoute[node1.index()] = node0;
+		return island0;
+	}
+	else
+	{
+		mergeIslandsInternal(is1, is0, island1, island0, node1, node0);
+		mIslandAwake.reset(island0);
+		mIslandHandles.freeHandle(island0);
+		mFastRoute[node0.index()] = node1;
+		return island1;
+	}
+}
+
+bool IslandSim::checkInternalConsistency()
+{
+	//Loop over islands, confirming that the island data is consistent...
+	//Really expensive. Turn off unless investigating some random issue...
+#if 0
+	for (PxU32 a = 0; a < mIslands.size(); ++a)
+	{
+		Island& island = mIslands[a];
+
+		PxU32 expectedSize = island.mSize[0] + island.mSize[1];
+		bool metLastNode = expectedSize == 0;
+
+		NodeIndex nodeId = island.mRootNode;
+
+		while (nodeId.index() != IG_INVALID_NODE)
+		{
+
+			PX_ASSERT(mIslandIds[nodeId.index()] == a);
+
+			if (nodeId.index() == island.mLastNode.index())
+			{
+				metLastNode = true;
+				PX_ASSERT(mNodes[nodeId.index()].mNextNode.index() == IG_INVALID_NODE);
+			}
+
+			--expectedSize;
+
+			nodeId = mNodes[nodeId.index()].mNextNode;
+		}
+
+		PX_ASSERT(expectedSize == 0);
+		PX_ASSERT(metLastNode);
+	}
+#endif
+
+	return true;
+
+}
+
+void IslandSim::mergeIslandsInternal(Island& island0, Island& island1, IslandId islandId0, IslandId islandId1, NodeIndex nodeIndex0, NodeIndex nodeIndex1)
+{
+	PX_ASSERT((island0.mSize[0] + island0.mSize[1]) >= (island1.mSize[0] + island1.mSize[1])); //We only ever merge the smaller island to the larger island
+	//Stage 1 - we need to move all the nodes across to the new island ID (i.e. write all their new island indices, move them to the 
+	//island and then also update their estimated hop counts to the root. As we don't want to do a full traversal at this point,
+	//instead, we estimate based on the route from the node to their previous root and then from that root to the new connection
+	//between the 2 islands. This is probably a very indirect route but it will be refined later.
+
+	//In this case, island1 is subsumed by island0
+
+	//It takes mHopCounts[nodeIndex1] to get from node1 to its old root. It takes mHopCounts[nodeIndex0] to get from nodeIndex0 to the new root
+	//and it takes 1 extra hop to go from node1 to node0. Therefore, a sub-optimal route can be planned going via the old root node that should take
+	//mHopCounts[nodeIndex0] + mHopCounts[nodeIndex1] + 1 + mHopCounts[nodeIndex] to travel from any arbitrary node (nodeIndex) in island1 to the root
+	//of island2.
+
+
+	PxU32 extraPath = mHopCounts[nodeIndex0.index()] + mHopCounts[nodeIndex1.index()] + 1;
+
+	NodeIndex islandNode = island1.mRootNode;
+	while(islandNode.index() != IG_INVALID_NODE)
+	{
+		mHopCounts[islandNode.index()] += extraPath;
+		mIslandIds[islandNode.index()] = islandId0;
+
+		//mFastRoute[islandNode] = IG_INVALID_NODE;
+		
+		Node& node = mNodes[islandNode.index()];
+		islandNode = node.mNextNode;				
+	}
+
+	//Now fill in the hop count for node1, which is directly connected to node0.
+	mHopCounts[nodeIndex1.index()] = mHopCounts[nodeIndex0.index()] + 1;
+	Node& lastNode = mNodes[island0.mLastNode.index()];
+	Node& firstNode = mNodes[island1.mRootNode.index()];
+	PX_ASSERT(lastNode.mNextNode.index() == IG_INVALID_NODE);
+	PX_ASSERT(firstNode.mPrevNode.index() == IG_INVALID_NODE);
+	PX_ASSERT(island1.mRootNode.index() != island0.mLastNode.index());
+
+	PX_ASSERT(mNodes[island0.mLastNode.index()].mNextNode.index() == IG_INVALID_NODE);
+	PX_ASSERT(mNodes[island1.mLastNode.index()].mNextNode.index() == IG_INVALID_NODE);
+
+	PX_ASSERT(mIslandIds[island0.mLastNode.index()] == islandId0);
+
+
+
+	lastNode.mNextNode = island1.mRootNode;
+	firstNode.mPrevNode = island0.mLastNode;
+
+	
+
+	island0.mLastNode = island1.mLastNode;
+	island0.mSize[0] += island1.mSize[0];
+	island0.mSize[1] += island1.mSize[1];
+	//island0.mStaticTouchCount += island1.mStaticTouchCount;
+	mIslandStaticTouchCount[islandId0] += mIslandStaticTouchCount[islandId1];
+
+	//Merge the edge list for the islands...
+	for(PxU32 a = 0; a < 2; ++a)
+	{
+		if(island0.mLastEdge[a] != IG_INVALID_EDGE)
+		{
+			PX_ASSERT(mEdges[island0.mLastEdge[a]].mNextIslandEdge == IG_INVALID_EDGE);
+			mEdges[island0.mLastEdge[a]].mNextIslandEdge = island1.mFirstEdge[a];
+		}
+		else
+		{
+			PX_ASSERT(island0.mFirstEdge[a] == IG_INVALID_EDGE);
+			island0.mFirstEdge[a] = island1.mFirstEdge[a];
+		}
+		if(island1.mFirstEdge[a] != IG_INVALID_EDGE)
+		{
+			PX_ASSERT(mEdges[island1.mFirstEdge[a]].mPrevIslandEdge == IG_INVALID_EDGE);
+			mEdges[island1.mFirstEdge[a]].mPrevIslandEdge = island0.mLastEdge[a];
+			island0.mLastEdge[a] = island1.mLastEdge[a];
+		}
+
+		island0.mEdgeCount[a] += island1.mEdgeCount[a];
+		island1.mFirstEdge[a] = IG_INVALID_EDGE;
+		island1.mLastEdge[a] = IG_INVALID_EDGE;
+		island1.mEdgeCount[a] = 0;
+	}
+
+
+	island1.mLastNode.setIndices(IG_INVALID_NODE, 0);
+	island1.mRootNode.setIndices(IG_INVALID_NODE, 0);
+	island1.mSize[0] = 0;
+	island1.mSize[1] = 0;
+	mIslandStaticTouchCount[islandId1] = 0;
+	//island1.mStaticTouchCount = 0;
+	
+	//Remove from active island list
+	if(island1.mActiveIndex != IG_INVALID_ISLAND)
+	{
+		markIslandInactive(islandId1);
+	}
+	
+}
+
+void IslandSim::removeEdgeFromActivatingList(EdgeIndex index)
+{
+	Edge& edge = mEdges[index];
+
+	if (edge.mEdgeState & Edge::eACTIVATING)
+	{
+		for (PxU32 a = 0, count = mActivatedEdges[edge.mEdgeType].size(); a < count; a++)
+		{
+			if (mActivatedEdges[edge.mEdgeType][a] == index)
+			{
+				mActivatedEdges[edge.mEdgeType].replaceWithLast(a);
+				break;
+			}
+		}
+
+		edge.mEdgeState &= (~Edge::eACTIVATING);
+	}
+
+
+	NodeIndex nodeIndex1 = mEdgeNodeIndices[index * 2];
+	NodeIndex nodeIndex2 = mEdgeNodeIndices[index * 2 + 1];
+
+	if (nodeIndex1.isValid() && nodeIndex2.isValid())
+	{
+		{
+			Node& node = mNodes[nodeIndex1.index()];
+			node.mActiveRefCount--;
+		}
+		{
+			Node& node = mNodes[nodeIndex2.index()];
+			node.mActiveRefCount--;
+		}
+	}
+
+	if(edge.mEdgeType == Edge::eCONTACT_MANAGER)
+		mActiveContactEdges.reset(index);
+
+}
+
+void IslandSim::setKinematic(IG::NodeIndex nodeIndex)
+{
+
+	Node& node = mNodes[nodeIndex.index()];
+
+	if(!node.isKinematic())
+	{
+
+		//Transition from dynamic to kinematic:
+		//(1) Remove this node from the island
+		//(2) Remove this node from the active node list
+		//(3) If active or referenced, add it to the active kinematic list
+		//(4) Tag the node as kinematic
+		//External code will re-filter interactions and lost touches will be reported
+
+		IslandId islandId = mIslandIds[nodeIndex.index()];
+		PX_ASSERT(islandId != IG_INVALID_ISLAND);
+
+		Island& island = mIslands[islandId];
+
+		mIslandIds[nodeIndex.index()] = IG_INVALID_ISLAND;
+
+		removeNodeFromIsland(island, nodeIndex);
+
+		bool isActive = node.isActive();
+
+		if (isActive)
+		{
+			//Remove from active list...
+			markInactive(nodeIndex);
+		}
+		else if (node.isActivating())
+		{
+			//Remove from activating list...
+			node.clearActivating();
+			PX_ASSERT(mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]].index() == nodeIndex.index());
+
+			NodeIndex replaceIndex = mActivatingNodes[mActivatingNodes.size() - 1];
+			mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[nodeIndex.index()];
+			mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]] = replaceIndex;
+			mActivatingNodes.forceSize_Unsafe(mActivatingNodes.size() - 1);
+			mActiveNodeIndex[nodeIndex.index()] = IG_INVALID_NODE;
+		}
+
+		node.setKinematicFlag();
+
+		node.clearActive();
+
+		if (/*isActive || */node.mActiveRefCount != 0)
+		{
+			//Add to active kinematic list...
+			PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == IG_INVALID_NODE);
+
+			mActiveNodeIndex[nodeIndex.index()] = mActivatingNodes.size();
+			mActivatingNodes.pushBack(nodeIndex);
+			node.setActivating();
+		}
+
+		PxU32 newSize = island.mSize[0] + island.mSize[1];
+		
+		{
+			//This node was in an island with other bodies. We need to force an island recomputation in case the
+			//islands became broken due to losing this connection. Same rules as losing a contact, we just 
+			//tag the nodes directly connected to the lost edge as "dirty" and force an island recomputation if 
+			//it resulted in lost connections
+			EdgeInstanceIndex edgeId = node.mFirstEdgeIndex;
+			while(edgeId != IG_INVALID_EDGE)
+			{
+				EdgeInstance& instance = mEdgeInstances[edgeId];
+				EdgeInstanceIndex nextId = instance.mNextEdge;
+
+				PxU32 idx = edgeId/2;
+				IG::Edge& edge = mEdges[edgeId/2];
+
+				removeEdgeFromIsland(island, idx);
+
+				removeConnectionInternal(idx);
+				removeConnectionFromGraph(idx);
+
+				edge.clearInserted();
+
+				if (edge.isActive())
+				{
+					removeEdgeFromActivatingList(idx);
+					edge.deactivateEdge();
+					mActiveEdgeCount[edge.mEdgeType]--;
+				}
+
+				if(!edge.isPendingDestroyed())
+				{
+					if(!edge.isInDirtyList())
+					{
+						PX_ASSERT(!contains(mDirtyEdges[edge.mEdgeType], idx));
+						mDirtyEdges[edge.mEdgeType].pushBack(idx);
+						edge.markInDirtyList();
+					}
+				}
+				else
+				{
+					edge.setReportOnlyDestroy();
+				}
+
+				edgeId = nextId;
+			}
+		}
+
+		if(newSize == 0)
+		{
+			//This node was in an island by itself, so no need to mess with any connections
+			for(PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
+			{
+				island.mFirstEdge[a] = island.mLastEdge[a] = IG_INVALID_EDGE;
+				island.mEdgeCount[a] = 0;
+				mIslandStaticTouchCount[islandId] = 0;
+				//island.mStaticTouchCount = 0;
+			}
+
+			if(island.mActiveIndex != IG_INVALID_ISLAND)
+			{
+				markIslandInactive(islandId);
+			}
+
+			mIslandAwake.reset(islandId);
+			mIslandHandles.freeHandle(islandId);
+		}
+	}
+}
+
+void IslandSim::setDynamic(IG::NodeIndex nodeIndex)
+{
+	//(1) Remove all edges involving this node from all islands they may be in
+	//(2) Mark all edges as "new" edges - let island gen re-process them!
+	//(3) Remove this node from the active kinematic list
+	//(4) Add this node to the active dynamic list (if it is active)
+	//(5) Mark node as dynamic
+	
+
+	Node& node = mNodes[nodeIndex.index()];
+
+	if(node.isKinematic())
+	{
+		//EdgeInstanceIndex edgeIndex = node.mFirstEdgeIndex;
+
+		EdgeInstanceIndex edgeId = node.mFirstEdgeIndex;
+		while(edgeId != IG_INVALID_EDGE)
+		{
+			EdgeInstance& instance = mEdgeInstances[edgeId];
+			EdgeInstanceIndex nextId = instance.mNextEdge;
+
+			NodeIndex otherNode = mEdgeNodeIndices[edgeId^1];
+
+			PxU32 idx = edgeId/2;
+			IG::Edge& edge = mEdges[edgeId/2];
+
+			if(!otherNode.isStaticBody())
+			{
+				IslandId islandId = mIslandIds[otherNode.index()];
+				if(islandId != IG_INVALID_ISLAND)
+					removeEdgeFromIsland(mIslands[islandId], idx);
+			}
+
+			removeConnectionInternal(idx);
+			removeConnectionFromGraph(idx);
+
+			edge.clearInserted();
+			if (edge.isActive())
+			{
+				edge.deactivateEdge();
+				removeEdgeFromActivatingList(idx);
+				mActiveEdgeCount[edge.mEdgeType]--;
+			}
+			
+			if(!edge.isPendingDestroyed())
+			{
+				if(!edge.isInDirtyList())
+				{
+					PX_ASSERT(!contains(mDirtyEdges[edge.mEdgeType], idx));
+					mDirtyEdges[edge.mEdgeType].pushBack(idx);
+					edge.markInDirtyList();
+				}
+			}
+			else
+			{
+				edge.setReportOnlyDestroy();
+			}
+
+			edgeId = nextId;
+		}
+
+		
+		if(!node.isActivating() && mActiveNodeIndex[nodeIndex.index()] != IG_INVALID_NODE)
+		{
+			//Remove from active kinematic list, add to active dynamic list
+			PxU32 oldRefCount = node.mActiveRefCount;
+			node.mActiveRefCount = 0;
+			markKinematicInactive(nodeIndex);
+			node.mActiveRefCount = oldRefCount;
+		}
+
+		node.clearKinematicFlag();
+
+		
+
+		//Create an island for this node. If there are any edges affecting this node, they will have been marked as 
+		//"new" and will be processed next island update.
+		{
+			IslandId islandHandle = mIslandHandles.getHandle();
+			
+			if(islandHandle == mIslands.capacity())
+			{
+				const PxU32 newCapacity = 2*mIslands.capacity()+1;
+				mIslands.reserve(newCapacity);
+				mIslandAwake.resize(newCapacity);
+				mIslandStaticTouchCount.resize(newCapacity);
+			}
+			mIslandAwake.reset(islandHandle);
+			mIslands.resize(PxMax(islandHandle+1, mIslands.size()));
+			mIslandStaticTouchCount.resize(PxMax(islandHandle + 1, mIslands.size()));
+			Island& island = mIslands[islandHandle];
+			island.mLastNode = island.mRootNode = nodeIndex;
+			PX_ASSERT(mNodes[nodeIndex.index()].mNextNode.index() == IG_INVALID_NODE);
+			island.mSize[node.mType] = 1;
+			mIslandIds[nodeIndex.index()] = islandHandle;
+			mIslandStaticTouchCount[islandHandle] = 0;
+
+
+			if(node.isActive())
+			{
+				node.clearActive();
+
+				activateNode(nodeIndex);
+			}
+		}
+	}
+}
+
+}
+}
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsMaterialCombiner.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsMaterialCombiner.cpp
new file mode 100644
index 00000000..9c06336b
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsMaterialCombiner.cpp
@@ -0,0 +1,102 @@
+// 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 "PxsMaterialCombiner.h"
+#include "PsMathUtils.h"
+#include "CmPhysXCommon.h"
+#include "PsFoundation.h"
+
+namespace physx   
+{
+
+
+PxsMaterialCombiner::PxsMaterialCombiner(PxReal staticFrictionScaling, PxReal dynamicFrictionScaling)
+:	mStaticFrictionScaling(staticFrictionScaling), mDynamicFrictionScaling(dynamicFrictionScaling)
+{}
+
+
+PxReal PxsMaterialCombiner::combineRestitution(const PxsMaterialData& mat0, const PxsMaterialData& mat1)
+{
+	/*return combineScalars(mat0.restitution, mat1.restitution, PxMax(mat0.restitutionCombineMode, mat1.restitutionCombineMode));*/
+	return combineScalars(mat0.restitution, mat1.restitution, PxMax(mat0.getRestitutionCombineMode(), mat1.getRestitutionCombineMode()));
+}
+   
+PxsMaterialCombiner::PxsCombinedMaterial PxsMaterialCombiner::combineIsotropicFriction(const PxsMaterialData& mat0, const PxsMaterialData& mat1)
+{
+	PxsCombinedMaterial dest;
+
+	dest.flags = (mat0.flags | mat1.flags); //& (PxMaterialFlag::eDISABLE_STRONG_FRICTION|PxMaterialFlag::eDISABLE_FRICTION);	//eventually set DisStrongFric flag, lower all others.
+
+	if (!(dest.flags & PxMaterialFlag::eDISABLE_FRICTION))
+	{
+		const PxI32 fictionCombineMode = PxMax(mat0.getFrictionCombineMode(), mat1.getFrictionCombineMode());
+		PxReal dynFriction = 0.f;
+		PxReal staFriction = 0.f;
+
+
+		switch (fictionCombineMode)
+		{
+		case PxCombineMode::eAVERAGE:
+			dynFriction = 0.5f * (mat0.dynamicFriction + mat1.dynamicFriction);
+			staFriction = 0.5f * (mat0.staticFriction + mat1.staticFriction);
+			break;
+		case PxCombineMode::eMIN:
+			dynFriction = PxMin(mat0.dynamicFriction, mat1.dynamicFriction);
+			staFriction = PxMin(mat0.staticFriction, mat1.staticFriction);
+			break;
+		case PxCombineMode::eMULTIPLY:
+			dynFriction = (mat0.dynamicFriction * mat1.dynamicFriction);
+			staFriction = (mat0.staticFriction * mat1.staticFriction);
+			break;
+		case PxCombineMode::eMAX:
+			dynFriction = PxMax(mat0.dynamicFriction, mat1.dynamicFriction);
+			staFriction = PxMax(mat0.staticFriction, mat1.staticFriction);
+			break;
+		}   
+
+		dynFriction*=mDynamicFrictionScaling;
+		staFriction*=mStaticFrictionScaling;
+		//isotropic case
+		const PxReal fDynFriction = PxMax(dynFriction, 0.f);
+
+		const PxReal fStaFriction = physx::intrinsics::fsel(staFriction - fDynFriction, staFriction, fDynFriction);
+		dest.dynFriction = fDynFriction;
+		dest.staFriction = fStaFriction;
+	}
+	else
+	{
+		dest.flags |= PxMaterialFlag::eDISABLE_STRONG_FRICTION;
+		dest.staFriction = 0.0f;
+		dest.dynFriction = 0.0f;
+	}
+
+	return dest;
+}
+}
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp
new file mode 100644
index 00000000..e7bcd737
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp
@@ -0,0 +1,952 @@
+// 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 "PxsContext.h"
+#include "CmFlushPool.h"
+#include "PxsSimpleIslandManager.h"
+
+//Enable tuner profiling.
+#ifdef PX_PS3
+#include "CellTimerMarker.h"
+#endif
+
+#if PX_SUPPORT_GPU_PHYSX
+#include "PxPhysXGpu.h"
+#include "task/PxGpuDispatcher.h"
+#endif
+
+#include "PxsContactManagerState.h"
+
+#include "PxsNphaseImplementationContext.h"
+#include "PxvGeometry.h"
+#include "PxvDynamics.h"
+
+#include "PxcNpContactPrepShared.h"
+
+using namespace physx;
+using namespace physx::shdfnd;
+
+
+class PxsCMDiscreteUpdateTask : public PxsCMUpdateTask
+{
+public:
+	PxsCMDiscreteUpdateTask(PxsContext* context, PxReal dt, PxsContactManager** cms, PxsContactManagerOutput* cmOutputs, Gu::Cache* caches, PxU32 nbCms,
+		PxContactModifyCallback* callback):
+	  PxsCMUpdateTask(context, dt, cms, cmOutputs, caches, nbCms, callback) 
+	{}
+
+	virtual ~PxsCMDiscreteUpdateTask()
+	{}
+
+	void runModifiableContactManagers(PxU32* modifiableIndices, PxU32 nbModifiableManagers, PxcNpThreadContext& threadContext, PxU32& foundPatchCount_, PxU32& lostPatchCount_,
+		PxU32& maxPatches_)
+	{
+		PX_ASSERT(nbModifiableManagers != 0);
+
+		PxU32 foundPatchCount = foundPatchCount_;
+		PxU32 lostPatchCount = lostPatchCount_;
+		PxU32 maxPatches = maxPatches_;
+
+		Cm::BitMap& localPatchChangedMap = threadContext.getLocalPatchChangeMap();
+
+		class PxcContactSet: public PxContactSet
+		{
+		public:
+			PxcContactSet(PxU32 count, PxModifiableContact *contacts)
+			{
+				mContacts = contacts;
+				mCount = count;
+			}
+			PxModifiableContact*	getContacts()	{ return mContacts; }
+			PxU32					getCount()		{ return mCount; }	
+
+		};
+
+
+
+		if(mCallback)
+		{
+			PX_ALLOCA(mModifiablePairArray, PxContactModifyPair, nbModifiableManagers);
+
+
+			PxsTransformCache& transformCache = mContext->getTransformCache();
+		
+			for(PxU32 i = 0; i < nbModifiableManagers; ++i)
+			{
+				PxU32 index = modifiableIndices[i];
+				PxsContactManager& cm = *mCmArray[index];
+	
+				PxsContactManagerOutput& output = mCmOutputs[index];
+	
+				PxU32 count = output.nbContacts;
+	
+				if(count)
+				{
+					PxContactModifyPair& p = mModifiablePairArray[i];
+					PxcNpWorkUnit &unit = cm.getWorkUnit();
+	
+					p.shape[0] = gPxvOffsetTable.convertPxsShape2Px(unit.shapeCore0);
+					p.shape[1] = gPxvOffsetTable.convertPxsShape2Px(unit.shapeCore1);
+	
+					p.actor[0] = unit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY0 ? gPxvOffsetTable.convertPxsRigidCore2PxRigidBody(unit.rigidCore0)
+						: gPxvOffsetTable.convertPxsRigidCore2PxRigidStatic(unit.rigidCore0);
+
+					p.actor[1] = unit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1 ? gPxvOffsetTable.convertPxsRigidCore2PxRigidBody(unit.rigidCore1)
+						: gPxvOffsetTable.convertPxsRigidCore2PxRigidStatic(unit.rigidCore1);
+	
+					p.transform[0] = transformCache.getTransformCache(unit.mTransformCache0).transform;
+					p.transform[1] = transformCache.getTransformCache(unit.mTransformCache1).transform;
+	
+					PxModifiableContact* contacts = reinterpret_cast<PxModifiableContact*>(output.contactPoints);
+					static_cast<PxcContactSet&>(p.contacts) = PxcContactSet(count, contacts);
+	
+					PxReal mi0 = unit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY0 ? static_cast<const PxsBodyCore*>(unit.rigidCore0)->maxContactImpulse : PX_MAX_F32;
+					PxReal mi1 = unit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1 ? static_cast<const PxsBodyCore*>(unit.rigidCore1)->maxContactImpulse : PX_MAX_F32;
+					PxReal maxImpulse = PxMin(mi0, mi1);
+					for (PxU32 j = 0; j < count; j++)
+						contacts[j].maxImpulse = maxImpulse;
+	
+		#if PX_ENABLE_SIM_STATS
+					PxU8 gt0 = Ps::to8(unit.geomType0), gt1 = Ps::to8(unit.geomType1);
+					threadContext.mModifiedContactPairs[PxMin(gt0, gt1)][PxMax(gt0, gt1)]++;
+		#endif
+				}
+			}
+	
+			mCallback->onContactModify(mModifiablePairArray, nbModifiableManagers);
+		}
+	
+		for(PxU32 i = 0; i < nbModifiableManagers; ++i)
+		{
+			PxU32 index = modifiableIndices[i];
+			PxsContactManager& cm = *mCmArray[index];
+	
+			//Loop through the contacts in the contact stream and update contact count!
+	
+			PxU32 numContacts = 0;
+			PxcNpWorkUnit& unit = cm.getWorkUnit();
+			PxsContactManagerOutput& output = mCmOutputs[index];
+	
+			
+			PxU32 numPatches = output.nbPatches;
+		
+			if (output.nbContacts)
+			{
+				//PxU8* compressedContacts = cm.getWorkUnit().compressedContacts;
+				//PxModifyContactHeader* header = reinterpret_cast<PxModifyContactHeader*>(compressedContacts);
+				PxContactPatch* patches = reinterpret_cast<PxContactPatch*>(output.contactPatches);
+				PxModifiableContact* points = reinterpret_cast<PxModifiableContact*>(output.contactPoints);
+
+				if (patches->internalFlags & PxContactPatch::eREGENERATE_PATCHES)
+				{
+					//Some data was modified that must trigger patch re-generation...
+					for (PxU8 k = 0; k < numPatches; ++k)
+					{
+						PxU32 startIndex = patches[k].startContactIndex;
+
+						patches[k].normal = points[startIndex].normal;
+						patches[k].dynamicFriction = points[startIndex].dynamicFriction;
+						patches[k].staticFriction = points[startIndex].staticFriction;
+						patches[k].restitution = points[startIndex].restitution;
+						patches[k].materialIndex0 = points[startIndex].materialIndex0;
+						patches[k].materialIndex1 = points[startIndex].materialIndex1;
+
+						for (PxU32 j = 1; j < patches[k].nbContacts; ++j)
+						{
+							if (points[startIndex].normal.dot(points[j + startIndex].normal) < PXC_SAME_NORMAL
+								&& points[startIndex].maxImpulse > 0.f) //TODO - this needs extending for material indices but we don't support modifying those yet
+							{
+								//The points are now in a separate friction patch...
+								for (PxU32 c = numPatches - 1; c > k; --c)
+								{
+									patches[c + 1] = patches[c];
+								}
+								numPatches++;
+								patches[k + 1].materialFlags = patches[k].materialFlags;
+								patches[k + 1].internalFlags = patches[k].internalFlags;
+								patches[k + 1].startContactIndex = Ps::to8(j + startIndex);
+								patches[k + 1].nbContacts = Ps::to8(patches[k].nbContacts - j);
+								//Fill in patch information now that final patches are available
+								patches[k].nbContacts = PxU8(j);
+								break;
+							}
+						}
+					}
+				}
+
+				if (output.prevPatches < numPatches)
+				{
+					foundPatchCount++;
+					localPatchChangedMap.growAndSet(unit.index);
+				}
+
+				maxPatches = PxMax(maxPatches, PxU32(numPatches));
+
+				output.nbPatches = PxU8(numPatches);
+
+				for (PxU32 a = 0; a < output.nbContacts; ++a)
+				{
+					numContacts += points[a].maxImpulse != 0.f;
+				}
+			}
+
+			if(output.nbPatches < output.prevPatches)
+			{
+				lostPatchCount++;
+				//Trigger a lost patch event...required to let the solver 
+				localPatchChangedMap.growAndSet(unit.index);
+			}
+	
+			if(!numContacts)
+			{	
+				//KS - we still need to retain the patch count from the previous frame to detect found/lost events...
+				PxcNpWorkUnitClearFrictionCachedState(unit);
+				output.nbPatches = 0;
+				output.nbContacts = 0;
+
+				if(output.prevPatches)
+				{
+					lostPatchCount++;
+					//Trigger a lost patch event...required to let the solver 
+					localPatchChangedMap.growAndSet(unit.index);
+				}
+
+				continue;
+			}
+	
+			if(threadContext.mContactStreamPool)
+			{
+				//We need to allocate a new structure inside the contact stream pool
+	
+				PxU32 patchSize = output.nbPatches * sizeof(PxContactPatch);
+				PxU32 contactSize = output.nbContacts * sizeof(PxExtendedContact);
+	
+				/*PxI32 increment = (PxI32)(patchSize + contactSize);
+				PxI32 index = Ps::atomicAdd(&mContactStreamPool->mSharedContactIndex, increment) - increment;
+				PxU8* address = mContactStreamPool->mContactStream + index;*/
+				bool isOverflown = false;
+	
+				PxI32 contactIncrement = PxI32(contactSize);
+				PxI32 contactIndex = Ps::atomicAdd(&threadContext.mContactStreamPool->mSharedDataIndex, contactIncrement);
+				
+				if (threadContext.mContactStreamPool->isOverflown())
+				{
+					PX_WARN_ONCE("Contact buffer overflow detected, please increase its size in the scene desc!\n");
+					isOverflown = true;
+				}
+							
+				PxU8* contactAddress = threadContext.mContactStreamPool->mDataStream  + threadContext.mContactStreamPool->mDataStreamSize - contactIndex;
+	
+				PxI32 patchIncrement = PxI32(patchSize);
+				PxI32 patchIndex = Ps::atomicAdd(&threadContext.mPatchStreamPool->mSharedDataIndex, patchIncrement);
+				
+				if (threadContext.mPatchStreamPool->isOverflown())
+				{
+					PX_WARN_ONCE("Patch buffer overflow detected, please increase its size in the scene desc!\n");
+					isOverflown = true;
+				}
+				
+				PxU8* patchAddress = threadContext.mPatchStreamPool->mDataStream + threadContext.mPatchStreamPool->mDataStreamSize - patchIndex;
+	
+	
+				PxU32 internalFlags = reinterpret_cast<PxContactPatch*>(output.contactPatches)->internalFlags;
+	
+				PxI32 increment2 = PxI32(output.nbContacts * sizeof(PxReal));
+				PxI32 index2 = Ps::atomicAdd(&threadContext.mForceAndIndiceStreamPool->mSharedDataIndex, increment2);
+				
+				if (threadContext.mForceAndIndiceStreamPool->isOverflown())
+				{
+					PX_WARN_ONCE("Force buffer overflow detected, please increase its size in the scene desc!\n");
+					isOverflown = true;
+				}
+	
+				if (isOverflown)
+				{
+					output.contactPoints = NULL;
+					output.contactPatches = NULL;
+					output.contactForces = NULL;
+				
+					output.nbContacts = output.nbPatches = 0;
+				}
+				else
+				{
+					output.contactForces = reinterpret_cast<PxReal*>(threadContext.mForceAndIndiceStreamPool->mDataStream + threadContext.mForceAndIndiceStreamPool->mDataStreamSize - index2);
+				
+					PxMemZero(output.contactForces, sizeof(PxReal) * output.nbContacts);
+					
+					PxExtendedContact* contacts = reinterpret_cast<PxExtendedContact*>(contactAddress);
+					PxMemCopy(patchAddress, output.contactPatches, sizeof(PxContactPatch) * output.nbPatches);
+	
+					PxContactPatch* newPatches = reinterpret_cast<PxContactPatch*>(patchAddress);
+	
+					internalFlags |= PxContactPatch::eCOMPRESSED_MODIFIED_CONTACT;
+	
+					for(PxU32 a = 0; a < output.nbPatches; ++a)
+					{
+						newPatches[a].internalFlags = PxU8(internalFlags);
+					}
+	
+					//KS - only the first patch will have mass modification properties set. For the GPU solver, this must be propagated to the remaining patches
+					for(PxU32 a = 1; a < output.nbPatches; ++a)
+					{
+						newPatches[a].mMassModification = newPatches->mMassModification;
+					}
+	
+					PxModifiableContact* sourceContacts = reinterpret_cast<PxModifiableContact*>(output.contactPoints);
+	
+					for(PxU32 a = 0; a < output.nbContacts; ++a)
+					{
+						PxExtendedContact& contact = contacts[a];
+						PxModifiableContact& srcContact = sourceContacts[a];
+						contact.contact = srcContact.contact;
+						contact.separation = srcContact.separation;
+						contact.targetVelocity = srcContact.targetVelocity;
+						contact.maxImpulse = srcContact.maxImpulse;
+					}
+	
+					output.contactPatches = patchAddress;
+					output.contactPoints = reinterpret_cast<PxU8*>(contacts);
+				}
+			}
+		}
+
+		foundPatchCount_ = foundPatchCount;
+		lostPatchCount_ = lostPatchCount;
+		maxPatches_ = maxPatches;
+	}
+
+
+	template < void (*NarrowPhase)(PxcNpThreadContext&, PxcNpWorkUnit&, Gu::Cache&, PxsContactManagerOutput&)>
+	void processCms(PxcNpThreadContext* threadContext)
+	{
+		// PT: use local variables to avoid reading class members N times, if possible
+		const PxU32 nb = mCmCount;
+		PxsContactManager** PX_RESTRICT cmArray = mCmArray;
+
+		PxU32 lostPatchCount = 0, foundPatchCount = 0;
+
+		PxU32 maxPatches = threadContext->mMaxPatches;
+
+		PxU32 newTouchCMCount = 0, lostTouchCMCount = 0;
+		Cm::BitMap& localChangeTouchCM = threadContext->getLocalChangeTouch();
+		Cm::BitMap& localPatchChangedMap = threadContext->getLocalPatchChangeMap();
+
+		PX_ALLOCA(modifiableIndices, PxU32, nb);
+		PxU32 modifiableCount = 0;
+
+		for(PxU32 i=0;i<nb;i++)
+		{
+			const PxU32 prefetch1 = PxMin(i + 1, nb - 1);
+			const PxU32 prefetch2 = PxMin(i + 2, nb - 1);
+
+			Ps::prefetchLine(cmArray[prefetch2]);
+			Ps::prefetchLine(&mCmOutputs[prefetch2]);
+			Ps::prefetchLine(cmArray[prefetch1]->getWorkUnit().shapeCore0);
+			Ps::prefetchLine(cmArray[prefetch1]->getWorkUnit().shapeCore1);
+			Ps::prefetchLine(&threadContext->mTransformCache->getTransformCache(cmArray[prefetch1]->getWorkUnit().mTransformCache0));
+			Ps::prefetchLine(&threadContext->mTransformCache->getTransformCache(cmArray[prefetch1]->getWorkUnit().mTransformCache1));
+
+			PxsContactManager* cm = cmArray[i];			
+
+			if(cm)
+			{
+				PxsContactManagerOutput& output = mCmOutputs[i];
+				PxcNpWorkUnit& unit = cm->getWorkUnit();
+
+				output.prevPatches = output.nbPatches;
+
+
+				PxU8 oldStatusFlag = output.statusFlag;
+
+				PxU8 oldTouch = Ps::to8(oldStatusFlag & PxsContactManagerStatusFlag::eHAS_TOUCH);
+
+				Gu::Cache& cache = mCaches[i];
+
+				NarrowPhase(*threadContext, unit, cache, output);
+				
+				PxU16 newTouch = Ps::to8(output.statusFlag & PxsContactManagerStatusFlag::eHAS_TOUCH);
+				
+
+				bool modifiable = output.nbPatches != 0 && unit.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT;
+
+				if(modifiable)
+				{
+					modifiableIndices[modifiableCount++] = i;
+				}
+				else
+				{
+					maxPatches = PxMax(maxPatches, Ps::to32(output.nbPatches));
+
+					if(output.prevPatches != output.nbPatches)
+					{
+						localPatchChangedMap.growAndSet(cmArray[i]->getIndex());	
+						if(output.prevPatches < output.nbPatches)
+							foundPatchCount++;
+						else
+							lostPatchCount++;
+					}
+				}
+
+				if (newTouch ^ oldTouch)
+				{
+					cm->getWorkUnit().statusFlags = PxU8(output.statusFlag | (unit.statusFlags & PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH));  //KS - todo - remove the need to access the work unit at all!
+					localChangeTouchCM.growAndSet(cmArray[i]->getIndex());
+					if(newTouch)
+						newTouchCMCount++;
+					else
+						lostTouchCMCount++;
+				}
+				else if (!(oldStatusFlag&PxsContactManagerStatusFlag::eTOUCH_KNOWN))
+				{
+					cm->getWorkUnit().statusFlags = PxU8(output.statusFlag | (unit.statusFlags & PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH));  //KS - todo - remove the need to access the work unit at all!
+				}
+			}
+		}
+
+		if(modifiableCount)
+		{
+			runModifiableContactManagers(modifiableIndices, modifiableCount, *threadContext, foundPatchCount, lostPatchCount, maxPatches);
+		}
+
+
+		threadContext->addLocalNewTouchCount(newTouchCMCount);
+		threadContext->addLocalLostTouchCount(lostTouchCMCount);
+
+		threadContext->addLocalFoundPatchCount(foundPatchCount);
+		threadContext->addLocalLostPatchCount(lostPatchCount);
+
+		threadContext->mMaxPatches = maxPatches;
+	}
+
+	virtual void runInternal()
+	{
+		PX_PROFILE_ZONE("Sim.narrowPhase", mContext->getContextId());
+
+		PxcNpThreadContext* PX_RESTRICT threadContext = mContext->getNpThreadContext(); 
+	
+		threadContext->mDt = mDt;
+
+		const bool pcm = mContext->getPCM();
+		threadContext->mPCM = pcm;
+		threadContext->mCreateAveragePoint = mContext->getCreateAveragePoint();
+		threadContext->mContactCache = mContext->getContactCacheFlag();
+		threadContext->mTransformCache = &mContext->getTransformCache();
+		threadContext->mContactDistance = mContext->getContactDistance();
+
+		if(pcm)
+		{
+			processCms<PxcDiscreteNarrowPhasePCM>(threadContext);
+		}
+		else
+		{
+			processCms<PxcDiscreteNarrowPhase>(threadContext);
+		}
+
+		mContext->putNpThreadContext(threadContext);
+	}
+
+	virtual const char* getName() const
+	{
+		return "PxsContext.contactManagerDiscreteUpdate";
+	}
+};
+
+void PxsNphaseImplementationContext::processContactManager(PxReal dt, PxsContactManagerOutput* cmOutputs, PxBaseTask* continuation)
+{
+		//Iterate all active contact managers
+	mContext.mTaskPool.lock();
+	const PxU32 nbCmsToProcess = mNarrowPhasePairs.mContactManagerMapping.size();
+
+	for(PxU32 a = 0; a < nbCmsToProcess;)
+	{
+		void* ptr = mContext.mTaskPool.allocateNotThreadSafe(sizeof(PxsCMDiscreteUpdateTask));
+		PxU32 nbToProcess = PxMin(nbCmsToProcess - a, PxsCMUpdateTask::BATCH_SIZE);
+		PxsCMDiscreteUpdateTask* task = PX_PLACEMENT_NEW(ptr, PxsCMDiscreteUpdateTask)(&mContext, dt, mNarrowPhasePairs.mContactManagerMapping.begin() + a, 
+			cmOutputs + a, mNarrowPhasePairs.mCaches.begin() + a, nbToProcess, mModifyCallback);
+
+		a += nbToProcess;
+
+		task->setContinuation(continuation);
+		task->removeReference();
+	}
+	mContext.mTaskPool.unlock();
+}
+
+void PxsNphaseImplementationContext::processContactManagerSecondPass(PxReal dt, PxBaseTask* continuation)
+{
+		//Iterate all active contact managers
+	mContext.mTaskPool.lock();
+
+	const PxU32 nbCmsToProcess = mNewNarrowPhasePairs.mContactManagerMapping.size();
+
+	for(PxU32 a = 0; a < nbCmsToProcess;)
+	{
+		void* ptr = mContext.mTaskPool.allocateNotThreadSafe(sizeof(PxsCMDiscreteUpdateTask));
+		PxU32 nbToProcess = PxMin(nbCmsToProcess - a, PxsCMUpdateTask::BATCH_SIZE);
+		PxsCMDiscreteUpdateTask* task = PX_PLACEMENT_NEW(ptr, PxsCMDiscreteUpdateTask)(&mContext, dt, mNewNarrowPhasePairs.mContactManagerMapping.begin() + a, 
+			mNewNarrowPhasePairs.mOutputContactManagers.begin() + a, mNewNarrowPhasePairs.mCaches.begin() + a, nbToProcess,
+			mModifyCallback);
+
+		a += nbToProcess;
+
+		task->setContinuation(continuation);
+		task->removeReference();
+	}
+	mContext.mTaskPool.unlock();
+}
+
+void PxsNphaseImplementationContext::updateContactManager(PxReal dt, bool /*hasBoundsArrayChanged*/, bool /*hasContactDistanceChanged*/, PxBaseTask* continuation, PxBaseTask* firstPassNpContinuation)
+{
+	PX_PROFILE_ZONE("Sim.queueNarrowPhase", mContext.mContextID);
+
+	firstPassNpContinuation->removeReference();
+
+	mContext.clearManagerTouchEvents();
+
+#if PX_ENABLE_SIM_STATS
+	mContext.mSimStats.mNbDiscreteContactPairsTotal = 0;
+	mContext.mSimStats.mNbDiscreteContactPairsWithCacheHits = 0;
+	mContext.mSimStats.mNbDiscreteContactPairsWithContacts = 0;
+#endif
+
+	
+	//KS - temporarily put this here. TODO - move somewhere better
+	mContext.mTotalCompressedCacheSize = 0;
+	mContext.mMaxPatches = 0;
+	
+	processContactManager(dt, mNarrowPhasePairs.mOutputContactManagers.begin(), continuation);
+
+}
+
+void PxsNphaseImplementationContext::secondPassUpdateContactManager(PxReal dt, PxBaseTask* continuation)
+{
+	PX_PROFILE_ZONE("Sim.queueNarrowPhase", mContext.mContextID);
+
+	processContactManagerSecondPass(dt, continuation);		
+}
+
+PxsNphaseImplementationContext* PxsNphaseImplementationContext::create(PxsContext& context, IG::IslandSim* islandSim)
+{
+	PxsNphaseImplementationContext* npImplContext = reinterpret_cast<PxsNphaseImplementationContext*>(
+		PX_ALLOC(sizeof(PxsNphaseImplementationContext), "PxsNphaseImplementationContext"));
+
+	if (npImplContext)
+	{
+		new(npImplContext) PxsNphaseImplementationContext(context, islandSim);
+	}
+
+	return npImplContext;
+}
+
+void PxsNphaseImplementationContext::destroy()
+{
+	this->~PxsNphaseImplementationContext();
+	PX_FREE(this);
+}
+
+void PxsNphaseImplementationContext::registerContactManagers(PxsContactManager** cms, PxU32 nbContactManagers, PxU32 maxContactManagerId)
+{
+	PX_UNUSED(maxContactManagerId);
+	for (PxU32 a = 0; a < nbContactManagers; ++a)
+	{
+		registerContactManager(cms[a], 0, 0);
+	}
+}
+
+void PxsNphaseImplementationContext::registerContactManager(PxsContactManager* cm, PxI32 touching, PxU32 patchCount)
+{
+	PxcNpWorkUnit& workUnit = cm->getWorkUnit();
+	PxsContactManagerOutput output;
+
+	PxU8 geomType0 = PxU8(workUnit.geomType0);
+	PxU8 geomType1 = PxU8(workUnit.geomType1);
+
+	Gu::Cache cache;
+
+	mContext.createCache(cache, cm, geomType0, geomType1);
+
+	PxMemZero(&output, sizeof(output));
+	output.nbPatches = Ps::to8(patchCount);
+
+	if(workUnit.flags & PxcNpWorkUnitFlag::eOUTPUT_CONSTRAINTS)
+		output.statusFlag |= PxsContactManagerStatusFlag::eREQUEST_CONSTRAINTS;
+
+	if (touching > 0)
+	{
+		output.statusFlag |= PxsContactManagerStatusFlag::eHAS_TOUCH;
+	}
+	else if (touching < 0)
+	{
+		output.statusFlag |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;
+	}
+
+	output.statusFlag |= PxsContactManagerStatusFlag::eDIRTY_MANAGER;
+
+	if (cm->getWorkUnit().statusFlags & PxcNpWorkUnitStatusFlag::eHAS_TOUCH)
+		cm->getWorkUnit().statusFlags |= PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH;
+
+	mNewNarrowPhasePairs.mOutputContactManagers.pushBack(output);
+	mNewNarrowPhasePairs.mCaches.pushBack(cache);
+	mNewNarrowPhasePairs.mContactManagerMapping.pushBack(cm);
+	PxU32 newSz = mNewNarrowPhasePairs.mOutputContactManagers.size();
+	cm->getWorkUnit().mNpIndex = mNewNarrowPhasePairs.computeId(newSz - 1) | PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK;
+}
+
+void PxsNphaseImplementationContext::removeContactManagersFallback(PxsContactManagerOutput* cmOutputs)
+{
+	if (mRemovedContactManagers.size())
+	{
+		Ps::sort(mRemovedContactManagers.begin(), mRemovedContactManagers.size(), Ps::Greater<PxU32>());
+
+		for (PxU32 a = 0; a < mRemovedContactManagers.size(); ++a)
+		{
+#if PX_DEBUG
+			if (a > 0)
+				PX_ASSERT(mRemovedContactManagers[a] < mRemovedContactManagers[a - 1]);
+#endif
+			unregisterContactManagerInternal(mRemovedContactManagers[a], mNarrowPhasePairs, cmOutputs);
+		}
+
+		mRemovedContactManagers.forceSize_Unsafe(0);
+	}
+}
+
+void PxsNphaseImplementationContext::unregisterContactManager(PxsContactManager* cm)
+{
+	PxcNpWorkUnit& unit = cm->getWorkUnit();
+	PxU32 index = unit.mNpIndex;
+	PX_ASSERT(index != 0xFFffFFff);
+
+	if (!(index & PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))
+	{
+		unregisterContactManagerInternal(index, mNarrowPhasePairs, mNarrowPhasePairs.mOutputContactManagers.begin());
+		mNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+	else
+	{
+		//KS - the index in the "new" list will be the index 
+		unregisterContactManagerInternal(index, mNewNarrowPhasePairs, mNewNarrowPhasePairs.mOutputContactManagers.begin());
+		mNewNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNewNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+}
+
+void PxsNphaseImplementationContext::refreshContactManager(PxsContactManager* cm)
+{
+	PxcNpWorkUnit& unit = cm->getWorkUnit();
+	PxU32 index = unit.mNpIndex;
+	PX_ASSERT(index != 0xFFffFFff);
+	PxsContactManagerOutput output;
+	if (!(index & PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))
+	{
+		output = mNarrowPhasePairs.mOutputContactManagers[PxsContactManagerBase::computeIndexFromId(index)];
+		unregisterContactManagerInternal(index, mNarrowPhasePairs, mNarrowPhasePairs.mOutputContactManagers.begin());
+		mNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+	else
+	{
+		output = mNewNarrowPhasePairs.mOutputContactManagers[PxsContactManagerBase::computeIndexFromId(index & (~PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))];
+		//KS - the index in the "new" list will be the index 
+		unregisterContactManagerInternal(index, mNewNarrowPhasePairs, mNewNarrowPhasePairs.mOutputContactManagers.begin());
+		mNewNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNewNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+	PxI32 touching = 0;
+	if(output.statusFlag & PxsContactManagerStatusFlag::eHAS_TOUCH)
+		touching = 1;
+	else if (output.statusFlag & PxsContactManagerStatusFlag::eHAS_NO_TOUCH)
+		touching = -1;
+	registerContactManager(cm, touching, output.nbPatches);
+}
+
+void PxsNphaseImplementationContext::unregisterContactManagerFallback(PxsContactManager* cm, PxsContactManagerOutput* /*cmOutputs*/)
+{
+	PxcNpWorkUnit& unit = cm->getWorkUnit();
+	PxU32 index = unit.mNpIndex;
+	PX_ASSERT(index != 0xFFffFFff);
+
+	if (!(index & PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))
+	{
+		mRemovedContactManagers.pushBack(index);
+	}
+	else
+	{
+		//KS - the index in the "new" list will be the index 
+		unregisterContactManagerInternal(index, mNewNarrowPhasePairs, mNewNarrowPhasePairs.mOutputContactManagers.begin());
+		mNewNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNewNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+}
+
+void PxsNphaseImplementationContext::refreshContactManagerFallback(PxsContactManager* cm, PxsContactManagerOutput* cmOutputs)
+{
+	PxcNpWorkUnit& unit = cm->getWorkUnit();
+	PxU32 index = unit.mNpIndex;
+	PX_ASSERT(index != 0xFFffFFff);
+
+	PxsContactManagerOutput output;
+	if (!(index & PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))
+	{
+		output = cmOutputs[PxsContactManagerBase::computeIndexFromId(index)];
+		//unregisterContactManagerInternal(index, mNarrowPhasePairs, cmOutputs);
+		unregisterContactManagerFallback(cm, cmOutputs);
+	}
+	else
+	{
+		//KS - the index in the "new" list will be the index 
+		output = mNewNarrowPhasePairs.mOutputContactManagers[PxsContactManagerBase::computeIndexFromId(index & (~PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))];
+		unregisterContactManagerInternal(index, mNewNarrowPhasePairs, mNewNarrowPhasePairs.mOutputContactManagers.begin());
+		mNewNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(mNewNarrowPhasePairs.mOutputContactManagers.size()-1);
+	}
+
+	PxI32 touching = 0;
+	if(output.statusFlag & PxsContactManagerStatusFlag::eHAS_TOUCH)
+	{
+		touching = 1;
+		cm->getWorkUnit().statusFlags |= PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH;
+	}
+	else if (output.statusFlag & PxsContactManagerStatusFlag::eHAS_NO_TOUCH)
+		touching = -1;
+	registerContactManager(cm, touching, output.nbPatches);
+		
+
+}
+
+void PxsNphaseImplementationContext::appendContactManagers()
+{
+	//Copy new pairs to end of old pairs. Clear new flag, update npIndex on CM and clear the new pair buffer
+	const PxU32 existingSize = mNarrowPhasePairs.mContactManagerMapping.size();
+	const PxU32 nbToAdd = mNewNarrowPhasePairs.mContactManagerMapping.size();
+	const PxU32 newSize =existingSize + nbToAdd;
+	
+	if(newSize > mNarrowPhasePairs.mContactManagerMapping.capacity())
+	{
+		PxU32 newSz = PxMax(256u, PxMax(mNarrowPhasePairs.mContactManagerMapping.capacity()*2, newSize));
+
+		mNarrowPhasePairs.mContactManagerMapping.reserve(newSz);
+		mNarrowPhasePairs.mOutputContactManagers.reserve(newSz);
+		mNarrowPhasePairs.mCaches.reserve(newSz);
+	}
+
+	mNarrowPhasePairs.mContactManagerMapping.forceSize_Unsafe(newSize);
+	mNarrowPhasePairs.mOutputContactManagers.forceSize_Unsafe(newSize);
+	mNarrowPhasePairs.mCaches.forceSize_Unsafe(newSize);
+
+	PxMemCopy(mNarrowPhasePairs.mContactManagerMapping.begin() + existingSize, mNewNarrowPhasePairs.mContactManagerMapping.begin(), sizeof(PxsContactManager*)*nbToAdd);
+	PxMemCopy(mNarrowPhasePairs.mOutputContactManagers.begin() + existingSize, mNewNarrowPhasePairs.mOutputContactManagers.begin(), sizeof(PxsContactManagerOutput)*nbToAdd);
+	PxMemCopy(mNarrowPhasePairs.mCaches.begin() + existingSize, mNewNarrowPhasePairs.mCaches.begin(), sizeof(Gu::Cache)*nbToAdd);
+
+	PxU32* edgeNodeIndices = mIslandSim->getEdgeNodeIndexPtr();
+
+	for(PxU32 a = 0; a < mNewNarrowPhasePairs.mContactManagerMapping.size(); ++a)
+	{
+		PxsContactManager* cm = mNewNarrowPhasePairs.mContactManagerMapping[a];
+		PxcNpWorkUnit& unit = cm->getWorkUnit();
+		unit.mNpIndex = mNarrowPhasePairs.computeId(existingSize + a);
+
+		if(unit.statusFlags & PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH)
+		{
+			unit.statusFlags &= (~PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH);
+			if(!(unit.flags & PxcNpWorkUnitFlag::eDISABLE_RESPONSE))
+			{
+				PartitionEdge* partitionEdge = mIslandSim->getFirstPartitionEdge(unit.mEdgeIndex);
+
+				while(partitionEdge)
+				{
+					edgeNodeIndices[partitionEdge->mUniqueIndex] = unit.mNpIndex;
+					partitionEdge = partitionEdge->mNextPatch;
+				}
+			}
+		}
+	}
+
+	mNewNarrowPhasePairs.clear();
+}
+
+void PxsNphaseImplementationContext::appendContactManagersFallback(PxsContactManagerOutput* cmOutputs)
+{
+	PX_PROFILE_ZONE("PxsNphaseImplementationContext.appendContactManagersFallback", mContext.mContextID);
+	//Copy new pairs to end of old pairs. Clear new flag, update npIndex on CM and clear the new pair buffer
+	//Copy new pairs to end of old pairs. Clear new flag, update npIndex on CM and clear the new pair buffer
+	const PxU32 existingSize = mNarrowPhasePairs.mContactManagerMapping.size();
+	const PxU32 nbToAdd = mNewNarrowPhasePairs.mContactManagerMapping.size();
+	const PxU32 newSize =existingSize + nbToAdd;
+	
+	if(newSize > mNarrowPhasePairs.mContactManagerMapping.capacity())
+	{
+		PxU32 newSz = PxMax(mNarrowPhasePairs.mContactManagerMapping.capacity()*2, newSize);
+
+		mNarrowPhasePairs.mContactManagerMapping.reserve(newSz);
+		mNarrowPhasePairs.mCaches.reserve(newSz);
+		/*mNarrowPhasePairs.mLostFoundPairsCms.reserve(2 * newSz);
+		mNarrowPhasePairs.mLostFoundPairsOutputData.reserve(2*newSz);*/
+	}
+
+	mNarrowPhasePairs.mContactManagerMapping.forceSize_Unsafe(newSize);
+	mNarrowPhasePairs.mCaches.forceSize_Unsafe(newSize);
+
+	PxMemCopy(mNarrowPhasePairs.mContactManagerMapping.begin() + existingSize, mNewNarrowPhasePairs.mContactManagerMapping.begin(), sizeof(PxsContactManager*)*nbToAdd);
+	PxMemCopy(cmOutputs + existingSize, mNewNarrowPhasePairs.mOutputContactManagers.begin(), sizeof(PxsContactManagerOutput)*nbToAdd);
+	PxMemCopy(mNarrowPhasePairs.mCaches.begin() + existingSize, mNewNarrowPhasePairs.mCaches.begin(), sizeof(Gu::Cache)*nbToAdd);
+
+	PxU32* edgeNodeIndices = mIslandSim->getEdgeNodeIndexPtr();
+
+	for(PxU32 a = 0; a < mNewNarrowPhasePairs.mContactManagerMapping.size(); ++a)
+	{
+		PxsContactManager* cm = mNewNarrowPhasePairs.mContactManagerMapping[a];
+		PxcNpWorkUnit& unit = cm->getWorkUnit();
+		unit.mNpIndex = mNarrowPhasePairs.computeId(existingSize + a);
+
+		if(unit.statusFlags & PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH)
+		{
+			unit.statusFlags &= (~PxcNpWorkUnitStatusFlag::eREFRESHED_WITH_TOUCH);
+			if(!(unit.flags & PxcNpWorkUnitFlag::eDISABLE_RESPONSE))
+			{
+				PartitionEdge* partitionEdge = mIslandSim->getFirstPartitionEdge(unit.mEdgeIndex);
+
+				while(partitionEdge)
+				{
+					edgeNodeIndices[partitionEdge->mUniqueIndex] = unit.mNpIndex;
+					partitionEdge = partitionEdge->mNextPatch;
+				}
+			}
+		}
+	}
+
+	//const PxU32 existingLostFoundPairs = mNarrowPhasePairs.mLostFoundPairsCms.size();
+	//const PxU32 newLostFoundPairs = mNewNarrowPhasePairs.mLostFoundPairsCms.size();
+	//const PxU32 newLostFoundSize = existingLostFoundPairs + newLostFoundPairs;
+
+	//if (mNarrowPhasePairs.mLostFoundPairsCms.capacity() < newLostFoundSize)
+	//{
+	//	const PxU32 newSz = PxMax(newLostFoundSize, 2 * mNarrowPhasePairs.mLostFoundPairsCms.capacity());
+	//	mNarrowPhasePairs.mLostFoundPairsCms.reserve(newSz);
+	//	mNarrowPhasePairs.mLostFoundPairsOutputData.reserve(newSz);
+	//}
+
+	//mNarrowPhasePairs.mLostFoundPairsCms.forceSize_Unsafe(newLostFoundSize);
+	//mNarrowPhasePairs.mLostFoundPairsOutputData.forceSize_Unsafe(newLostFoundSize);
+
+	//PxMemCopy(mNarrowPhasePairs.mLostFoundPairsCms.begin() + existingLostFoundPairs, mNewNarrowPhasePairs.mLostFoundPairsCms.begin(), sizeof(PxsContactManager*)* newLostFoundPairs);
+	//PxMemCopy(mNarrowPhasePairs.mLostFoundPairsOutputData.begin() + existingLostFoundPairs, mNewNarrowPhasePairs.mLostFoundPairsOutputData.begin(), sizeof(PxsContactManagerOutput) * newLostFoundPairs);
+
+	mNewNarrowPhasePairs.clear();
+}
+
+
+
+void PxsNphaseImplementationContext::unregisterContactManagerInternal(PxU32 npIndex, PxsContactManagers& managers, PxsContactManagerOutput* cmOutputs)
+{
+	//TODO - remove this element from the list.
+	PxU32 index = PxsContactManagerBase::computeIndexFromId((npIndex & (~PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK)));
+
+	//Now we replace-with-last and remove the elements...
+
+	PxU32 replaceIndex = managers.mContactManagerMapping.size()-1;
+
+	PxsContactManager* replaceManager = managers.mContactManagerMapping[replaceIndex];
+
+	mContext.destroyCache(managers.mCaches[index]);
+
+	managers.mContactManagerMapping[index] = replaceManager;
+	managers.mCaches[index] = managers.mCaches[replaceIndex];
+	cmOutputs[index] = cmOutputs[replaceIndex];
+
+	PxU32* edgeNodeIndices = mIslandSim->getEdgeNodeIndexPtr();
+
+	PxcNpWorkUnit& replaceUnit = replaceManager->getWorkUnit();
+	replaceUnit.mNpIndex = npIndex;
+	if(replaceUnit.statusFlags & PxcNpWorkUnitStatusFlag::eHAS_TOUCH)
+	{
+		if(!(replaceUnit.flags & PxcNpWorkUnitFlag::eDISABLE_RESPONSE))
+		{
+			PartitionEdge* partitionEdge = mIslandSim->getFirstPartitionEdge(replaceUnit.mEdgeIndex);
+			while(partitionEdge)
+			{
+				edgeNodeIndices[partitionEdge->mUniqueIndex] = replaceUnit.mNpIndex;
+				partitionEdge = partitionEdge->mNextPatch;
+			}
+		}
+	}
+
+	managers.mContactManagerMapping.forceSize_Unsafe(replaceIndex);
+	managers.mCaches.forceSize_Unsafe(replaceIndex);
+}
+
+PxsContactManagerOutput& PxsNphaseImplementationContext::getNewContactManagerOutput(PxU32 npId)
+{
+	PX_ASSERT(npId & PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK);
+	return this->mNewNarrowPhasePairs.mOutputContactManagers[PxsContactManagerBase::computeIndexFromId(npId & (~PxsContactManagerBase::NEW_CONTACT_MANAGER_MASK))];
+}
+
+void PxsNphaseImplementationContext::registerShape(const PxsShapeCore& shapeCore)
+{
+	PX_UNUSED(shapeCore);
+}
+
+void PxsNphaseImplementationContext::updateShapeMaterial(const PxsShapeCore& shapeCore)
+{
+	PX_UNUSED(shapeCore);
+}
+
+void PxsNphaseImplementationContext::updateShapeContactOffset(const PxsShapeCore& shapeCore)
+{
+	PX_UNUSED(shapeCore);
+}
+
+void PxsNphaseImplementationContext::unregisterShape(const PxsShapeCore& shapeCore)
+{
+	PX_UNUSED(shapeCore);
+}
+
+void PxsNphaseImplementationContext::registerMaterial(const PxsMaterialCore& materialCore)
+{
+	PX_UNUSED(materialCore);
+}
+
+void PxsNphaseImplementationContext::updateMaterial(const PxsMaterialCore& materialCore)
+{
+	PX_UNUSED(materialCore);
+}
+
+void PxsNphaseImplementationContext::unregisterMaterial(const PxsMaterialCore& materialCore)
+{
+	PX_UNUSED(materialCore);
+}
+
+PxsContactManagerOutputIterator PxsNphaseImplementationContext::getContactManagerOutputs()
+{
+	PxU32 offsets[1] = {0};
+	return PxsContactManagerOutputIterator(offsets, 1, this->mNarrowPhasePairs.mOutputContactManagers.begin());
+}
+
+
+PxvNphaseImplementationContextUsableAsFallback* physx::createNphaseImplementationContext(PxsContext& context, IG::IslandSim* islandSim)
+{
+	return PxsNphaseImplementationContext::create(context, islandSim);
+}
+
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsSimpleIslandManager.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsSimpleIslandManager.cpp
new file mode 100644
index 00000000..b01eefb0
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsSimpleIslandManager.cpp
@@ -0,0 +1,369 @@
+// 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/PxProfiler.h"
+#include "PxsSimpleIslandManager.h"
+#include "PsSort.h"
+#include "PxsContactManager.h"
+#include "CmTask.h"
+
+#define IG_SANITY_CHECKS 0
+
+namespace physx
+{
+namespace IG
+{
+
+	SimpleIslandManager::SimpleIslandManager(bool useEnhancedDeterminism, PxU64 contextID) : 
+		mDestroyedNodes(PX_DEBUG_EXP("mDestroyedNodes")), 
+		mInteractions(PX_DEBUG_EXP("mInteractions")), 
+		mDestroyedEdges(PX_DEBUG_EXP("mDestroyedEdges")), 
+		mFirstPartitionEdges(PX_DEBUG_EXP("mFirstPartitionEdges")), 
+		mDestroyedPartitionEdges(PX_DEBUG_EXP("IslandSim::mDestroyedPartitionEdges")), 
+		mEdgeNodeIndices(PX_DEBUG_EXP("mEdgeNodeIndices")),
+		mConstraintOrCm(PX_DEBUG_EXP("mConstraintOrCm")),
+		mIslandManager(&mFirstPartitionEdges, mEdgeNodeIndices, &mDestroyedPartitionEdges, contextID),
+		mSpeculativeIslandManager(NULL, mEdgeNodeIndices, NULL, contextID),
+		mSpeculativeThirdPassTask(*this, mSpeculativeIslandManager),
+		mAccurateThirdPassTask(*this, mIslandManager),
+		mPostThirdPassTask(*this),
+		mContextID(contextID)
+{
+	mFirstPartitionEdges.resize(1024);
+	mMaxDirtyNodesPerFrame = useEnhancedDeterminism ? 0xFFFFFFFF : 1000u;
+}
+
+SimpleIslandManager::~SimpleIslandManager()
+{
+}
+
+NodeIndex SimpleIslandManager::addRigidBody(PxsRigidBody* body, bool isKinematic, bool isActive)
+{
+	PxU32 handle = mNodeHandles.getHandle();
+	NodeIndex nodeIndex(handle);
+	mIslandManager.addRigidBody(body, isKinematic, isActive, nodeIndex);
+	mSpeculativeIslandManager.addRigidBody(body, isKinematic, isActive, nodeIndex);
+	return nodeIndex;
+}
+
+void SimpleIslandManager::removeNode(const NodeIndex index)
+{
+	PX_ASSERT(mNodeHandles.isValidHandle(index.index()));
+	mDestroyedNodes.pushBack(index);
+}
+
+NodeIndex SimpleIslandManager::addArticulation(Sc::ArticulationSim* articulation, Dy::Articulation* llArtic, bool isActive)
+{
+	PxU32 handle = mNodeHandles.getHandle();
+	NodeIndex nodeIndex(handle);
+	mIslandManager.addArticulation(articulation, llArtic, isActive, nodeIndex);
+	mSpeculativeIslandManager.addArticulation(articulation, llArtic, isActive, nodeIndex);
+	return nodeIndex;
+}
+
+EdgeIndex SimpleIslandManager::addContactManager(PxsContactManager* manager, NodeIndex nodeHandle1, NodeIndex nodeHandle2, Sc::Interaction* interaction)
+{
+	EdgeIndex handle = mEdgeHandles.getHandle();
+
+	PxU32 nodeIds = 2 * handle;
+	if (mEdgeNodeIndices.size() == nodeIds)
+	{
+		mEdgeNodeIndices.resize(2 * (nodeIds + 2));
+		mConstraintOrCm.resize(2 * (handle + 1));
+		mInteractions.resize(2 * (handle + 1));
+	}
+
+	mEdgeNodeIndices[nodeIds] = nodeHandle1;
+	mEdgeNodeIndices[nodeIds+1] = nodeHandle2;
+	mConstraintOrCm[handle].mCm = manager;
+	mInteractions[handle] = interaction;
+
+	mSpeculativeIslandManager.addContactManager(manager, nodeHandle1, nodeHandle2, handle);
+
+	if (manager)
+		manager->getWorkUnit().mEdgeIndex = handle;
+
+	if(mConnectedMap.size() == handle)
+	{
+		mConnectedMap.resize(2 * (handle + 1));
+	}
+	if (mFirstPartitionEdges.capacity() == handle)
+	{
+		mFirstPartitionEdges.resize(2 * (handle + 1));
+	}
+	mConnectedMap.reset(handle);
+	return handle;
+}
+
+EdgeIndex SimpleIslandManager::addConstraint(Dy::Constraint* constraint, NodeIndex nodeHandle1, NodeIndex nodeHandle2, Sc::Interaction* interaction)
+{
+	EdgeIndex handle = mEdgeHandles.getHandle();
+
+	PxU32 nodeIds = 2 * handle;
+	if (mEdgeNodeIndices.size() == nodeIds)
+	{
+		mEdgeNodeIndices.resize(2 * (mEdgeNodeIndices.size() + 2));
+		mConstraintOrCm.resize(2 * (handle + 1));
+		mInteractions.resize(2 * (handle + 1));
+	}
+
+	mEdgeNodeIndices[nodeIds] = nodeHandle1;
+	mEdgeNodeIndices[nodeIds + 1] = nodeHandle2;
+
+	mConstraintOrCm[handle].mConstraint = constraint;
+
+	mInteractions[handle] = interaction;
+
+	mIslandManager.addConstraint(constraint, nodeHandle1, nodeHandle2, handle);
+	mSpeculativeIslandManager.addConstraint(constraint, nodeHandle1, nodeHandle2, handle);
+	if(mConnectedMap.size() == handle)
+	{
+		mConnectedMap.resize(2*(mConnectedMap.size()+1));
+	}
+
+	if (mFirstPartitionEdges.capacity() == handle)
+	{
+		mFirstPartitionEdges.resize(2 * (mFirstPartitionEdges.capacity() + 1));
+	}
+	mConnectedMap.set(handle);
+	return handle;
+}
+
+void SimpleIslandManager::activateNode(NodeIndex index)
+{
+	mIslandManager.activateNode(index);
+	mSpeculativeIslandManager.activateNode(index);
+}
+
+void SimpleIslandManager::deactivateNode(NodeIndex index)
+{
+	mIslandManager.deactivateNode(index);
+	mSpeculativeIslandManager.deactivateNode(index);
+}
+
+void SimpleIslandManager::putNodeToSleep(NodeIndex index)
+{
+	mIslandManager.putNodeToSleep(index);
+	mSpeculativeIslandManager.putNodeToSleep(index);
+}
+
+void SimpleIslandManager::removeConnection(EdgeIndex edgeIndex)
+{
+	if(edgeIndex == IG_INVALID_EDGE)
+		return;
+	mDestroyedEdges.pushBack(edgeIndex);
+	mSpeculativeIslandManager.removeConnection(edgeIndex);
+	if(mConnectedMap.test(edgeIndex))
+	{
+		mIslandManager.removeConnection(edgeIndex);
+		mConnectedMap.reset(edgeIndex);
+	}
+
+	mConstraintOrCm[edgeIndex].mCm = NULL;
+	mInteractions[edgeIndex] = NULL;
+}
+
+void SimpleIslandManager::firstPassIslandGen()
+{
+	PX_PROFILE_ZONE("Basic.firstPassIslandGen", getContextId());
+	mSpeculativeIslandManager.clearDeactivations();
+	mSpeculativeIslandManager.wakeIslands();
+	mSpeculativeIslandManager.processNewEdges();
+	mSpeculativeIslandManager.removeDestroyedEdges();
+	mSpeculativeIslandManager.processLostEdges(mDestroyedNodes, false, false, mMaxDirtyNodesPerFrame);
+}
+
+void SimpleIslandManager::additionalSpeculativeActivation()
+{
+	mSpeculativeIslandManager.wakeIslands2();
+}
+
+void SimpleIslandManager::secondPassIslandGen()
+{
+	PX_PROFILE_ZONE("Basic.secondPassIslandGen", getContextId());
+	
+	mIslandManager.wakeIslands();
+	mIslandManager.processNewEdges();
+
+	mIslandManager.removeDestroyedEdges();
+	mIslandManager.processLostEdges(mDestroyedNodes, false, false, mMaxDirtyNodesPerFrame);
+
+	for(PxU32 a = 0; a < mDestroyedNodes.size(); ++a)
+	{
+		mNodeHandles.freeHandle(mDestroyedNodes[a].index());
+	}
+	mDestroyedNodes.clear();
+	mDestroyedEdges.clear();
+}
+
+bool SimpleIslandManager::validateDeactivations() const
+{
+	//This method sanity checks the deactivations produced by third-pass island gen. Specifically, it ensures that any bodies that 
+	//the speculative IG wants to deactivate are also candidates for deactivation in the accurate island gen. In practice, both should be the case. If this fails, something went wrong...
+
+	const NodeIndex* const nodeIndices = mSpeculativeIslandManager.getNodesToDeactivate(Node::eRIGID_BODY_TYPE);
+	const PxU32 nbNodesToDeactivate = mSpeculativeIslandManager.getNbNodesToDeactivate(Node::eRIGID_BODY_TYPE);
+
+	for(PxU32 i = 0; i < nbNodesToDeactivate; ++i)
+	{
+		//Node is active in accurate sim => mismatch between accurate and inaccurate sim!
+		const Node& node = mIslandManager.getNode(nodeIndices[i]);
+		const Node& speculativeNode = mSpeculativeIslandManager.getNode(nodeIndices[i]);
+		//KS - we need to verify that the bodies in the "deactivating" list are still candidates for deactivation. There are cases where they may not no longer be candidates, e.g. if the application
+		//put bodies to sleep and activated them
+		if(node.isActive() && !speculativeNode.isActive())
+			return false;
+	}
+	return true;
+}
+
+void ThirdPassTask::runInternal()
+{
+	PX_PROFILE_ZONE("Basic.thirdPassIslandGen", mIslandSim.getContextId());
+	mIslandSim.removeDestroyedEdges();
+	mIslandSim.processLostEdges(mIslandManager.mDestroyedNodes, true, true, mIslandManager.mMaxDirtyNodesPerFrame);
+}
+
+void PostThirdPassTask::runInternal()
+{
+	for (PxU32 a = 0; a < mIslandManager.mDestroyedNodes.size(); ++a)
+	{
+		mIslandManager.mNodeHandles.freeHandle(mIslandManager.mDestroyedNodes[a].index());
+	}
+	mIslandManager.mDestroyedNodes.clear();
+
+	for (PxU32 a = 0; a < mIslandManager.mDestroyedEdges.size(); ++a)
+	{
+		mIslandManager.mEdgeHandles.freeHandle(mIslandManager.mDestroyedEdges[a]);
+	}
+	mIslandManager.mDestroyedEdges.clear();
+
+	PX_ASSERT(mIslandManager.validateDeactivations());
+}
+
+void SimpleIslandManager::thirdPassIslandGen(PxBaseTask* continuation)
+{
+
+	mIslandManager.clearDeactivations();
+
+	mPostThirdPassTask.setContinuation(continuation);
+	
+	mSpeculativeThirdPassTask.setContinuation(&mPostThirdPassTask);
+	mAccurateThirdPassTask.setContinuation(&mPostThirdPassTask);
+
+	mSpeculativeThirdPassTask.removeReference();
+	mAccurateThirdPassTask.removeReference();
+
+	mPostThirdPassTask.removeReference();
+
+	//PX_PROFILE_ZONE("Basic.thirdPassIslandGen", getContextId());
+	//mSpeculativeIslandManager.removeDestroyedEdges();
+	//mSpeculativeIslandManager.processLostEdges(mDestroyedNodes, true, true);
+
+	//mIslandManager.removeDestroyedEdges();
+	//mIslandManager.processLostEdges(mDestroyedNodes, true, true);
+
+	
+}
+
+bool SimpleIslandManager::checkInternalConsistency()
+{
+	return mIslandManager.checkInternalConsistency() && mSpeculativeIslandManager.checkInternalConsistency();
+}
+
+void SimpleIslandManager::clearDestroyedEdges()
+{
+	mDestroyedPartitionEdges.forceSize_Unsafe(0);
+}
+
+void SimpleIslandManager::setEdgeConnected(EdgeIndex edgeIndex)
+{
+	if(!mConnectedMap.test(edgeIndex))
+	{
+		mIslandManager.addContactManager(mConstraintOrCm[edgeIndex].mCm, mEdgeNodeIndices[edgeIndex*2], mEdgeNodeIndices[edgeIndex*2+1], edgeIndex);
+		mConnectedMap.set(edgeIndex);
+	}
+}
+
+bool SimpleIslandManager::getIsEdgeConnected(EdgeIndex edgeIndex)
+{
+	return !!mConnectedMap.test(edgeIndex);
+}
+
+void SimpleIslandManager::deactivateEdge(const EdgeIndex edgeIndex)
+{
+	if (mFirstPartitionEdges[edgeIndex])
+	{
+		mDestroyedPartitionEdges.pushBack(mFirstPartitionEdges[edgeIndex]);
+		mFirstPartitionEdges[edgeIndex] = NULL;
+	}
+}
+
+void SimpleIslandManager::setEdgeDisconnected(EdgeIndex edgeIndex)
+{
+	if(mConnectedMap.test(edgeIndex))
+	{
+		//PX_ASSERT(!mIslandManager.getEdge(edgeIndex).isInDirtyList());
+		mIslandManager.removeConnection(edgeIndex);
+		mConnectedMap.reset(edgeIndex);
+	}
+}
+
+void SimpleIslandManager::setEdgeRigidCM(const EdgeIndex edgeIndex, PxsContactManager* cm)
+{
+	mConstraintOrCm[edgeIndex].mCm = cm;
+	cm->getWorkUnit().mEdgeIndex = edgeIndex;
+}
+
+void SimpleIslandManager::clearEdgeRigidCM(const EdgeIndex edgeIndex)
+{
+	mConstraintOrCm[edgeIndex].mCm = NULL;
+	if (mFirstPartitionEdges[edgeIndex])
+	{
+		//this is the partition edges created/updated by the gpu solver
+		mDestroyedPartitionEdges.pushBack(mFirstPartitionEdges[edgeIndex]);
+		mFirstPartitionEdges[edgeIndex] = NULL;
+	}
+}
+
+void SimpleIslandManager::setKinematic(IG::NodeIndex nodeIndex) 
+{ 
+	mIslandManager.setKinematic(nodeIndex); 
+	mSpeculativeIslandManager.setKinematic(nodeIndex);
+}
+
+void SimpleIslandManager::setDynamic(IG::NodeIndex nodeIndex) 
+{ 
+	mIslandManager.setDynamic(nodeIndex); 
+	mSpeculativeIslandManager.setDynamic(nodeIndex);
+}
+
+}
+}
+