feat(physics): wire physx sdk into build
This commit is contained in:
863
engine/third_party/physx/source/simulationcontroller/src/ScBodySim.cpp
vendored
Normal file
863
engine/third_party/physx/source/simulationcontroller/src/ScBodySim.cpp
vendored
Normal file
@@ -0,0 +1,863 @@
|
||||
// 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 "ScBodySim.h"
|
||||
#include "ScShapeSim.h"
|
||||
#include "ScScene.h"
|
||||
#include "ScArticulationSim.h"
|
||||
#include "PxsContext.h"
|
||||
#include "PxsSimpleIslandManager.h"
|
||||
#include "PxsSimulationController.h"
|
||||
#include "ScSimStateData.h"
|
||||
|
||||
using namespace physx;
|
||||
using namespace Dy;
|
||||
using namespace Sc;
|
||||
|
||||
#define PX_FREEZE_INTERVAL 1.5f
|
||||
#define PX_FREE_EXIT_THRESHOLD 4.f
|
||||
#define PX_FREEZE_TOLERANCE 0.25f
|
||||
|
||||
#define PX_SLEEP_DAMPING 0.5f
|
||||
#define PX_FREEZE_SCALE 0.9f
|
||||
|
||||
static void updateBPGroup(ActorSim* sim)
|
||||
{
|
||||
PxU32 nbElems = sim->getNbElements();
|
||||
ElementSim** elems = sim->getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* current = static_cast<ShapeSim*>(*elems++);
|
||||
current->updateBPGroup();
|
||||
}
|
||||
}
|
||||
|
||||
BodySim::BodySim(Scene& scene, BodyCore& core, bool compound) :
|
||||
RigidSim (scene, core),
|
||||
mLLBody (&core.getCore(), PX_FREEZE_INTERVAL),
|
||||
mSimStateData (NULL),
|
||||
mVelModState (VMF_GRAVITY_DIRTY),
|
||||
mArticulation (NULL)
|
||||
{
|
||||
core.getCore().numCountedInteractions = 0;
|
||||
core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
||||
mLLBody.mInternalFlags |= PxsRigidBody::eSPECULATIVE_CCD;
|
||||
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_GYROSCOPIC_FORCES)
|
||||
mLLBody.mInternalFlags |= PxsRigidBody::eENABLE_GYROSCOPIC;
|
||||
|
||||
if(core.getFlags() & PxRigidBodyFlag::eRETAIN_ACCELERATIONS)
|
||||
mLLBody.mInternalFlags |= PxsRigidBody::eRETAIN_ACCELERATION;
|
||||
|
||||
// PT: don't read the core ptr we just wrote, use input param
|
||||
// PT: at time of writing we get a big L2 here because even though bodycore has been prefetched, the wake counter is 160 bytes away
|
||||
const bool isAwake = (core.getWakeCounter() > 0) ||
|
||||
(!core.getLinearVelocity().isZero()) ||
|
||||
(!core.getAngularVelocity().isZero());
|
||||
|
||||
const bool isKine = isKinematic();
|
||||
|
||||
IG::SimpleIslandManager* simpleIslandManager = scene.getSimpleIslandManager();
|
||||
if(!isArticulationLink())
|
||||
{
|
||||
mNodeIndex = simpleIslandManager->addNode(isAwake, isKine, IG::Node::eRIGID_BODY_TYPE, &mLLBody);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(mArticulation)
|
||||
{
|
||||
const PxU32 linkIndex = mArticulation->findBodyIndex(*this);
|
||||
const PxNodeIndex index = mArticulation->getIslandNodeIndex();
|
||||
mNodeIndex.setIndices(index.index(), linkIndex);
|
||||
}
|
||||
}
|
||||
|
||||
PX_ASSERT(mActiveListIndex == SC_NOT_IN_SCENE_INDEX);
|
||||
|
||||
// A.B. need to set the compound rigid flag early enough, so that we add the rigid into
|
||||
// active list and do not create the shape bounds
|
||||
if(compound)
|
||||
raiseInternalFlag(BF_IS_COMPOUND_RIGID);
|
||||
|
||||
setActive(isAwake, true);
|
||||
|
||||
if(isAwake)
|
||||
{
|
||||
scene.addToActiveList(*this);
|
||||
PX_ASSERT(isActive());
|
||||
}
|
||||
else
|
||||
{
|
||||
mActiveListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
|
||||
mActiveCompoundListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
|
||||
PX_ASSERT(!isActive());
|
||||
|
||||
simpleIslandManager->deactivateNode(mNodeIndex);
|
||||
}
|
||||
|
||||
if(isKine)
|
||||
{
|
||||
initKinematicStateBase(core, true);
|
||||
setupSimStateData(true);
|
||||
notifyPutToSleep(); // sleep state of kinematics is fully controlled by the simulation controller not the island manager
|
||||
|
||||
mFilterFlags |= PxFilterObjectFlag::eKINEMATIC;
|
||||
}
|
||||
}
|
||||
|
||||
BodySim::~BodySim()
|
||||
{
|
||||
Scene& scene = mScene;
|
||||
const bool active = isActive();
|
||||
|
||||
tearDownSimStateData(isKinematic());
|
||||
PX_ASSERT(!mSimStateData);
|
||||
|
||||
// PX-4603. AD: assuming that the articulation code cleans up the dirty state in case this is an articulation link.
|
||||
if (!isArticulationLink())
|
||||
mScene.getVelocityModifyMap().boundedReset(mNodeIndex.index());
|
||||
|
||||
PX_ASSERT(!readInternalFlag(BF_ON_DEATHROW)); // Before 3.0 it could happen that destroy could get called twice. Assert to make sure this is fixed.
|
||||
raiseInternalFlag(BF_ON_DEATHROW);
|
||||
|
||||
scene.removeBody(*this);
|
||||
|
||||
//Articulations are represented by a single node, so they must only be removed by the articulation and not the links!
|
||||
if(mArticulation == NULL && mNodeIndex.articulationLinkId() == 0) //If it wasn't an articulation link, then we can remove it
|
||||
scene.getSimpleIslandManager()->removeNode(mNodeIndex);
|
||||
|
||||
PX_ASSERT(mActiveListIndex != SC_NOT_IN_SCENE_INDEX);
|
||||
|
||||
if (active)
|
||||
scene.removeFromActiveList(*this);
|
||||
|
||||
mActiveListIndex = SC_NOT_IN_SCENE_INDEX;
|
||||
mActiveCompoundListIndex = SC_NOT_IN_SCENE_INDEX;
|
||||
|
||||
mCore.setSim(NULL);
|
||||
}
|
||||
|
||||
void BodySim::updateCached(PxBitMapPinned* shapeChangedMap)
|
||||
{
|
||||
if(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN))
|
||||
{
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* current = static_cast<ShapeSim*>(*elems++);
|
||||
current->updateCached(0, shapeChangedMap);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
|
||||
{
|
||||
PX_ASSERT(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN)); // PT: should not be called otherwise
|
||||
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* current = static_cast<ShapeSim*>(*elems++);
|
||||
current->updateCached(transformCache, boundsArray);
|
||||
}
|
||||
}
|
||||
|
||||
bool BodySim::setupSimStateData(bool isKinematic)
|
||||
{
|
||||
SimStateData* data = mSimStateData;
|
||||
if(!data)
|
||||
{
|
||||
data = mScene.getSimStateDataPool()->construct();
|
||||
if(!data)
|
||||
return false;
|
||||
}
|
||||
|
||||
if(isKinematic)
|
||||
{
|
||||
PX_ASSERT(!mSimStateData || !mSimStateData->isKine());
|
||||
|
||||
PX_PLACEMENT_NEW(data, SimStateData(SimStateData::eKine));
|
||||
Kinematic* kine = data->getKinematicData();
|
||||
kine->targetValid = 0;
|
||||
simStateBackupAndClearBodyProperties(data, getBodyCore().getCore());
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(!mSimStateData || !mSimStateData->isVelMod());
|
||||
|
||||
PX_PLACEMENT_NEW(data, SimStateData(SimStateData::eVelMod));
|
||||
VelocityMod* velmod = data->getVelocityModData();
|
||||
velmod->clear();
|
||||
}
|
||||
mSimStateData = data;
|
||||
return true;
|
||||
}
|
||||
|
||||
void BodySim::tearDownSimStateData(bool isKinematic)
|
||||
{
|
||||
PX_ASSERT(!mSimStateData || mSimStateData->isKine() == isKinematic);
|
||||
|
||||
if (mSimStateData)
|
||||
{
|
||||
if (isKinematic)
|
||||
simStateRestoreBodyProperties(mSimStateData, getBodyCore().getCore());
|
||||
|
||||
mScene.getSimStateDataPool()->destroy(mSimStateData);
|
||||
mSimStateData = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::switchToKinematic()
|
||||
{
|
||||
setupSimStateData(true);
|
||||
|
||||
{
|
||||
initKinematicStateBase(getBodyCore(), false);
|
||||
|
||||
// - interactions need to get refiltered to make sure that kinematic-kinematic and kinematic-static pairs get suppressed
|
||||
// - unlike postSwitchToDynamic(), constraint interactions are not marked dirty here because a transition to kinematic will put the object asleep which in turn
|
||||
// triggers onDeactivate() on the constraint pairs that are active. If such pairs get deactivated, they will get removed from the list of active breakable
|
||||
// constraints automatically.
|
||||
setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE);
|
||||
|
||||
mScene.getSimpleIslandManager()->setKinematic(mNodeIndex);
|
||||
|
||||
updateBPGroup(this);
|
||||
}
|
||||
|
||||
mScene.setDynamicsDirty();
|
||||
|
||||
mFilterFlags |= PxFilterObjectFlag::eKINEMATIC;
|
||||
}
|
||||
|
||||
void BodySim::switchToDynamic()
|
||||
{
|
||||
tearDownSimStateData(true);
|
||||
|
||||
{
|
||||
mScene.getSimpleIslandManager()->setDynamic(mNodeIndex);
|
||||
|
||||
setForcesToDefaults(true);
|
||||
|
||||
// - interactions need to get refiltered to make sure that former kinematic-kinematic and kinematic-static pairs get enabled
|
||||
// - switching from kinematic to dynamic does not change the sleep state of the body. The constraint interactions are marked dirty
|
||||
// to check later whether they need to be activated plus potentially tracked for constraint break testing. This special treatment
|
||||
// is necessary because constraints between two kinematic bodies are considered inactive, no matter whether one of the kinematics
|
||||
// is active (has a target) or not.
|
||||
setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE | InteractionFlag::eCONSTRAINT);
|
||||
|
||||
clearInternalFlag(BF_KINEMATIC_MOVE_FLAGS);
|
||||
|
||||
if(isActive())
|
||||
mScene.swapInActiveBodyList(*this);
|
||||
|
||||
//
|
||||
updateBPGroup(this);
|
||||
}
|
||||
|
||||
mScene.setDynamicsDirty();
|
||||
|
||||
mFilterFlags &= ~PxFilterObjectFlag::eKINEMATIC;
|
||||
}
|
||||
|
||||
void BodySim::setKinematicTarget(const PxTransform& p)
|
||||
{
|
||||
PX_ASSERT(getSimStateData(true));
|
||||
PX_ASSERT(getSimStateData(true)->isKine());
|
||||
simStateSetKinematicTarget(getSimStateData_Unchecked(), p);
|
||||
PX_ASSERT(getSimStateData(true)->getKinematicData()->targetValid);
|
||||
|
||||
raiseInternalFlag(BF_KINEMATIC_MOVED); // Important to set this here already because trigger interactions need to have this information when being activated.
|
||||
clearInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY);
|
||||
}
|
||||
|
||||
void BodySim::addSpatialAcceleration(const PxVec3* linAcc, const PxVec3* angAcc)
|
||||
{
|
||||
notifyDirtySpatialAcceleration();
|
||||
|
||||
if (!mSimStateData || !mSimStateData->isVelMod())
|
||||
setupSimStateData(false);
|
||||
|
||||
VelocityMod* velmod = mSimStateData->getVelocityModData();
|
||||
if (linAcc)
|
||||
velmod->accumulateLinearVelModPerSec(*linAcc);
|
||||
if (angAcc)
|
||||
velmod->accumulateAngularVelModPerSec(*angAcc);
|
||||
}
|
||||
|
||||
void BodySim::setSpatialAcceleration(const PxVec3* linAcc, const PxVec3* angAcc)
|
||||
{
|
||||
notifyDirtySpatialAcceleration();
|
||||
|
||||
if (!mSimStateData || !mSimStateData->isVelMod())
|
||||
setupSimStateData(false);
|
||||
|
||||
VelocityMod* velmod = mSimStateData->getVelocityModData();
|
||||
if (linAcc)
|
||||
velmod->setLinearVelModPerSec(*linAcc);
|
||||
if (angAcc)
|
||||
velmod->setAngularVelModPerSec(*angAcc);
|
||||
}
|
||||
|
||||
void BodySim::clearSpatialAcceleration(bool force, bool torque)
|
||||
{
|
||||
PX_ASSERT(force || torque);
|
||||
|
||||
notifyDirtySpatialAcceleration();
|
||||
|
||||
if (mSimStateData)
|
||||
{
|
||||
PX_ASSERT(mSimStateData->isVelMod());
|
||||
VelocityMod* velmod = mSimStateData->getVelocityModData();
|
||||
if (force)
|
||||
velmod->clearLinearVelModPerSec();
|
||||
if (torque)
|
||||
velmod->clearAngularVelModPerSec();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::addSpatialVelocity(const PxVec3* linVelDelta, const PxVec3* angVelDelta)
|
||||
{
|
||||
notifyDirtySpatialVelocity();
|
||||
|
||||
if (!mSimStateData || !mSimStateData->isVelMod())
|
||||
setupSimStateData(false);
|
||||
|
||||
VelocityMod* velmod = mSimStateData->getVelocityModData();
|
||||
if (linVelDelta)
|
||||
velmod->accumulateLinearVelModPerStep(*linVelDelta);
|
||||
if (angVelDelta)
|
||||
velmod->accumulateAngularVelModPerStep(*angVelDelta);
|
||||
}
|
||||
|
||||
void BodySim::clearSpatialVelocity(bool force, bool torque)
|
||||
{
|
||||
PX_ASSERT(force || torque);
|
||||
|
||||
notifyDirtySpatialVelocity();
|
||||
|
||||
if (mSimStateData)
|
||||
{
|
||||
PX_ASSERT(mSimStateData->isVelMod());
|
||||
VelocityMod* velmod = mSimStateData->getVelocityModData();
|
||||
if (force)
|
||||
velmod->clearLinearVelModPerStep();
|
||||
if (torque)
|
||||
velmod->clearAngularVelModPerStep();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::raiseVelocityModFlagAndNotify(VelocityModFlags flag)
|
||||
{
|
||||
//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to
|
||||
//the expense of querying the simStateData for the velmod values.
|
||||
raiseVelocityModFlag(flag);
|
||||
|
||||
if (!isArticulationLink())
|
||||
mScene.getVelocityModifyMap().growAndSet(getNodeIndex().index());
|
||||
else
|
||||
mScene.addDirtyArticulationSim(getArticulation());
|
||||
}
|
||||
|
||||
void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
|
||||
{
|
||||
// PT: don't convert to bool if not needed
|
||||
const PxU32 wasWeightless = oldFlags & PxActorFlag::eDISABLE_GRAVITY;
|
||||
const PxU32 isWeightless = newFlags & PxActorFlag::eDISABLE_GRAVITY;
|
||||
|
||||
if (isWeightless != wasWeightless)
|
||||
{
|
||||
if (mVelModState == 0)
|
||||
raiseVelocityModFlag(VMF_GRAVITY_DIRTY);
|
||||
|
||||
getBodyCore().getCore().disableGravity = isWeightless!=0;
|
||||
|
||||
if(mArticulation)
|
||||
mArticulation->setArticulationDirty(ArticulationDirtyFlag::eDIRTY_LINKS); // forces an update in PxgSimulationController::updateGpuArticulationSim
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::postBody2WorldChange()
|
||||
{
|
||||
mLLBody.saveLastCCDTransform();
|
||||
notifyShapesOfTransformChange();
|
||||
}
|
||||
|
||||
void BodySim::postSetWakeCounter(PxReal t, bool forceWakeUp)
|
||||
{
|
||||
if ((t > 0.0f) || forceWakeUp)
|
||||
notifyNotReadyForSleeping();
|
||||
else
|
||||
{
|
||||
const bool readyForSleep = checkSleepReadinessBesidesWakeCounter();
|
||||
if (readyForSleep)
|
||||
notifyReadyForSleeping();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::postPosePreviewChange(PxU32 posePreviewFlag)
|
||||
{
|
||||
if (isActive())
|
||||
{
|
||||
if (posePreviewFlag & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
||||
mScene.addToPosePreviewList(*this);
|
||||
else
|
||||
mScene.removeFromPosePreviewList(*this);
|
||||
}
|
||||
else
|
||||
PX_ASSERT(!mScene.isInPosePreviewList(*this));
|
||||
}
|
||||
|
||||
void BodySim::activate()
|
||||
{
|
||||
BodyCore& core = getBodyCore();
|
||||
|
||||
// Activate body
|
||||
{
|
||||
PX_ASSERT((!isKinematic()) || notInScene() || readInternalFlag(InternalFlags(BF_KINEMATIC_MOVED | BF_KINEMATIC_SURFACE_VELOCITY))); // kinematics should only get activated when a target is set.
|
||||
// exception: object gets newly added, then the state change will happen later
|
||||
if(!isArticulationLink())
|
||||
{
|
||||
mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
|
||||
// Put in list of activated bodies. The list gets cleared at the end of a sim step after the sleep callbacks have been fired.
|
||||
mScene.onBodyWakeUp(this);
|
||||
}
|
||||
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
||||
{
|
||||
PX_ASSERT(!mScene.isInPosePreviewList(*this));
|
||||
mScene.addToPosePreviewList(*this);
|
||||
}
|
||||
createSqBounds();
|
||||
}
|
||||
|
||||
activateInteractions(*this);
|
||||
|
||||
//set speculative CCD bit map if speculative CCD flag is on
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
||||
addToSpeculativeCCDMap();
|
||||
}
|
||||
|
||||
void BodySim::deactivate()
|
||||
{
|
||||
deactivateInteractions(*this);
|
||||
|
||||
BodyCore& core = getBodyCore();
|
||||
|
||||
// Deactivate body
|
||||
{
|
||||
PX_ASSERT((!isKinematic()) || notInScene() || !readInternalFlag(BF_KINEMATIC_MOVED)); // kinematics should only get deactivated when no target is set.
|
||||
// exception: object gets newly added, then the state change will happen later
|
||||
if(!readInternalFlag(BF_ON_DEATHROW))
|
||||
{
|
||||
// Set velocity to 0.
|
||||
// Note: this is also fine if the method gets called because the user puts something to sleep (this behavior is documented in the API)
|
||||
PX_ASSERT(core.getWakeCounter() == 0.0f);
|
||||
const PxVec3 zero(0.0f);
|
||||
core.setLinearVelocityInternal(zero);
|
||||
core.setAngularVelocityInternal(zero);
|
||||
|
||||
setForcesToDefaults(!core.getCore().disableGravity);
|
||||
}
|
||||
|
||||
if(!isArticulationLink()) // Articulations have their own sleep logic.
|
||||
mScene.onBodySleep(this);
|
||||
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
|
||||
{
|
||||
PX_ASSERT(mScene.isInPosePreviewList(*this));
|
||||
mScene.removeFromPosePreviewList(*this);
|
||||
}
|
||||
destroySqBounds();
|
||||
}
|
||||
|
||||
// reset speculative CCD bit map if speculative CCD flag is on
|
||||
if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
||||
removeFromSpeculativeCCDMap();
|
||||
}
|
||||
|
||||
void BodySim::wakeUp()
|
||||
{
|
||||
setActive(true);
|
||||
notifyWakeUp();
|
||||
}
|
||||
|
||||
void BodySim::putToSleep()
|
||||
{
|
||||
PX_ASSERT(getBodyCore().getWakeCounter() == 0.0f);
|
||||
PX_ASSERT(getBodyCore().getLinearVelocity().isZero());
|
||||
PX_ASSERT(getBodyCore().getAngularVelocity().isZero());
|
||||
|
||||
notifyDirtySpatialAcceleration();
|
||||
notifyDirtySpatialVelocity();
|
||||
simStateClearVelMod(getSimStateData_Unchecked());
|
||||
|
||||
setActive(false);
|
||||
notifyPutToSleep();
|
||||
}
|
||||
|
||||
void BodySim::internalWakeUp(PxReal wakeCounterValue)
|
||||
{
|
||||
if(mArticulation)
|
||||
mArticulation->internalWakeUp(wakeCounterValue);
|
||||
else
|
||||
internalWakeUpBase(wakeCounterValue);
|
||||
}
|
||||
|
||||
void BodySim::internalWakeUpArticulationLink(PxReal wakeCounterValue)
|
||||
{
|
||||
PX_ASSERT(mArticulation);
|
||||
internalWakeUpBase(wakeCounterValue);
|
||||
}
|
||||
|
||||
void BodySim::internalWakeUpBase(PxReal wakeCounterValue) //this one can only increase the wake counter, not decrease it, so it can't be used to put things to sleep!
|
||||
{
|
||||
if ((!isKinematic()) && (getBodyCore().getWakeCounter() < wakeCounterValue))
|
||||
{
|
||||
PX_ASSERT(wakeCounterValue > 0.0f);
|
||||
getBodyCore().setWakeCounterFromSim(wakeCounterValue);
|
||||
|
||||
//we need to update the gpu body sim because we reset the wake counter for the body core
|
||||
mScene.updateBodySim(*this);
|
||||
setActive(true);
|
||||
notifyWakeUp();
|
||||
|
||||
if(0) // PT: commented-out for PX-2197
|
||||
mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::notifyReadyForSleeping()
|
||||
{
|
||||
if(mArticulation == NULL)
|
||||
mScene.getSimpleIslandManager()->deactivateNode(mNodeIndex);
|
||||
}
|
||||
|
||||
void BodySim::notifyNotReadyForSleeping()
|
||||
{
|
||||
mScene.getSimpleIslandManager()->activateNode(mNodeIndex);
|
||||
}
|
||||
|
||||
void BodySim::notifyWakeUp()
|
||||
{
|
||||
mScene.getSimpleIslandManager()->activateNode(mNodeIndex);
|
||||
}
|
||||
|
||||
void BodySim::notifyPutToSleep()
|
||||
{
|
||||
mScene.getSimpleIslandManager()->putNodeToSleep(mNodeIndex);
|
||||
}
|
||||
|
||||
//This function will be called by CPU sleepCheck code
|
||||
// PT: TODO: actually this seems to be only called by the articulation sim code, while regular rigid bodies use a copy of that code in LowLevelDynamics?
|
||||
PxReal BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity)
|
||||
{
|
||||
// update the body's sleep state and
|
||||
BodyCore& core = getBodyCore();
|
||||
|
||||
const PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;
|
||||
|
||||
PxReal wc = core.getWakeCounter();
|
||||
|
||||
{
|
||||
PxVec3 bcSleepLinVelAcc = mLLBody.mSleepLinVelAcc;
|
||||
PxVec3 bcSleepAngVelAcc = mLLBody.mSleepAngVelAcc;
|
||||
|
||||
if(wc < wakeCounterResetTime * 0.5f || wc < dt)
|
||||
{
|
||||
const PxTransform& body2World = getBody2World();
|
||||
|
||||
// calculate normalized energy: kinetic energy divided by mass
|
||||
const PxVec3 t = core.getInverseInertia();
|
||||
const PxVec3 inertia(t.x > 0.0f ? 1.0f/t.x : 1.0f, t.y > 0.0f ? 1.0f/t.y : 1.0f, t.z > 0.0f ? 1.0f/t.z : 1.0f);
|
||||
|
||||
PxVec3 sleepLinVelAcc = motionVelocity.linear;
|
||||
PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);
|
||||
|
||||
bcSleepLinVelAcc += sleepLinVelAcc;
|
||||
bcSleepAngVelAcc += sleepAngVelAcc;
|
||||
|
||||
PxReal invMass = core.getInverseMass();
|
||||
if(invMass == 0.0f)
|
||||
invMass = 1.0f;
|
||||
|
||||
const PxReal angular = bcSleepAngVelAcc.multiply(bcSleepAngVelAcc).dot(inertia) * invMass;
|
||||
const PxReal linear = bcSleepLinVelAcc.magnitudeSquared();
|
||||
const PxReal normalizedEnergy = 0.5f * (angular + linear);
|
||||
|
||||
// scale threshold by cluster factor (more contacts => higher sleep threshold)
|
||||
const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
|
||||
const PxReal threshold = clusterFactor*energyThreshold;
|
||||
|
||||
if (normalizedEnergy >= threshold)
|
||||
{
|
||||
PX_ASSERT(isActive());
|
||||
mLLBody.resetSleepFilter();
|
||||
const float factor = threshold == 0.0f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
|
||||
PxReal oldWc = wc;
|
||||
wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
|
||||
core.setWakeCounterFromSim(wc);
|
||||
if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
|
||||
notifyNotReadyForSleeping();
|
||||
|
||||
return wc;
|
||||
}
|
||||
}
|
||||
|
||||
mLLBody.mSleepLinVelAcc = bcSleepLinVelAcc;
|
||||
mLLBody.mSleepAngVelAcc = bcSleepAngVelAcc;
|
||||
}
|
||||
|
||||
wc = PxMax(wc-dt, 0.0f);
|
||||
core.setWakeCounterFromSim(wc);
|
||||
return wc;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void BodySim::initKinematicStateBase(BodyCore&, bool asPartOfCreation)
|
||||
{
|
||||
PX_ASSERT(!readInternalFlag(BF_KINEMATIC_MOVED));
|
||||
|
||||
if (!asPartOfCreation && isActive())
|
||||
mScene.swapInActiveBodyList(*this);
|
||||
|
||||
//mLLBody.setAccelerationV(Cm::SpatialVector::zero());
|
||||
|
||||
// Need to be before setting setRigidBodyFlag::KINEMATIC
|
||||
}
|
||||
|
||||
bool BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, PxU32& index, Cm::SpatialVector* acceleration,
|
||||
PxsExternalAccelerationProvider* externalAccelerations, PxU32 maxNumExternalAccelerations)
|
||||
{
|
||||
PxVec3 linVelDt(0.0f), angVelDt(0.0f);
|
||||
|
||||
const bool accDirty = readVelocityModFlag(VMF_ACC_DIRTY);
|
||||
const bool velDirty = readVelocityModFlag(VMF_VEL_DIRTY);
|
||||
|
||||
SimStateData* simStateData = NULL;
|
||||
|
||||
bool forceChangeApplied = false;
|
||||
|
||||
//if we change the logic like this, which means we don't need to have two separate variables in the pxgbodysim to represent linAcc and angAcc. However, this
|
||||
//means angAcc will be always 0
|
||||
if( (accDirty || velDirty) && ((simStateData = getSimStateData(false)) != NULL) )
|
||||
{
|
||||
VelocityMod* velmod = simStateData->getVelocityModData();
|
||||
|
||||
//we don't have support for articulation yet
|
||||
if (updatedBodySims)
|
||||
{
|
||||
updatedBodySims[index] = &getLowLevelBody();
|
||||
updatedBodyNodeIndices[index++] = getNodeIndex().index();
|
||||
}
|
||||
|
||||
if(velDirty)
|
||||
{
|
||||
linVelDt = velmod->getLinearVelModPerStep();
|
||||
angVelDt = velmod->getAngularVelModPerStep();
|
||||
}
|
||||
|
||||
if (accDirty)
|
||||
{
|
||||
linVelDt += velmod->getLinearVelModPerSec()*dt;
|
||||
angVelDt += velmod->getAngularVelModPerSec()*dt;
|
||||
}
|
||||
|
||||
if (acceleration)
|
||||
{
|
||||
const PxReal invDt = 1.f / dt;
|
||||
acceleration->linear = linVelDt * invDt;
|
||||
acceleration->angular = angVelDt * invDt;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (mScene.getFlags() & PxSceneFlag::eENABLE_EXTERNAL_FORCES_EVERY_ITERATION_TGS)
|
||||
{
|
||||
if (linVelDt != PxVec3(0.0f) || angVelDt != PxVec3(0.0f))
|
||||
{
|
||||
const PxReal invDt = 1.f / dt;
|
||||
PxsRigidBodyExternalAcceleration acc(linVelDt * invDt, angVelDt * invDt);
|
||||
externalAccelerations->setValue(acc, getNodeIndex().index(), maxNumExternalAccelerations);
|
||||
}
|
||||
}
|
||||
else
|
||||
getBodyCore().updateVelocities(linVelDt, angVelDt);
|
||||
}
|
||||
|
||||
forceChangeApplied = true;
|
||||
}
|
||||
|
||||
setForcesToDefaults(readVelocityModFlag(VMF_ACC_DIRTY));
|
||||
|
||||
return forceChangeApplied;
|
||||
}
|
||||
|
||||
void BodySim::onConstraintDetach()
|
||||
{
|
||||
PX_ASSERT(readInternalFlag(BF_HAS_CONSTRAINTS));
|
||||
|
||||
PxU32 size = getActorInteractionCount();
|
||||
Interaction** interactions = getActorInteractions();
|
||||
unregisterCountedInteraction();
|
||||
|
||||
while(size--)
|
||||
{
|
||||
const Interaction* interaction = *interactions++;
|
||||
if(interaction->getType() == InteractionType::eCONSTRAINTSHADER)
|
||||
return;
|
||||
}
|
||||
|
||||
clearInternalFlag(BF_HAS_CONSTRAINTS); // There are no other constraint interactions left
|
||||
}
|
||||
|
||||
void BodySim::setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex)
|
||||
{
|
||||
mArticulation = a;
|
||||
if(a)
|
||||
{
|
||||
PxNodeIndex index = mArticulation->getIslandNodeIndex();
|
||||
mNodeIndex.setIndices(index.index(), bodyIndex);
|
||||
getBodyCore().setWakeCounterFromSim(wakeCounter);
|
||||
|
||||
if (getFlagsFast() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
|
||||
mScene.setSpeculativeCCDArticulationLink(mNodeIndex.index());
|
||||
|
||||
//Articulations defer registering their shapes with the nphaseContext until the IG node index is known.
|
||||
{
|
||||
// PT: TODO: skip this on CPU
|
||||
PxvNphaseImplementationContext* ctx = mScene.getLowLevelContext()->getNphaseImplementationContext();
|
||||
ElementSim** current = getElements();
|
||||
PxU32 nbElements = getNbElements();
|
||||
while (nbElements--)
|
||||
{
|
||||
ShapeSim* sim = static_cast<ShapeSim*>(*current++);
|
||||
ctx->registerShape(mNodeIndex, sim->getCore().getCore(), sim->getElementID(), sim->getActor().getPxActor());
|
||||
}
|
||||
}
|
||||
|
||||
//Force node index into LL shapes
|
||||
{
|
||||
PxsSimulationController* sc = getScene().getSimulationController();
|
||||
if(sc->mGPU)
|
||||
{
|
||||
const PxNodeIndex nodeIndex = mNodeIndex;
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
|
||||
sc->setPxgShapeBodyNodeIndex(nodeIndex, sim->getElementID());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (a->getCore().getArticulationFlags() & PxArticulationFlag::eDISABLE_SELF_COLLISION)
|
||||
{
|
||||
//We need to reset the group IDs for all shapes in this body...
|
||||
ElementSim** current = getElements();
|
||||
PxU32 nbElements = getNbElements();
|
||||
|
||||
Bp::AABBManagerBase* aabbMgr = mScene.getAABBManager();
|
||||
|
||||
Bp::FilterGroup::Enum rootGroup = Bp::getFilterGroup(false, a->getRootActorIndex(), false);
|
||||
|
||||
while (nbElements--)
|
||||
{
|
||||
ShapeSim* sim = static_cast<ShapeSim*>(*current++);
|
||||
aabbMgr->setBPGroup(sim->getElementID(), rootGroup);
|
||||
}
|
||||
}
|
||||
|
||||
if (!asleep)
|
||||
{
|
||||
setActive(true);
|
||||
notifyWakeUp();
|
||||
}
|
||||
else
|
||||
{
|
||||
notifyReadyForSleeping();
|
||||
notifyPutToSleep();
|
||||
setActive(false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//Setting a 1 in the articulation ID to avoid returning the node Index to the node index
|
||||
//manager
|
||||
mNodeIndex.setIndices(PX_INVALID_NODE, 1);
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::createSqBounds()
|
||||
{
|
||||
if(!isActive() || usingSqKinematicTarget() || readInternalFlag(BF_IS_COMPOUND_RIGID))
|
||||
return;
|
||||
|
||||
PX_ASSERT(!isFrozen());
|
||||
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* current = static_cast<ShapeSim*>(*elems++);
|
||||
current->createSqBounds();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::destroySqBounds()
|
||||
{
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* current = static_cast<ShapeSim*>(*elems++);
|
||||
current->destroySqBounds();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::freezeTransforms(PxBitMapPinned* shapeChangedMap)
|
||||
{
|
||||
PxU32 nbElems = getNbElements();
|
||||
ElementSim** elems = getElements();
|
||||
while (nbElems--)
|
||||
{
|
||||
ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
|
||||
sim->updateCached(PxsTransformFlag::eFROZEN, shapeChangedMap);
|
||||
sim->destroySqBounds();
|
||||
}
|
||||
}
|
||||
|
||||
void BodySim::disableCompound()
|
||||
{
|
||||
if(isActive())
|
||||
mScene.removeFromActiveCompoundBodyList(*this);
|
||||
clearInternalFlag(BF_IS_COMPOUND_RIGID);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user