Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 952 insertions, 0 deletions

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);
+}
+