Skip to content

A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.

25.7 KB
Raw Blame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0

#include "common/PxProfileZone.h"
#include "ScBodySim.h"
#include "ScShapeSim.h"
#include "ScArticulationSim.h"
#include "ScArticulationCore.h"
#include "DyIslandManager.h"

using namespace physx;
using namespace Sc;
using namespace Cm;

///////////////////////////////////////////////////////////////////////////////

void BodySim::addToSpeculativeCCDMap()
{
	if(mNodeIndex.isValid())
	{
		if(isArticulationLink())
			mScene.setSpeculativeCCDArticulationLink(mNodeIndex.index());
		else
			mScene.setSpeculativeCCDRigidBody(mNodeIndex.index());
	}
}

void BodySim::removeFromSpeculativeCCDMap()
{
	if(mNodeIndex.isValid())
	{
		if(isArticulationLink())
			mScene.resetSpeculativeCCDArticulationLink(mNodeIndex.index());
		else
			mScene.resetSpeculativeCCDRigidBody(mNodeIndex.index());
	}
}

// PT: TODO: consider using a non-member function for this one
void BodySim::updateContactDistance(PxReal* contactDistance, PxReal dt, const Bp::BoundsArray& boundsArray)
{
	const PxsRigidBody& llBody = getLowLevelBody();

	const PxRigidBodyFlags flags = llBody.getCore().mFlags;

	// PT: TODO: no need to test eENABLE_SPECULATIVE_CCD if we parsed mSpeculativeCCDRigidBodyBitMap initially 
	if((flags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD) && !(llBody.mInternalFlags & PxsRigidBody::eFROZEN))
	{
		// PT: if both CCD flags are enabled we're in "hybrid mode" and we only use speculative contacts for the angular part
		const PxReal linearInflation = (flags & PxRigidBodyFlag::eENABLE_CCD) ? 0.0f : llBody.getLinearVelocity().magnitude() * dt;

		const float angVelMagTimesDt = llBody.getAngularVelocity().magnitude() * dt;

		PxU32 nbElems = getNbElements();
		ElementSim** elems = getElements();
		while(nbElems--)
		{
			ShapeSim* current = static_cast<ShapeSim*>(*elems++);

			const PxU32 index = current->getElementID();

			const PxBounds3& bounds = boundsArray.getBounds(index);

			const PxReal radius = bounds.getExtents().magnitude();

			//Heuristic for angular velocity...
			const PxReal angularInflation = angVelMagTimesDt * radius;

			contactDistance[index] = linearInflation + current->getContactOffset() + angularInflation;
		}
	}
}

static void updateLinksContactDistances(Sc::ArticulationSim* articulation, PxReal* contactDistance, PxReal dt, const Bp::BoundsArray& boundsArray)
{
	BodySim** bodies = articulation->getBodies();
	const PxU32 size = articulation->getNbBodies();
	for(PxU32 i=0; i<size; i++)
		bodies[i]->updateContactDistance(contactDistance, dt, boundsArray);
}

namespace
{
class SpeculativeCCDBaseTask : public Cm::Task
{
	PX_NOCOPY(SpeculativeCCDBaseTask)
	public:
	const Bp::BoundsArray&	mBoundsArray;
	float*					mContactDistances;
	const float				mDt;

	SpeculativeCCDBaseTask(PxU64 contextID, const Bp::BoundsArray& boundsArray, PxReal* contactDistances, PxReal dt) :
		Cm::Task			(contextID),
		mBoundsArray		(boundsArray),
		mContactDistances	(contactDistances),
		mDt					(dt)
	{}
};

class SpeculativeCCDContactDistanceUpdateTask : public SpeculativeCCDBaseTask
{
public:
	static const PxU32 MaxBodies = 128;
	BodySim*	mBodySims[MaxBodies];
	PxU32		mNbBodies;

	SpeculativeCCDContactDistanceUpdateTask(PxU64 contextID, PxReal* contactDistances, PxReal dt, const Bp::BoundsArray& boundsArray) :
		SpeculativeCCDBaseTask	(contextID, boundsArray, contactDistances, dt),
		mNbBodies				(0)
	{}

