PhysX 3.4, APEX 1.4 patch release @24698370

8 files changed, 438 insertions, 88 deletions

diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp
index c43c20cb..313e27f3 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp
@@ -3336,20 +3336,22 @@ void BroadPhaseMBP::update(const PxU32 numCpuTasks, PxcScratchAllocator* scratch
 
 	setUpdateData(updateData);
 
-	mMBPPostUpdateWorkTask.setScratchAllocator(scratchAllocator);
-	mMBPUpdateWorkTask.setScratchAllocator(scratchAllocator);
-
-	mMBPPostUpdateWorkTask.setBroadphase(this);
-	mMBPUpdateWorkTask.setBroadphase(this);
-
-	mMBPPostUpdateWorkTask.setNumCpuTasks(numCpuTasks);
-	mMBPUpdateWorkTask.setNumCpuTasks(numCpuTasks);
+	if(1)
+	{
+		update();
+		postUpdate();
+	}
+	else
+	{
+		mMBPPostUpdateWorkTask.set(this, scratchAllocator, numCpuTasks);
+		mMBPUpdateWorkTask.set(this, scratchAllocator, numCpuTasks);
 
-	mMBPPostUpdateWorkTask.setContinuation(continuation);
-	mMBPUpdateWorkTask.setContinuation(&mMBPPostUpdateWorkTask);
+		mMBPPostUpdateWorkTask.setContinuation(continuation);
+		mMBPUpdateWorkTask.setContinuation(&mMBPPostUpdateWorkTask);
 
-	mMBPPostUpdateWorkTask.removeReference();
-	mMBPUpdateWorkTask.removeReference();
+		mMBPPostUpdateWorkTask.removeReference();
+		mMBPUpdateWorkTask.removeReference();
+	}
 }
 
 static PX_FORCE_INLINE void computeMBPBounds(MBP_AABB& aabb, const PxBounds3* PX_RESTRICT boundsXYZ, const PxReal* PX_RESTRICT contactDistances, const BpHandle index)
@@ -3413,6 +3415,16 @@ static PX_FORCE_INLINE void computeMBPBounds(MBP_AABB& aabb, const PxBounds3* PX
 #endif*/
 }
 
