feat(physics): wire physx sdk into build
This commit is contained in:
720
engine/third_party/physx/source/simulationcontroller/src/ScCCD.cpp
vendored
Normal file
720
engine/third_party/physx/source/simulationcontroller/src/ScCCD.cpp
vendored
Normal file
@@ -0,0 +1,720 @@
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions
|
||||
// are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
||||
// contributors may be used to endorse or promote products derived
|
||||
// from this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
||||
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
||||
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
||||
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//
|
||||
// Copyright (c) 2008-2025 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 "common/PxProfileZone.h"
|
||||
#include "ScBodySim.h"
|
||||
#include "ScShapeSim.h"
|
||||
#include "ScArticulationSim.h"
|
||||
#include "ScScene.h"
|
||||
#include "DyIslandManager.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Sc;
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Sc::ArticulationSim::updateContactDistance(PxReal* contactDistance, PxReal dt, const Bp::BoundsArray& boundsArray)
|
||||
{
|
||||
const PxU32 size = mBodies.size();
|
||||
for(PxU32 i=0; i<size; i++)
|
||||
mBodies[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()
|
||||
{
|
||||
const PxU32 nb = mNbBodies;
|
||||
for(PxU32 i=0; i<nb; i++)
|
||||
mBodySims[i]->updateContactDistance(mContactDistances, mDt, mBoundsArray);
|
||||
}
|
||||
|
||||
virtual const char* getName() const { 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()
|
||||
{
|
||||
mArticulation->updateContactDistance(mContactDistances, mDt, mBoundsArray);
|
||||
}
|
||||
|
||||
virtual const char* getName() const { 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....
|
||||
|
||||
// PT: TODO: why do we need to involve the island manager here?
|
||||
// PT: TODO: why do we do that on sleeping bodies? Why don't we use mActiveBodies?
|
||||
// PxArray<BodyCore*> mActiveBodies; // Sorted: kinematic before dynamic
|
||||
// ===> because we remove bodies from the bitmap in BodySim::deactivate()
|
||||
|
||||
//calculate contact distance for speculative CCD shapes
|
||||
if(1)
|
||||
{
|
||||
PxBitMap::Iterator speculativeCCDIter(mSpeculativeCCDRigidBodyBitMap);
|
||||
|
||||
SpeculativeCCDContactDistanceUpdateTask* ccdTask = createCCDTask(pool, mContextId, mContactDistance->begin(), mDt, *mBoundsArray);
|
||||
|
||||
PxBitMapPinned& 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: TODO: isn't there a problem here? 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.
|
||||
//
|
||||
// Also we test some bodySim data and one bit of each ShapeSim here, not great.
|
||||
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
|
||||
{
|
||||
articulationSim->updateContactDistance(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 { 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()
|
||||
{
|
||||
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
|
||||
{
|
||||
PxBitMapPinned& 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.
|
||||
|
||||
PxU32 newTouchCount, lostTouchCount, ccdTouchCount;
|
||||
mLLContext->getManagerTouchEventCount(&newTouchCount, &lostTouchCount, &ccdTouchCount);
|
||||
PX_ALLOCA(newTouches, PxvContactManagerTouchEvent, newTouchCount);
|
||||
PX_ALLOCA(lostTouches, PxvContactManagerTouchEvent, lostTouchCount);
|
||||
PX_ALLOCA(ccdTouches, PxvContactManagerTouchEvent, 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(newTouches, newTouchCount, lostTouches, lostTouchCount, ccdTouches, ccdTouchCount);
|
||||
for(PxU32 i=0; i<newTouchCount; ++i)
|
||||
{
|
||||
ShapeInteraction* si = getSI(newTouches[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<lostTouchCount; ++i)
|
||||
{
|
||||
ShapeInteraction* si = getSI(lostTouches[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<ccdTouchCount; ++i)
|
||||
{
|
||||
ShapeInteraction* si = getSI(ccdTouches[i]);
|
||||
PX_ASSERT(si);
|
||||
si->sendCCDRetouch(currentPass, outputs);
|
||||
}
|
||||
checkForceThresholdContactEvents(currentPass);
|
||||
{
|
||||
PxBitMapPinned& changedAABBMgrActorHandles = mAABBManager->getChangedAABBMgActorHandleMap();
|
||||
|
||||
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(&changedAABBMgrActorHandles);
|
||||
}
|
||||
|
||||
ArticulationCore* const* articList = mArticulations.getEntries();
|
||||
for(PxU32 i=0;i<mArticulations.size();i++)
|
||||
articList[i]->getSim()->updateCached(&changedAABBMgrActorHandles);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user