	virtual void runInternal() PX_OVERRIDE
	{
		const PxU32 nb = mNbBodies;
		for(PxU32 i=0; i<nb; i++)
			mBodySims[i]->updateContactDistance(mContactDistances, mDt, mBoundsArray);
	}

	virtual const char* getName() const PX_OVERRIDE { return "SpeculativeCCDContactDistanceUpdateTask"; }

private:
	PX_NOCOPY(SpeculativeCCDContactDistanceUpdateTask)
};

class SpeculativeCCDContactDistanceArticulationUpdateTask : public SpeculativeCCDBaseTask
{
public:
	ArticulationSim* mArticulation;

	SpeculativeCCDContactDistanceArticulationUpdateTask(PxU64 contextID, PxReal* contactDistances, PxReal dt, const Bp::BoundsArray& boundsArray, ArticulationSim* sim) :
		SpeculativeCCDBaseTask	(contextID, boundsArray, contactDistances, dt),
		mArticulation			(sim)
	{}

	virtual void runInternal() PX_OVERRIDE
	{
		updateLinksContactDistances(mArticulation, mContactDistances, mDt, mBoundsArray);
	}

	virtual const char* getName() const PX_OVERRIDE { return "SpeculativeCCDContactDistanceArticulationUpdateTask"; }

private:
	PX_NOCOPY(SpeculativeCCDContactDistanceArticulationUpdateTask)
};
}

static SpeculativeCCDContactDistanceUpdateTask* createCCDTask(Cm::FlushPool& pool, PxU64 contextID, PxReal* contactDistances, PxReal dt, const Bp::BoundsArray& boundsArray)
{
	return PX_PLACEMENT_NEW(pool.allocate(sizeof(SpeculativeCCDContactDistanceUpdateTask)), SpeculativeCCDContactDistanceUpdateTask)(contextID, contactDistances, dt, boundsArray);
}