+void MBPUpdateWorkTask::runInternal()
+{
+	mMBP->update();
+}
+
+void MBPPostUpdateWorkTask::runInternal()
+{
+	mMBP->postUpdate();
+}
+
 void BroadPhaseMBP::removeObjects(const BroadPhaseUpdateData& updateData)
 {
 	const BpHandle* PX_RESTRICT removed = updateData.getRemovedHandles();
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h
index eb302c2e..08f6e771 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h
@@ -41,8 +41,6 @@ namespace physx
 {
 namespace Bp
 {
-	class BPDefaultMemoryAllocator;
-
 	class BroadPhaseMBP : public BroadPhase, public Ps::UserAllocated
 	{
 											PX_NOCOPY(BroadPhaseMBP)
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp
index d8aa0b54..e295ca78 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp
@@ -622,7 +622,7 @@ bool BroadPhaseSap::setUpdateData(const BroadPhaseUpdateData& updateData)
 	return true;
 }
 
-void BroadPhaseSap::postUpdate(PxBaseTask* /*continuation*/)
+void BroadPhaseSap::postUpdate()
 {
 	PX_PROFILE_ZONE("BroadPhase.SapPostUpdate", mContextID);
 
@@ -678,7 +678,7 @@ void BroadPhaseBatchUpdateWorkTask::runInternal()
 	mSap->batchUpdate(mAxis, mPairs, mPairsSize, mPairsCapacity);
 }
 
-void BroadPhaseSap::update(PxBaseTask* /*continuation*/)
+void BroadPhaseSap::update()
 {
 	PX_PROFILE_ZONE("BroadPhase.SapUpdate", mContextID);
 
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h
index eeee3651..4ec745db 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h
@@ -200,8 +200,8 @@ private:
 			PxU32						mActualDeletedPairSize;
 
 			bool						setUpdateData(const BroadPhaseUpdateData& updateData);
-			void						update(physx::PxBaseTask* continuation);
-			void						postUpdate(physx::PxBaseTask* continuation);
+			void						update();
+			void						postUpdate();
 
 	//Batch create/remove/update.
 			void						batchCreate();
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp
index 6781ea48..517e2dc6 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp
@@ -27,37 +27,3 @@
 // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
 // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
 
-#include "BpMBPTasks.h"
-#include "BpBroadPhaseMBP.h"
-#include "PsAllocator.h"
-
-using namespace physx;
-using namespace Bp;
-
-///////////////////////////////////////////////////////////////////////////////
-
-MBPUpdateWorkTask::MBPUpdateWorkTask(PxU64 contextId) : MBPTask(contextId)
-{
-}
-
-MBPUpdateWorkTask::~MBPUpdateWorkTask()
-{
-}
-
-MBPPostUpdateWorkTask::MBPPostUpdateWorkTask(PxU64 contextId) : MBPTask(contextId)
-{
-}
-
-void MBPUpdateWorkTask::runInternal()
-{
-	mMBP->update();
-}
-
-///////////////////////////////////////////////////////////////////////////////
-
-void MBPPostUpdateWorkTask::runInternal()
-{
-	mMBP->postUpdate();
-}
-
-///////////////////////////////////////////////////////////////////////////////
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h
index 742e0d29..8e353dbf 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h
@@ -47,22 +47,18 @@ namespace physx
 	class MBPTask : public Cm::Task, public shdfnd::UserAllocated
 	{
 		public:
-												MBPTask(PxU64 contextId) :
-												Cm::Task		(contextId),
-												mMBP			(NULL),
-												mNumCpuTasks	(0)
-												{}
-
-		PX_FORCE_INLINE	void					setBroadphase(Bp::BroadPhaseMBP* mbp)			{ mMBP = mbp;					}
-		PX_FORCE_INLINE	void					setScratchAllocator(PxcScratchAllocator* sa)	{ mScratchAllocator = sa;		}
-		PX_FORCE_INLINE void					setNumCpuTasks(const PxU32 numCpuTasks)			{ mNumCpuTasks = numCpuTasks;	}
-
+												MBPTask(PxU64 contextId) : Cm::Task(contextId), mMBP(NULL), mNumCpuTasks(0)		{}
+
+		PX_FORCE_INLINE	void					set(Bp::BroadPhaseMBP* mbp, PxcScratchAllocator* sa, PxU32 numCpuTasks)
+												{
+													mMBP = mbp;
+													mScratchAllocator = sa;
+													mNumCpuTasks = numCpuTasks;
+												}
 		protected:
 						Bp::BroadPhaseMBP*		mMBP;
 						PxU32					mNumCpuTasks;
-
 						PxcScratchAllocator*	mScratchAllocator;
-
 		private:
 		MBPTask& operator=(const MBPTask&);
 	};
@@ -71,8 +67,8 @@ namespace physx
 	class MBPUpdateWorkTask : public MBPTask
 	{
 	public:							
-								MBPUpdateWorkTask(PxU64 contextId);
-								~MBPUpdateWorkTask();
+								MBPUpdateWorkTask(PxU64 contextId) : MBPTask(contextId)	{}
+								~MBPUpdateWorkTask()									{}
 		// PxBaseTask
 		virtual const char*		getName() const { return "BpMBP.updateWork"; }
 		//~PxBaseTask
@@ -90,8 +86,8 @@ namespace physx
 	class MBPPostUpdateWorkTask : public MBPTask
 	{
 	public:
-								MBPPostUpdateWorkTask(PxU64 contextId);					
-
+								MBPPostUpdateWorkTask(PxU64 contextId) : MBPTask(contextId)	{}
+								~MBPPostUpdateWorkTask()									{}
 		// PxBaseTask
 		virtual const char*		getName() const { return "BpMBP.postUpdateWork"; }
 		//~PxBaseTask
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp
index ef6dee35..3584154d 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp
@@ -52,12 +52,12 @@ namespace Bp
 
 void SapUpdateWorkTask::runInternal()
 {
-	mSAP->update(getContinuation());
+	mSAP->update();
 }
 
 void SapPostUpdateWorkTask::runInternal()
 {
-	mSAP->postUpdate(getContinuation());
+	mSAP->postUpdate();
 #ifdef DUMP_TOTAL_SAP_TIME
 	PxU64 endTime = shdfnd::Time::getCurrentCounterValue();
 	printf("SAP Time: %" PX_PRIu64 "\n", endTime - gStartTime);
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp
index 0dd4cc01..2e0bc7e0 100644
--- a/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp
@@ -189,6 +189,7 @@ static void resetOrClear(T& a)
 															return mRS.GetRanks();
 														}
 #endif
+
 						PxBounds3						mBounds;
 		private:
 #ifndef STORE_SORTED_BOUNDS
@@ -596,7 +597,7 @@ class PersistentPairs : public Ps::UserAllocated
 										mShouldBeDeleted(false)					{}
 	virtual							~PersistentPairs()							{}
 
-	virtual			bool			update(SimpleAABBManager& /*manager*/)		{ return false; }
+	virtual			bool			update(SimpleAABBManager& /*manager*/, BpCacheData* /*data*/ = NULL)		{ return false; }
 
 	PX_FORCE_INLINE	void			updatePairs(PxU32 timestamp, const PxBounds3* bounds, const float* contactDistances, const Bp::FilterGroup::Enum* groups,
 #ifdef BP_FILTERING_USES_TYPE_IN_GROUP
@@ -883,7 +884,7 @@ class PersistentActorAggregatePair : public PersistentPairs
 	, const bool* PX_RESTRICT lut
 #endif
 		);
-	virtual			bool		update(SimpleAABBManager& manager);
+	virtual			bool		update(SimpleAABBManager& manager, BpCacheData* data);
 
 					ShapeHandle	mAggregateHandle;
 					ShapeHandle	mActorHandle;
@@ -1013,7 +1014,7 @@ void PersistentActorAggregatePair::findOverlaps(PairArray& pairs, const PxBounds
 	}
 }
 
-bool PersistentActorAggregatePair::update(SimpleAABBManager& manager)
+bool PersistentActorAggregatePair::update(SimpleAABBManager& manager, BpCacheData* data)
 {
 	if(mShouldBeDeleted || shouldPairBeDeleted(manager.mGroups, mAggregateHandle, mActorHandle))
 		return true;
@@ -1022,7 +1023,7 @@ bool PersistentActorAggregatePair::update(SimpleAABBManager& manager)
 		return true;
 
 	if(mAggregate->isDirty() || manager.mChangedHandleMap.boundedTest(mActorHandle))
-		manager.updatePairs(*this);
+		manager.updatePairs(*this, data);
 
 	return false;
 }
@@ -1040,7 +1041,7 @@ class PersistentAggregateAggregatePair : public PersistentPairs
 	, const bool* PX_RESTRICT lut
 #endif
 		);
-	virtual			bool		update(SimpleAABBManager& manager);
+	virtual			bool		update(SimpleAABBManager& manager, BpCacheData*);
 
 					ShapeHandle	mAggregateHandle0;
 					ShapeHandle	mAggregateHandle1;
@@ -1183,7 +1184,7 @@ void PersistentAggregateAggregatePair::findOverlaps(PairArray& pairs, const PxBo
 	}
 }
 
-bool PersistentAggregateAggregatePair::update(SimpleAABBManager& manager)
+bool PersistentAggregateAggregatePair::update(SimpleAABBManager& manager, BpCacheData* data)
 {
 	if(mShouldBeDeleted || shouldPairBeDeleted(manager.mGroups, mAggregateHandle0, mAggregateHandle1))
 		return true;
@@ -1192,7 +1193,7 @@ bool PersistentAggregateAggregatePair::update(SimpleAABBManager& manager)
 		return true;
 
 	if(mAggregate0->isDirty() || mAggregate1->isDirty())
-		manager.updatePairs(*this);
+		manager.updatePairs(*this, data);
 
 	return false;
 }
@@ -1584,6 +1585,8 @@ void SimpleAABBManager::reserveShapeSpace(PxU32 nbTotalBounds)
 SimpleAABBManager::SimpleAABBManager(	BroadPhase& bp, BoundsArray& boundsArray, Ps::Array<PxReal, Ps::VirtualAllocator>& contactDistance,
 										PxU32 maxNbAggregates, PxU32 maxNbShapes, Ps::VirtualAllocator& allocator, PxU64 contextID,
 										PxPairFilteringMode::Enum kineKineFilteringMode, PxPairFilteringMode::Enum staticKineFilteringMode) :
+	mPostBroadPhase2			(contextID, *this),
+	mPostBroadPhase3			(contextID, this, "SimpleAABBManager::postBroadPhaseStage3"),
 	mFinalizeUpdateTask			(contextID),
 	mChangedHandleMap			(allocator),
 	mGroups						(allocator),
@@ -1673,6 +1676,14 @@ void SimpleAABBManager::destroy()
 		}
 	}
 
+	BpCacheData* entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+	while (entry)
+	{
+		entry->~BpCacheData();
+		PX_FREE(entry);
+		entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+	}
+
 	PX_DELETE(this);
 }
 
