Revision control
++ // 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 "ScBodySim.h"
++ #include "ScShapeSim.h"
++ #include "ScArticulationSim.h"
++ #include "ScArticulationCore.h"
++
++ using namespace physx;
++ using namespace Dy;
++ using namespace Sc;
++ using namespace Cm;
++
++ #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),
++ mArticulation (NULL)
++ {
++ PxU16 internalFlags = mLLBody.mInternalFlags | VMF_GRAVITY_DIRTY;
++
++ core.getCore().numCountedInteractions = 0;
++ core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
++
++ if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
++ internalFlags |= PxsRigidBody::eSPECULATIVE_CCD_GPU;
++
++ if(core.getFlags() & PxRigidBodyFlag::eENABLE_GYROSCOPIC_FORCES)
++ internalFlags |= PxsRigidBody::eENABLE_GYROSCOPIC_GPU;
++
++ if(core.getFlags() & PxRigidBodyFlag::eRETAIN_ACCELERATIONS)
++ internalFlags |= PxsRigidBody::eRETAIN_ACCELERATION_GPU;
++
++ mLLBody.mInternalFlags = internalFlags;
++
++ // 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
++ {
++ // PT: ?? can mArticulation be non null here?
++ 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);
++ }
++
++ 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 ((mLLBody.mInternalFlags & VMF_ALL_FLAGS) == 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()
++ {
++ // Reset acceleration state only on genuine sleep-to-wake transition for
++ // dynamic bodies. Kinematic sleep/wake is a normal part of their lifecycle
++ // (sleep when idle, wake on setKinematicTarget) and prevVel is still valid.
++ const bool resetAccel = !isActive() && !isKinematic();
++
++ setActive(true);
++ notifyWakeUp();
++
++ if(resetAccel)
++ raiseInternalFlag(BF_RESET_ACCELERATION);
++ }
++
++ 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.gpu_updateBodySim(*this);
++ setActive(true);
++ notifyWakeUp();
++
++ // Reset acceleration state to avoid spurious spike after wake-up
++ raiseInternalFlag(BF_RESET_ACCELERATION);
++
++ 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);
++ }
++
++ bool BodySim::checkSleepReadinessBesidesWakeCounter()
++ {
++ // If sleeping is disabled in the scene, bodies are never ready for sleep
++ if(mScene.getFlags() & PxSceneFlag::eDISABLE_SLEEPING)
++ return false;
++
++ const BodyCore& bodyCore = getBodyCore();
++ const SimStateData* simStateData = getSimStateData(false);
++ const VelocityMod* velmod = simStateData ? simStateData->getVelocityModData() : NULL;
++
++ bool readyForSleep = bodyCore.getLinearVelocity().isZero() && bodyCore.getAngularVelocity().isZero();
++
++ if (readVelocityModFlag(VMF_ACC_DIRTY))
++ {
++ readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerSec().isZero());
++ readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerSec().isZero());
++ }
++
++ if (readVelocityModFlag(VMF_VEL_DIRTY))
++ {
++ readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerStep().isZero());
++ readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerStep().isZero());
++ }
++
++ return readyForSleep;
++ }
++
++ 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();
++
++ if(wc < wakeCounterResetTime * 0.5f || wc < dt)
++ {
++ PxVec3 bcSleepLinVelAcc = mLLBody.mSleepLinVelAcc;
++ PxVec3 bcSleepAngVelAcc = mLLBody.mSleepAngVelAcc;
++
++ {
++ 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);
++
++ const 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
++ {
++ // PdHC: For GPU dynamics mode with body accelerations enabled, we must use external
++ // accelerations instead of directly modifying CPU velocity. This ensures that the
++ // velocity-delta method for acceleration computation works correctly:
++ // - If we modify CPU velocity directly, the "previous" velocity capture sees the post-force
++ // velocity, causing the force effect to cancel out in the delta.
++ // - With external accelerations, the GPU integrates forces during simulation, so
++ // velocity-delta correctly captures: accel = (curVel - prevVel) / dt = gravity + force/mass
++ // Note: Only enable for GPU when eENABLE_BODY_ACCELERATIONS is set, otherwise use the
++ // original updateVelocities path to avoid breaking eRETAIN_ACCELERATIONS behavior.
++ const bool useExternalAccelerations =
++ (mScene.getFlags() & PxSceneFlag::eENABLE_EXTERNAL_FORCES_EVERY_ITERATION_TGS) ||
++ (mScene.isUsingGpuDynamics() && (mScene.getFlags() & PxSceneFlag::eENABLE_BODY_ACCELERATIONS));
++
++ if (useExternalAccelerations)
++ {
++ 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(), 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::disableCompound()
++ {
++ if(isActive())
++ mScene.removeFromActiveCompoundBodyList(*this);
++ clearInternalFlag(BF_IS_COMPOUND_RIGID);
++ }
++