void Sc::Scene::updateContactDistances(PxBaseTask* continuation)
{
	PX_PROFILE_ZONE("Scene.updateContactDistances", mContextId);
	
	Cm::FlushPool& pool = mLLContext->getTaskPool();
	IG::IslandSim& islandSim = mSimpleIslandManager->getAccurateIslandSim();
	bool hasContactDistanceChanged = mHasContactDistanceChanged;

	// PT: TODO: it is quite unfortunate that we cannot shortcut parsing the bitmaps. Consider switching to arrays.
	// We remove sleeping bodies from the map but we never shrink it....
	// We use a bitmap because we remove sleeping bodies from the structure in BodySim::deactivate(), i.e. the lookup
	// must be fast (we cannot just use an O(n) search on PxArray). We could use an intrusive index in BodySim though.

	// PT: why do we do that on sleeping bodies? Why don't we use mActiveBodies?
	// PxArray<BodyCore*>			mActiveBodies;				// Sorted: kinematic before dynamic
	// ===> because that array contains all bodies, even the ones that do not have CCD enabled.
	// That array is already sorted though, so we could organize it in 3 parts instead of 2 (like we did with the old
	// "pruning sections" from SQ in ~PhysX2). But it is unclear whether this is worth the extra complexity.

	// PT: TODO: why do we need to involve the island manager here?

	//calculate contact distance for speculative CCD shapes
	if(1)
	{
		PxBitMap::Iterator speculativeCCDIter(mSpeculativeCCDRigidBodyBitMap);

		SpeculativeCCDContactDistanceUpdateTask* ccdTask = createCCDTask(pool, mContextId, mContactDistance->begin(), mDt, *mBoundsArray);

		PinnableBitMap& changedMap = mAABBManager->getChangedAABBMgActorHandleMap();

		const size_t bodyOffset = PX_OFFSET_OF_RT(BodySim, getLowLevelBody());

		//printf("\n");
		//PxU32 count = 0;

		PxU32 nbBodies = 0;
		PxU32 index;
		while((index = speculativeCCDIter.getNext()) != PxBitMap::Iterator::DONE)
		{
			PxsRigidBody* rigidBody = getRigidBodyFromIG(islandSim, PxNodeIndex(index));
			BodySim* bodySim = reinterpret_cast<BodySim*>(reinterpret_cast<PxU8*>(rigidBody)-bodyOffset);
			if(bodySim)
			{
				//printf("%d\n", bodySim->getActiveListIndex());
				//printf("%d: %d\n", count++, bodySim->isActive());

				hasContactDistanceChanged = true;
				ccdTask->mBodySims[nbBodies++] = bodySim;

				// PT: ### changedMap pattern #1
				// PT: The task function will only touch the shapes whose body has the speculative flag and
				// isn't frozen, but here we mark all shapes as changed no matter what. It is most likely fine
				// because the map only contains objects that have the speculative flag enabled, and we remove
				// sleeping objects from it (which should include frozen objects).

				// PT: we test some bodySim data and one bit of each ShapeSim here, not great.
				// PT: TODO: consider doing this in the tasks with atomic ORs
				PxU32 nbElems = bodySim->getNbElements();
				ElementSim** elems = bodySim->getElements();
				while(nbElems--)
				{
					ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
					if(sim->getFlags() & PxShapeFlag::eSIMULATION_SHAPE)
						changedMap.growAndSet(sim->getElementID());
				}

				// PT: TODO: better load balancing?
				if(nbBodies == SpeculativeCCDContactDistanceUpdateTask::MaxBodies)
				{
					ccdTask->mNbBodies = nbBodies;
					nbBodies = 0;
					startTask(ccdTask, continuation);

					if(continuation)
						ccdTask = createCCDTask(pool, mContextId, mContactDistance->begin(), mDt, *mBoundsArray);
					else
						ccdTask->mNbBodies = 0;	// PT: no need to create a new task in single-threaded mode
				}
			}
		}

		if(nbBodies)
		{
			ccdTask->mNbBodies = nbBodies;
			startTask(ccdTask, continuation);
		}
	}
/*	else
	{
		// PT: codepath without mSpeculativeCCDRigidBodyBitMap
		PxU32 nb = mActiveBodies.size();
		BodyCore** bodies = mActiveBodies.begin();

		while(nb--)
		{
			const BodyCore* current = *bodies++;
			BodySim* bodySim = current->getSim();
			if(bodySim)
			{
				...
			}
		}
	}*/

	//calculate contact distance for articulation links
	{
		PxBitMap::Iterator articulateCCDIter(mSpeculativeCDDArticulationBitMap);
		PxU32 index;
		while((index = articulateCCDIter.getNext()) != PxBitMap::Iterator::DONE)
		{
			ArticulationSim* articulationSim = getArticulationSim(islandSim, PxNodeIndex(index));
			if(articulationSim)
			{
				hasContactDistanceChanged = true;
				if(continuation)
				{
					SpeculativeCCDContactDistanceArticulationUpdateTask* articulationUpdateTask = PX_PLACEMENT_NEW(pool.allocate(sizeof(SpeculativeCCDContactDistanceArticulationUpdateTask)), SpeculativeCCDContactDistanceArticulationUpdateTask)(mContextId, mContactDistance->begin(), mDt, *mBoundsArray, articulationSim);
					articulationUpdateTask->setContinuation(continuation);
					articulationUpdateTask->removeReference();
				}
				else
				{
					updateLinksContactDistances(articulationSim, mContactDistance->begin(), mDt, *mBoundsArray);
				}
			}
		}
	}

	mHasContactDistanceChanged = hasContactDistanceChanged;
}

///////////////////////////////////////////////////////////////////////////////

#include "ScNPhaseCore.h"
#include "ScShapeInteraction.h"
#include "PxsCCD.h"
#include "PxsSimulationController.h"
#include "CmTransformUtils.h"

void Sc::Scene::setCCDContactModifyCallback(PxCCDContactModifyCallback* callback)
{
	mCCDContext->setCCDContactModifyCallback(callback);
}

PxCCDContactModifyCallback* Sc::Scene::getCCDContactModifyCallback() const
{
	return mCCDContext->getCCDContactModifyCallback();
}