@@ -2412,13 +2423,24 @@ PersistentAggregateAggregatePair* SimpleAABBManager::createPersistentAggregateAg
 	return PX_NEW(PersistentAggregateAggregatePair)(aggregate0, aggregate1);	// PT: TODO: use a pool or something
 }
 
-void SimpleAABBManager::updatePairs(PersistentPairs& p)
+void SimpleAABBManager::updatePairs(PersistentPairs& p, BpCacheData* data)
 {
+	if (data)
+	{
 #ifdef BP_FILTERING_USES_TYPE_IN_GROUP
-	p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), &mLUT[0][0], mVolumeData, mCreatedOverlaps, mDestroyedOverlaps);
+		p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), &mLUT[0][0], mVolumeData, data->mCreatedPairs, data->mDeletedPairs);
 #else
-	p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), mVolumeData, mCreatedOverlaps, mDestroyedOverlaps);
+		p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), mVolumeData, data->mCreatedPairs, data->mDeletedPairs);
 #endif
+	}
+	else
+	{
+#ifdef BP_FILTERING_USES_TYPE_IN_GROUP
+		p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), &mLUT[0][0], mVolumeData, mCreatedOverlaps, mDestroyedOverlaps);
+#else
+		p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), mVolumeData, mCreatedOverlaps, mDestroyedOverlaps);
+#endif
+	}
 }
 
 void SimpleAABBManager::processBPCreatedPair(const BroadPhasePair& pair)