void Sc::Scene::setCCDMaxPasses(PxU32 ccdMaxPasses)
{
	mCCDContext->setCCDMaxPasses(ccdMaxPasses);
}

PxU32 Sc::Scene::getCCDMaxPasses() const
{
	return mCCDContext->getCCDMaxPasses();
}

void Sc::Scene::setCCDThreshold(PxReal t)
{
	mCCDContext->setCCDThreshold(t);
}

PxReal Sc::Scene::getCCDThreshold() const
{
	return mCCDContext->getCCDThreshold();
}

void Sc::Scene::collectPostSolverVelocitiesBeforeCCD()
{
	if(mContactReportsNeedPostSolverVelocity)
	{
		ActorPairReport*const* actorPairs = mNPhaseCore->getContactReportActorPairs();
		PxU32 nbActorPairs = mNPhaseCore->getNbContactReportActorPairs();
		for(PxU32 i=0; i < nbActorPairs; i++)
		{
			if(i < (nbActorPairs - 1))
				PxPrefetchLine(actorPairs[i+1]);

			ActorPairReport* aPair = actorPairs[i];

			ContactStreamManager& cs = aPair->getContactStreamManager();

			PxU32 streamManagerFlag = cs.getFlags();
			if(streamManagerFlag & ContactStreamManagerFlag::eINVALID_STREAM)
				continue;

			PxU8* stream = mNPhaseCore->getContactReportPairData(cs.bufferIndex);
			
			if(i + 1 < nbActorPairs)
				PxPrefetch(&(actorPairs[i+1]->getContactStreamManager()));

			if(!cs.extraDataSize)
				continue;
			else if (streamManagerFlag & ContactStreamManagerFlag::eNEEDS_POST_SOLVER_VELOCITY)
				cs.setContactReportPostSolverVelocity(stream, aPair->getActorA(), aPair->getActorB());
		}
	}
}

void Sc::Scene::updateCCDMultiPass(PxBaseTask* parentContinuation)
{
	getCcdBodies().forceSize_Unsafe(mSimulationControllerCallback->getNbCcdBodies());
	
	// second run of the broadphase for making sure objects we have integrated did not tunnel.
	if(mPublicFlags & PxSceneFlag::eENABLE_CCD)
	{
		if(mContactReportsNeedPostSolverVelocity)
		{
			// the CCD code will overwrite the post solver body velocities, hence, we need to extract the info
			// first if any CCD enabled pair requested it.
			collectPostSolverVelocitiesBeforeCCD();
		}

		//We use 2 CCD task chains to be able to chain together an arbitrary number of ccd passes
		if(mPostCCDPass.size() != 2)
		{
			mPostCCDPass.clear();
			mUpdateCCDSinglePass.clear();
			mCCDBroadPhase.clear();
			mCCDBroadPhaseAABB.clear();
			mPostCCDPass.reserve(2);
			mUpdateCCDSinglePass.reserve(2);
			mUpdateCCDSinglePass2.reserve(2);
			mUpdateCCDSinglePass3.reserve(2);
			mCCDBroadPhase.reserve(2);
			mCCDBroadPhaseAABB.reserve(2);
			for (int j = 0; j < 2; j++)
			{
				mPostCCDPass.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::postCCDPass>(mContextId, this, "ScScene.postCCDPass"));
				mUpdateCCDSinglePass.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePass>(mContextId, this, "ScScene.updateCCDSinglePass"));
				mUpdateCCDSinglePass2.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePassStage2>(mContextId, this, "ScScene.updateCCDSinglePassStage2"));
				mUpdateCCDSinglePass3.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePassStage3>(mContextId, this, "ScScene.updateCCDSinglePassStage3"));
				mCCDBroadPhase.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::ccdBroadPhase>(mContextId, this, "ScScene.ccdBroadPhase"));
				mCCDBroadPhaseAABB.pushBack(Cm::DelegateTask<Sc::Scene, &Sc::Scene::ccdBroadPhaseAABB>(mContextId, this, "ScScene.ccdBroadPhaseAABB"));
			}
		}

		//reset thread context in a place we know all tasks possibly accessing it, are in sync with. (see US6664)
		mLLContext->resetThreadContexts();

		mCCDContext->updateCCDBegin();

		mCCDBroadPhase[0].setContinuation(parentContinuation);
		mCCDBroadPhaseAABB[0].setContinuation(&mCCDBroadPhase[0]);
		mCCDBroadPhase[0].removeReference();
		mCCDBroadPhaseAABB[0].removeReference();
	}
}

namespace
{
class UpdateCCDBoundsTask : public Cm::Task
{
	Bp::BoundsArray*	mBoundArray;
	PxsTransformCache*	mTransformCache;
	BodySim**			mBodySims;
	PxU32				mNbToProcess;
	PxI32*				mNumFastMovingShapes;

public:

	static const PxU32 MaxPerTask = 256;

	UpdateCCDBoundsTask(PxU64 contextID, Bp::BoundsArray* boundsArray, PxsTransformCache* transformCache, BodySim** bodySims, PxU32 nbToProcess, PxI32* numFastMovingShapes) :
		Cm::Task			(contextID),
		mBoundArray			(boundsArray),
		mTransformCache		(transformCache),
		mBodySims			(bodySims), 
		mNbToProcess		(nbToProcess),
		mNumFastMovingShapes(numFastMovingShapes)
	{
	}

	virtual const char* getName() const PX_OVERRIDE { return "UpdateCCDBoundsTask";}

	PxIntBool	updateSweptBounds(ShapeSim* sim, BodySim* body)
	{
		PX_ASSERT(body==sim->getBodySim());

		const PxU32 elementID = sim->getElementID();

		const ShapeCore& shapeCore = sim->getCore();
		const PxTransform& endPose = mTransformCache->getTransformCache(elementID).transform;

		const PxGeometry& shapeGeom = shapeCore.getGeometry();

		const PxsRigidBody& rigidBody = body->getLowLevelBody();
		const PxsBodyCore& bodyCore = body->getBodyCore().getCore();
		PX_ALIGN(16, PxTransform shape2World);
		Cm::getDynamicGlobalPoseAligned(rigidBody.mLastTransform, shapeCore.getShape2Actor(), bodyCore.getBody2Actor(), shape2World);

		const float ccdThreshold = computeCCDThreshold(shapeGeom);
		PxBounds3 bounds = Gu::computeBounds(shapeGeom, endPose);
		PxIntBool isFastMoving;
		if(1)
		{
			// PT: this alternative implementation avoids computing the start bounds for slow moving objects.
			isFastMoving = (shape2World.p - endPose.p).magnitudeSquared() >= ccdThreshold * ccdThreshold ? 1 : 0;
			if(isFastMoving)
			{
				const PxBounds3 startBounds = Gu::computeBounds(shapeGeom, shape2World);
				bounds.include(startBounds);
			}
		}
		else
		{
			const PxBounds3 startBounds = Gu::computeBounds(shapeGeom, shape2World);

			isFastMoving = (startBounds.getCenter() - bounds.getCenter()).magnitudeSquared() >= ccdThreshold * ccdThreshold ? 1 : 0;

			if(isFastMoving)
				bounds.include(startBounds);
		}

		PX_ASSERT(bounds.minimum.x <= bounds.maximum.x
			&&	  bounds.minimum.y <= bounds.maximum.y
			&&	  bounds.minimum.z <= bounds.maximum.z);

		mBoundArray->setBounds(bounds, elementID);

		return isFastMoving;
	}

	virtual void runInternal() PX_OVERRIDE
	{
		PxU32 activeShapes = 0;
		const PxU32 nb = mNbToProcess;
		for(PxU32 i=0; i<nb; i++)
		{
			PxU32 isFastMoving = 0;
			BodySim& bodySim = *mBodySims[i];

			PxU32 nbElems = bodySim.getNbElements();
			ElementSim** elems = bodySim.getElements();
			while(nbElems--)
			{
				ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
				if(sim->getFlags() & PxU32(PxShapeFlag::eSIMULATION_SHAPE | PxShapeFlag::eTRIGGER_SHAPE))
				{
					const PxIntBool fastMovingShape = updateSweptBounds(sim, &bodySim);
					activeShapes += fastMovingShape;

					isFastMoving = isFastMoving | fastMovingShape;
				}
			}

			bodySim.getLowLevelBody().getCore().isFastMoving = isFastMoving!=0;
		}

		PxAtomicAdd(mNumFastMovingShapes, PxI32(activeShapes));
	}
};
}