@@ -2566,10 +2588,222 @@ static void processAggregatePairs(AggPairMap& map, SimpleAABBManager& manager)
 	}
 }
 
-void SimpleAABBManager::postBroadPhase(PxBaseTask*, PxBaseTask* narrowPhaseUnlockTask)
+struct PairData
+{
+	Ps::Array<AABBOverlap>* mArray;
+	PxU32 mStartIdx;
+	PxU32 mCount;
+
+	PairData() : mArray(NULL), mStartIdx(0), mCount(0)
+	{
+	}
+};
+
+class ProcessAggPairsBase : public Cm::Task
+{
+public:
+	static const PxU32 MaxPairs = 16;
+
+	PairData mCreatedPairs[2];
+	PairData mDestroyedPairs[2];
+
+	ProcessAggPairsBase(PxU64 contextID) : Cm::Task(contextID)
+	{
+	}
+
+	void setCache(BpCacheData& data)
+	{
+		for (PxU32 i = 0; i < 2; ++i)
+		{
+			mCreatedPairs[i].mArray = &data.mCreatedPairs[i];
+			mCreatedPairs[i].mStartIdx = data.mCreatedPairs[i].size();
+			mDestroyedPairs[i].mArray = &data.mDeletedPairs[i];
+			mDestroyedPairs[i].mStartIdx = data.mDeletedPairs[i].size();
+		}
+	}
+
+	void updateCounters()
+	{
+		for (PxU32 i = 0; i < 2; ++i)
+		{
+			mCreatedPairs[i].mCount = mCreatedPairs[i].mArray->size() - mCreatedPairs[i].mStartIdx;
+			mDestroyedPairs[i].mCount = mDestroyedPairs[i].mArray->size() - mDestroyedPairs[i].mStartIdx;
+		}
+	}
+};
+
+
+class ProcessAggPairsParallelTask : public ProcessAggPairsBase
+{
+public:
+	PersistentPairs* mPersistentPairs[MaxPairs];
+	Bp::AggPair mAggPairs[MaxPairs];
+	PxU32 mNbPairs;
+	Bp::SimpleAABBManager* mManager;
+	AggPairMap* mMap;
+	Ps::Mutex* mMutex;
+	const char* mName;
+
+	ProcessAggPairsParallelTask(PxU64 contextID, Ps::Mutex* mutex, Bp::SimpleAABBManager* manager, AggPairMap* map, const char* name) : ProcessAggPairsBase(contextID),
+		mNbPairs(0), mManager(manager), mMap(map), mMutex(mutex), mName(name)
+	{
+	}
+
+
+	void runInternal()
+	{
+		BpCacheData* data = mManager->getBpCacheData();
+
+		setCache(*data);
+
+
+		Ps::InlineArray<AggPair, MaxPairs> removedEntries;
+		for (PxU32 i = 0; i < mNbPairs; ++i)
+		{
+			if (mPersistentPairs[i]->update(*mManager, data))
+			{
+				removedEntries.pushBack(mAggPairs[i]);
+				PX_DELETE(mPersistentPairs[i]);
+			}
+		}
+
+		updateCounters();
+
+		mManager->putBpCacheData(data);
+
+
+		if (removedEntries.size())
+		{
+			Ps::Mutex::ScopedLock lock(*mMutex);
+			for (PxU32 i = 0; i < removedEntries.size(); i++)
+			{
+				bool status = mMap->erase(removedEntries[i]);
+				PX_ASSERT(status);
+				PX_UNUSED(status);
+			}
+		}
+
+		
+	}
+
+	virtual const char* getName() const { return mName; }
+
+};
+
+class SortAggregateBoundsParallel : public Cm::Task
+{
+public:
+	static const PxU32 MaxPairs = 16;
+	Aggregate** mAggregates;
+	PxU32 mNbAggs;
+	Bp::SimpleAABBManager* mManager;
+
+	SortAggregateBoundsParallel(PxU64 contextID, Aggregate** aggs, PxU32 nbAggs) : Cm::Task(contextID),
+		mAggregates(aggs), mNbAggs(nbAggs)
+	{
+	}
+
+
+	void runInternal()
+	{
+		PX_PROFILE_ZONE("SortBounds", mContextID);
+		for (PxU32 i = 0; i < mNbAggs; i++)
+		{
+			Aggregate* aggregate = mAggregates[i];
+
+			aggregate->getSortedMinBounds();
+		}
+	}
+
+	virtual const char* getName() const { return "SortAggregateBoundsParallel"; }
+
+};
+
+
+class ProcessSelfCollisionPairsParallel : public ProcessAggPairsBase
+{
+public:
+	Aggregate** mAggregates;
+	PxU32 mNbAggs;
+	Bp::SimpleAABBManager* mManager;
+
+	ProcessSelfCollisionPairsParallel(PxU64 contextID, Aggregate** aggs, PxU32 nbAggs, SimpleAABBManager* manager) : ProcessAggPairsBase(contextID),
+		mAggregates(aggs), mNbAggs(nbAggs), mManager(manager)
+	{
+	}
+
+
+	void runInternal()
+	{
+		BpCacheData* data = mManager->getBpCacheData();
+		setCache(*data);
+		PX_PROFILE_ZONE("ProcessSelfCollisionPairs", mContextID);
+		for (PxU32 i = 0; i < mNbAggs; i++)
+		{
+			Aggregate* aggregate = mAggregates[i];
+
+			if (aggregate->mSelfCollisionPairs)
+				mManager->updatePairs(*aggregate->mSelfCollisionPairs, data);
+		}
+		updateCounters();
+		mManager->putBpCacheData(data);
+	}
+
+	virtual const char* getName() const { return "ProcessSelfCollisionPairsParallel"; }
+
+};
+
+static void processAggregatePairsParallel(AggPairMap& map, SimpleAABBManager& manager, Cm::FlushPool& flushPool,
+	PxBaseTask* continuation, const char* taskName, Ps::Array<ProcessAggPairsBase*>& pairTasks)
+{
+	// PT: TODO: hmmm we have a list of dirty aggregates but we don't have a list of dirty pairs.
+	// PT: not sure how the 3.4 trunk solves this but let's just iterate all pairs for now
+	// PT: atm we reuse this loop to delete removed interactions
+	// PT: TODO: in fact we could handle all the "lost pairs" stuff right there with extra aabb-abb tests
+
+	// PT: TODO: replace with decent hash map - or remove the hashmap entirely and use a linear array
+
+	manager.mMapLock.lock();
+
+	ProcessAggPairsParallelTask* task = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(ProcessAggPairsParallelTask)), ProcessAggPairsParallelTask)(0, &manager.mMapLock, &manager, &map, taskName);
+
+	for (AggPairMap::Iterator iter = map.getIterator(); !iter.done(); ++iter)
+	{
+		task->mAggPairs[task->mNbPairs] = iter->first;
+		task->mPersistentPairs[task->mNbPairs++] = iter->second;
+		if (task->mNbPairs == ProcessAggPairsParallelTask::MaxPairs)
+		{
+			pairTasks.pushBack(task);
+			task->setContinuation(continuation);
+			task->removeReference();
+			//task->runInternal();
+			task = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(ProcessAggPairsParallelTask)), ProcessAggPairsParallelTask)(0, &manager.mMapLock, &manager, &map, taskName);
+		}
+	}
+
+	manager.mMapLock.unlock();
+
+	if (task->mNbPairs)
+	{
+		pairTasks.pushBack(task);
+		task->setContinuation(continuation);
+		task->removeReference();
+		//task->runInternal();
+	}
+}
+
+void SimpleAABBManager::postBroadPhase(PxBaseTask* continuation, PxBaseTask* narrowPhaseUnlockTask, Cm::FlushPool& flushPool)
 {
 	PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase", getContextId());
 
+	//KS - There is a continuation task for discrete broad phase, but not for CCD broad phase. PostBroadPhase for CCD broad phase runs in-line.
+	//This probably should be revisited but it means that we can't use the parallel codepath within CCD.
+	if (continuation)
+	{
+		mPostBroadPhase3.setContinuation(continuation);
+		mPostBroadPhase2.setContinuation(&mPostBroadPhase3);
+	}
+
 	mTimestamp++;
 
 	// PT: TODO: consider merging mCreatedOverlaps & mDestroyedOverlaps
@@ -2592,28 +2826,141 @@ void SimpleAABBManager::postBroadPhase(PxBaseTask*, PxBaseTask* narrowPhaseUnloc
 	}
 
 	{
+		//If there is a continuation task, then this is not part of CCD, so we can trigger bounds to be recomputed in parallel before pair generation runs during
+		//stage 2.			
+		if (continuation)
+		{
+			const PxU32 size = mDirtyAggregates.size();
+			for (PxU32 i = 0; i < size; i += SortAggregateBoundsParallel::MaxPairs)
+			{
+				const PxU32 nbToProcess = PxMin(size - i, SortAggregateBoundsParallel::MaxPairs);
+
+				SortAggregateBoundsParallel* task = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(SortAggregateBoundsParallel)), SortAggregateBoundsParallel)
+					(mContextID, &mDirtyAggregates[i], nbToProcess);
+
+				task->setContinuation(&mPostBroadPhase2);
+				task->removeReference();
+			}
+		}
+	}
+
+	
+
+	if (continuation)
+	{
+		mPostBroadPhase2.setFlushPool(&flushPool);
+		mPostBroadPhase3.removeReference();
+		mPostBroadPhase2.removeReference();
+	}
+	else
+	{
+		postBpStage2(NULL, flushPool);
+		postBpStage3(NULL);
+	}
+
+}
+
+void PostBroadPhaseStage2Task::runInternal()
+{
+	mManager.postBpStage2(mCont, *mFlushPool);
+}
+
+void SimpleAABBManager::postBpStage2(PxBaseTask* continuation, Cm::FlushPool& flushPool)
+{
+	{
+		{
+			const PxU32 size = mDirtyAggregates.size();
+			for (PxU32 i = 0; i < size; i += ProcessSelfCollisionPairsParallel::MaxPairs)
+			{
+				const PxU32 nbToProcess = PxMin(size - i, ProcessSelfCollisionPairsParallel::MaxPairs);
+
+				ProcessSelfCollisionPairsParallel* task = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(ProcessSelfCollisionPairsParallel)), ProcessSelfCollisionPairsParallel)
+					(mContextID, &mDirtyAggregates[i], nbToProcess, this);
+				if (continuation)
+				{
+					task->setContinuation(continuation);
+					task->removeReference();
+				}
+				else
+					task->runInternal();
+				mAggPairTasks.pushBack(task);
+			}
+		}
+
 		{
-			PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase - process actor-aggregate pairs", getContextId());
-			processAggregatePairs(mActorAggregatePairs, *this);
+			if (continuation)
+				processAggregatePairsParallel(mAggregateAggregatePairs, *this, flushPool, continuation, "AggAggPairs", mAggPairTasks);
+			else
+				processAggregatePairs(mAggregateAggregatePairs, *this);
 		}