void Sc::Scene::ccdBroadPhaseAABB(PxBaseTask* continuation)
{
	PX_PROFILE_START_CROSSTHREAD("Sim.ccdBroadPhaseComplete", mContextId);
	PX_PROFILE_ZONE("Sim.ccdBroadPhaseAABB", mContextId);
	PX_UNUSED(continuation);

	PxU32 currentPass = mCCDContext->getCurrentCCDPass();

	Cm::FlushPool& flushPool = mLLContext->getTaskPool();

	mNumFastMovingShapes = 0;

	//If we are on the 1st pass or we had some sweep hits previous CCD pass, we need to run CCD again
	if(currentPass == 0 || mCCDContext->getNumSweepHits())
	{
		PxsTransformCache& transformCache = getLowLevelContext()->getTransformCache();
		for(PxU32 i = 0; i < mCcdBodies.size(); i+= UpdateCCDBoundsTask::MaxPerTask)
		{
			const PxU32 nbToProcess = PxMin(UpdateCCDBoundsTask::MaxPerTask, mCcdBodies.size() - i);
			UpdateCCDBoundsTask* task = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(UpdateCCDBoundsTask)), UpdateCCDBoundsTask)(mContextId, mBoundsArray, &transformCache, &mCcdBodies[i], nbToProcess, &mNumFastMovingShapes);
			task->setContinuation(continuation);
			task->removeReference();
		}
	}
}

void Sc::Scene::ccdBroadPhase(PxBaseTask* continuation)
{
	PX_PROFILE_ZONE("Sim.ccdBroadPhase", mContextId);

	PxU32 currentPass = mCCDContext->getCurrentCCDPass();
	const PxU32 ccdMaxPasses = mCCDContext->getCCDMaxPasses();
	mCCDPass = currentPass+1;

	//If we are on the 1st pass or we had some sweep hits previous CCD pass, we need to run CCD again
	if( (currentPass == 0 || mCCDContext->getNumSweepHits()) && mNumFastMovingShapes != 0)
	{
		const PxU32 currIndex = currentPass & 1;
		const PxU32 nextIndex = 1 - currIndex;
		//Initialize the CCD task chain unless this is the final pass
		if(currentPass != (ccdMaxPasses - 1))
		{
			mCCDBroadPhase[nextIndex].setContinuation(continuation);
			mCCDBroadPhaseAABB[nextIndex].setContinuation(&mCCDBroadPhase[nextIndex]);
		}
		mPostCCDPass[currIndex].setContinuation(currentPass == ccdMaxPasses-1 ? continuation : &mCCDBroadPhaseAABB[nextIndex]);
		mUpdateCCDSinglePass3[currIndex].setContinuation(&mPostCCDPass[currIndex]);
		mUpdateCCDSinglePass2[currIndex].setContinuation(&mUpdateCCDSinglePass3[currIndex]);
		mUpdateCCDSinglePass[currIndex].setContinuation(&mUpdateCCDSinglePass2[currIndex]);

		//Do the actual broad phase
		PxBaseTask* continuationTask = &mUpdateCCDSinglePass[currIndex];
//		const PxU32 numCpuTasks = continuationTask->getTaskManager()->getCpuDispatcher()->getWorkerCount();

		mCCDBp = true;

		setupBroadPhaseFirstAndSecondPassTasks(continuationTask);
		
		//mAABBManager->updateAABBsAndBP(numCpuTasks, mLLContext->getTaskPool(), &mLLContext->getScratchAllocator(), false, continuationTask, NULL);

		//Allow the CCD task chain to continue
		mPostCCDPass[currIndex].removeReference();
		mUpdateCCDSinglePass3[currIndex].removeReference();
		mUpdateCCDSinglePass2[currIndex].removeReference();
		mUpdateCCDSinglePass[currIndex].removeReference();
		if(currentPass != (ccdMaxPasses - 1))
		{
			mCCDBroadPhase[nextIndex].removeReference();
			mCCDBroadPhaseAABB[nextIndex].removeReference();
		}
	}
	else if (currentPass == 0)
	{
		PX_PROFILE_STOP_CROSSTHREAD("Sim.ccdBroadPhaseComplete", mContextId);
		mCCDContext->resetContactManagers();
	}
}

void Sc::Scene::updateCCDSinglePass(PxBaseTask* continuation)
{
	PX_PROFILE_ZONE("Sim.updateCCDSinglePass", mContextId);
	mReportShapePairTimeStamp++;  // This will makes sure that new report pairs will get created instead of re-using the existing ones.

	mAABBManager->postBroadPhase(NULL, *getFlushPool());
	finishBroadPhase(continuation);
	
	const PxU32 currentPass = mCCDContext->getCurrentCCDPass() + 1;  // 0 is reserved for discrete collision phase
	if(currentPass == 1)		// reset the handle map so we only update CCD objects from here on
	{
		PinnableBitMap& changedAABBMgrActorHandles = mAABBManager->getChangedAABBMgActorHandleMap();
		//changedAABBMgrActorHandles.clear();
		for(PxU32 i = 0; i < mCcdBodies.size();i++)
		{
			// PT: ### changedMap pattern #1
			PxU32 nbElems = mCcdBodies[i]->getNbElements();
			ElementSim** elems = mCcdBodies[i]->getElements();
			while(nbElems--)
			{
				ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
				if(sim->getFlags()&PxU32(PxShapeFlag::eSIMULATION_SHAPE | PxShapeFlag::eTRIGGER_SHAPE))	// TODO: need trigger shape here?
					changedAABBMgrActorHandles.growAndSet(sim->getElementID());
			}
		}
	}
}

void Sc::Scene::updateCCDSinglePassStage2(PxBaseTask* continuation)
{
	PX_PROFILE_ZONE("Sim.updateCCDSinglePassStage2", mContextId);
	postBroadPhaseStage2(continuation);
}

void Sc::Scene::updateCCDSinglePassStage3(PxBaseTask* continuation)
{
	PX_PROFILE_ZONE("Sim.updateCCDSinglePassStage3", mContextId);
	mReportShapePairTimeStamp++;  // This will makes sure that new report pairs will get created instead of re-using the existing ones.

	const PxU32 currentPass = mCCDContext->getCurrentCCDPass() + 1;  // 0 is reserved for discrete collision phase
	finishBroadPhaseStage2(currentPass);
	PX_PROFILE_STOP_CROSSTHREAD("Sim.ccdBroadPhaseComplete", mContextId);

	//reset thread context in a place we know all tasks possibly accessing it, are in sync with. (see US6664)
	mLLContext->resetThreadContexts();

	mCCDContext->updateCCD(mDt, continuation, mSimpleIslandManager->getAccurateIslandSim(), (mPublicFlags & PxSceneFlag::eDISABLE_CCD_RESWEEP), mNumFastMovingShapes);
}

static PX_FORCE_INLINE Sc::ShapeInteraction* getSI(PxvContactManagerTouchEvent& evt)
{
	return reinterpret_cast<Sc::ShapeInteraction*>(evt.getCMTouchEventUserData());
}