+
+
+
 		{
-			PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase - process aggregate pairs", getContextId());
-			processAggregatePairs(mAggregateAggregatePairs, *this);
+			if (continuation)
+				processAggregatePairsParallel(mActorAggregatePairs, *this, flushPool, continuation, "AggActorPairs", mAggPairTasks);
+			else
+				processAggregatePairs(mActorAggregatePairs, *this);
 		}
 
+
+	}
+}
+
+void SimpleAABBManager::postBpStage3(PxBaseTask*)
+{
+
+	{
 		{
 			PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase - aggregate self-collisions", getContextId());
 			const PxU32 size = mDirtyAggregates.size();
-			for(PxU32 i=0;i<size;i++)
+			for (PxU32 i = 0; i < size; i++)
 			{
 				Aggregate* aggregate = mDirtyAggregates[i];
 				aggregate->resetDirtyState();
 
-				if(aggregate->mSelfCollisionPairs)
-					updatePairs(*aggregate->mSelfCollisionPairs);
 			}
 			resetOrClear(mDirtyAggregates);
 		}
+		
+		{
+			PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase - append pairs", getContextId());
+
+			for (PxU32 a = 0; a < mAggPairTasks.size(); ++a)
+			{
+				ProcessAggPairsBase* task = mAggPairTasks[a];
+				for (PxU32 t = 0; t < 2; t++)
+				{
+					for (PxU32 i = 0, startIdx = task->mCreatedPairs[t].mStartIdx; i < task->mCreatedPairs[t].mCount; ++i)
+					{
+						mCreatedOverlaps[t].pushBack((*task->mCreatedPairs[t].mArray)[i+ startIdx]);
+					}
+					for (PxU32 i = 0, startIdx = task->mDestroyedPairs[t].mStartIdx; i < task->mDestroyedPairs[t].mCount; ++i)
+					{
+						mDestroyedOverlaps[t].pushBack((*task->mDestroyedPairs[t].mArray)[i + startIdx]);
+					}
+				}
+			}
+
+			mAggPairTasks.forceSize_Unsafe(0);
+
+
+			Ps::InlineArray<BpCacheData*, 16> bpCache;
+			BpCacheData* entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+
+			while (entry)
+			{
+				entry->reset();
+				bpCache.pushBack(entry);
+				entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+			}
+
+			//Now reinsert back into queue...
+			for (PxU32 i = 0; i < bpCache.size(); ++i)
+			{
+				mBpThreadContextPool.push(*bpCache[i]);
+			}
+		}
 	}
 
 	{
@@ -2708,6 +3055,37 @@ void SimpleAABBManager::shiftOrigin(const PxVec3& shift)
 	mOriginShifted = true;
 }
 
+BpCacheData* SimpleAABBManager::getBpCacheData()
+{
+	BpCacheData* rv = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+	if (rv == NULL)
+	{
+		rv = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(BpCacheData), PX_DEBUG_EXP("BpCacheData")), BpCacheData)();
+	}
+	return rv;
+}
+void SimpleAABBManager::putBpCacheData(BpCacheData* data)
+{
+	mBpThreadContextPool.push(*data);
+}
+void SimpleAABBManager::resetBpCacheData()
+{
+	Ps::InlineArray<BpCacheData*, 16> bpCache;
+	BpCacheData* entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+	while (entry)
+	{
+		entry->reset();
+		bpCache.pushBack(entry);
+		entry = static_cast<BpCacheData*>(mBpThreadContextPool.pop());
+	}
+
+	//Now reinsert back into queue...
+	for (PxU32 i = 0; i < bpCache.size(); ++i)
+	{
+		mBpThreadContextPool.push(*bpCache[i]);
+	}
+}
+
 // PT: disabled this by default, since it bypasses all per-shape/per-actor visualization flags
 //static const bool gVisualizeAggregateElems = false;