void Sc::Scene::postCCDPass(PxBaseTask* /*continuation*/)
{
	// - Performs sleep check
	// - Updates touch flags

	PxU32 currentPass = mCCDContext->getCurrentCCDPass();
	PX_ASSERT(currentPass > 0); // to make sure changes to the CCD pass counting get noticed. For contact reports, 0 means discrete collision phase.

	// Local array that uses a stack buffer for small counts, avoiding heap allocation in the common case.
	// Falls back to heap allocation if the array grows beyond the inline capacity.
	struct LocalArray : public PxArray<PxvContactManagerTouchEvent>
	{
		LocalArray()
		{
			mSize		= 0;
			mCapacity	= 64|PX_SIGN_BITMASK;
			mData		= mBuffer;
		}

		PxvContactManagerTouchEvent	mBuffer[64];
	};

	PxU32 newTouchCount, lostTouchCount, ccdTouchCount;
	mLLContext->getManagerTouchEventCount(&newTouchCount, &lostTouchCount, &ccdTouchCount);

	LocalArray newTouchEvents;
	LocalArray lostTouchEvents;
	LocalArray ccdTouchEvents;

	if(newTouchCount > 64)
		newTouchEvents.reserve(newTouchCount);
	if(lostTouchCount > 64)
		lostTouchEvents.reserve(lostTouchCount);
	if(ccdTouchCount > 64)
		ccdTouchEvents.reserve(ccdTouchCount);

	PxsContactManagerOutputIterator outputs = mLLContext->getNphaseImplementationContext()->getContactManagerOutputs();

	// Note: For contact notifications it is important that the new touch pairs get processed before the lost touch pairs.
	//       This allows to know for sure if a pair of actors lost all touch (see eACTOR_PAIR_LOST_TOUCH).
	mLLContext->fillManagerTouchEvents(newTouchEvents, lostTouchEvents, &ccdTouchEvents);
	for(PxU32 i=0; i<newTouchEvents.size(); ++i)
	{
		ShapeInteraction* si = getSI(newTouchEvents[i]);
		PX_ASSERT(si);
		mNPhaseCore->managerNewTouch(*si);
		si->managerNewTouch(currentPass, outputs);
		if (!si->readFlag(ShapeInteraction::CONTACTS_RESPONSE_DISABLED))
		{
			mSimpleIslandManager->setEdgeConnected(si->getEdgeIndex(), IG::Edge::eCONTACT_MANAGER);
		}
	}
	for(PxU32 i=0; i<lostTouchEvents.size(); ++i)
	{
		ShapeInteraction* si = getSI(lostTouchEvents[i]);
		PX_ASSERT(si);
		if (si->managerLostTouch(currentPass, outputs) && !si->readFlag(ShapeInteraction::CONTACTS_RESPONSE_DISABLED))
			addToLostTouchList(si->getActor0(), si->getActor1());

		mSimpleIslandManager->setEdgeDisconnected(si->getEdgeIndex());
	}
	for(PxU32 i=0; i<ccdTouchEvents.size(); ++i)
	{
		ShapeInteraction* si = getSI(ccdTouchEvents[i]);
		PX_ASSERT(si);
		si->sendCCDRetouch(currentPass, outputs);
	}
	checkForceThresholdContactEvents(currentPass);
	{
		PinnableBitMap& changedAABBMgrActorHandles = mAABBManager->getChangedAABBMgActorHandleMap();

		const UpdateCachedParams params(mLLContext->getTransformCache(), getBoundsArray());

		for (PxU32 i = 0, s = mCcdBodies.size(); i < s; i++)
		{
			BodySim*const body = mCcdBodies[i];
			if(i+8 < s)
				PxPrefetch(mCcdBodies[i+8], 512);

			PX_ASSERT(body->getBody2World().p.isFinite());
			PX_ASSERT(body->getBody2World().q.isFinite());

			body->updateCached_NotThreadSafe(params, &changedAABBMgrActorHandles, false, false);
		}

		ArticulationCore* const* articList = mArticulations.getEntries();
		for(PxU32 i=0;i<mArticulations.size();i++)
			articList[i]->getSim()->updateCached_NotThreadSafe(params, &changedAABBMgrActorHandles, false, false);
	}
}

void updateCCDLinks(Sc::ArticulationSim& artic, PxArray<BodySim*>& sims)
{
	const PxU32 nbBodies = artic.getNbBodies();
	BodySim** bodies = artic.getBodies();

	for(PxU32 i=0; i<nbBodies; i++)
	{
		if(bodies[i]->getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_CCD)
			sims.pushBack(bodies[i]);
	}
}