Revision control
Copy as Markdown
// 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 "ScArticulationSim.h"
#include "ScArticulationCore.h"
#include "ScArticulationJointCore.h"
#include "ScBodySim.h"
#include "ScArticulationTendonSim.h"
#include "ScArticulationMimicJointSim.h"
#include "ScScene.h"
#include "ScShapeSim.h"
#include "DyIslandManager.h"
using namespace physx;
using namespace Dy;
using namespace Sc;
using namespace IG;
using namespace Cm;
ArticulationSim* Sc::getArticulationSim(const IslandSim& islandSim, PxNodeIndex nodeIndex)
{
return static_cast<ArticulationSim*>(getArticulationFromIG(islandSim, nodeIndex));
}
Sc::ArticulationSim::ArticulationSim(ArticulationCore& core, Scene& scene, BodyCore& root) :
mScene (scene),
mCore (core),
mLinks ("ScArticulationSim::links"),
mBodies ("ScArticulationSim::bodies"),
mJoints ("ScArticulationSim::joints"),
mIsLLArticulationInitialized(false)
{
mLinks.reserve(16);
mJoints.reserve(16);
mBodies.reserve(16);
mIslandNodeIndex = scene.getSimpleIslandManager()->addNode(false, false, IG::Node::eARTICULATION_TYPE, this);
PX_ASSERT(root.getSim());
addBody(*root.getSim(), NULL, NULL);
mCore.setSim(this);
initData(&core.getCore(), NULL);
}
Sc::ArticulationSim::~ArticulationSim()
{
mScene.getSimpleIslandManager()->removeNode(mIslandNodeIndex);
mCore.setSim(NULL);
}
PxU32 Sc::ArticulationSim::findBodyIndex(BodySim& body) const
{
for(PxU32 i=0; i<mBodies.size(); i++)
{
if(mBodies[i]==&body)
return i;
}
PX_ASSERT(0);
return 0x80000000;
}
void Sc::ArticulationSim::addLoopConstraint(ConstraintSim* constraintSim)
{
const PxU32 size = mLoopConstraints.size();
if (size < mLoopConstraints.size())
mLoopConstraints.reserve(size*2 + 1);
BodySim* bodySim0 = constraintSim->getBody(0);
BodySim* bodySim1 = constraintSim->getBody(1);
ArticulationLoopConstraint lConstraint;
if (bodySim0)
lConstraint.linkIndex0 = findBodyIndex(*bodySim0);
else
lConstraint.linkIndex0 = 0x80000000;
if(bodySim1)
lConstraint.linkIndex1 = findBodyIndex(*bodySim1);
else
lConstraint.linkIndex1 = 0x80000000;
lConstraint.constraint = &constraintSim->getLowLevelConstraint();
mLoopConstraints.pushBack(lConstraint);
}
void Sc::ArticulationSim::removeLoopConstraint(ConstraintSim* constraintSim)
{
Dy::Constraint* constraint = &constraintSim->getLowLevelConstraint();
const PxU32 size = mLoopConstraints.size();
PxU32 index = 0;
while (index < size && mLoopConstraints[index].constraint != constraint)
++index;
if (index != size)
mLoopConstraints.replaceWithLast(index);
}
void Sc::ArticulationSim::addBody(BodySim& body, BodySim* parent, ArticulationJointSim* joint)
{
mBodies.pushBack(&body);
mJoints.pushBack(joint);
FeatherstoneArticulation::addBody();
const PxU32 index = mLinks.size();
PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) ||
(((index!=0) && joint) && (parent && (parent->getArticulation() == this))));
ArticulationLink& link = *mLinks.insert();
link.initBody(&body.getBodyCore().getCore());
bool shouldSleep;
bool currentlyAsleep;
const bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter();
const PxReal wakeCounter = getCore().getWakeCounter();
if(parent)
{
currentlyAsleep = !mBodies[0]->isActive();
shouldSleep = currentlyAsleep && bodyReadyForSleep;
const PxU32 parentIndex = findBodyIndex(*parent);
link.initJoint(&joint->getCore().getCore(), parentIndex);
ArticulationLink& parentLink = mLinks[parentIndex];
if (parentLink.mChildrenStartIndex == 0xffffffff)
parentLink.mChildrenStartIndex = index;
parentLink.mNumChildren++;
}
else
{
link.initJoint(NULL, DY_ARTICULATION_LINK_NONE);
currentlyAsleep = (wakeCounter == 0.0f);
shouldSleep = currentlyAsleep && bodyReadyForSleep;
}
if(currentlyAsleep && !shouldSleep)
{
for(PxU32 i=0; i < (mBodies.size() - 1); i++)
mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
}
body.setArticulation(this, wakeCounter, shouldSleep, index);
}
void Sc::ArticulationSim::removeBody(BodySim& body)
{
for (PxU32 i = 0; i < mBodies.size(); ++i)
{
if (mBodies[i] == &body)
{
mBodies.replaceWithLast(i);
mJoints.replaceWithLast(i);
break;
}
}
}
void Sc::ArticulationSim::addTendon(ArticulationSpatialTendonSim* const tendonSim)
{
tendonSim->mArtiSim = this;
const PxU32 index = mSpatialTendons.size();
Dy::ArticulationSpatialTendon& llTendon = tendonSim->mLLTendon;
llTendon.setTendonIndex(index);
mSpatialTendons.pushBack(&llTendon);
//mSpatialTendons.pushBack(&tendonSim->mLLTendon);
}
void Sc::ArticulationSim::addTendon(ArticulationFixedTendonSim* const tendonSim)
{
tendonSim->mArtiSim = this;
const PxU32 index = mFixedTendons.size();
Dy::ArticulationFixedTendon& llTendon = tendonSim->mLLTendon;
llTendon.setTendonIndex(index);
mFixedTendons.pushBack(&llTendon);
}
void Sc::ArticulationSim::addMimicJoint(ArticulationMimicJointSim* const mimicJointSim, const PxU32 linkA, const PxU32 linkB)
{
const PxU32 index = mMimicJoints.size();
mimicJointSim->setLowLevelIndex(index);
mimicJointSim->mArticulationSim = this;
Dy::ArticulationMimicJointCore& llMimicJoint = mimicJointSim->getLLMimicJoint();
llMimicJoint.linkA = linkA;
llMimicJoint.linkB = linkB;
mMimicJoints.pushBack(&llMimicJoint);
}
void Sc::ArticulationSim::createLLStructure()
{
if(!mBodies.size())
return;
setupLinks(mLinks.size(), mLinks.begin());
assignTendons(mSpatialTendons.size(), mSpatialTendons.begin());
assignTendons(mFixedTendons.size(), mFixedTendons.begin());
assignMimicJoints(mMimicJoints.size(), mMimicJoints.begin());
mIsLLArticulationInitialized = true;
}
void Sc::ArticulationSim::initializeConfiguration()
{
Dy::ArticulationData& data = getArticulationData();
jcalc(data);
mJcalcDirty = false;
const Dy::ArticulationLink* PX_RESTRICT links = data.getLinks();
const Dy::ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();
const PxU32 linkCount = data.getLinkCount();
PxReal* PX_RESTRICT jointVelocites = data.getJointVelocities();
PxReal* PX_RESTRICT jointPositions = data.getJointPositions();
PxReal* PX_RESTRICT jointTargetPositions = data.getJointTargetPositions();
PxReal* PX_RESTRICT jointTargetVelocities = data.getJointTargetVelocities();
for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
{
const Dy::ArticulationLink& link = links[linkID];
const Dy::ArticulationJointCore* joint = link.inboundJoint;
const Dy::ArticulationJointCoreData& jointDatum = jointData[linkID];
PxReal* jPositions = &jointPositions[jointDatum.jointOffset];
PxReal* jVelocites = &jointVelocites[jointDatum.jointOffset];
PxReal* jTargetPositions = &jointTargetPositions[jointDatum.jointOffset];
PxReal* jTargetVelocities = &jointTargetVelocities[jointDatum.jointOffset];
for (PxU8 i = 0; i < jointDatum.nbDof; ++i)
{
const PxU32 dofId = joint->dofIds[i];
jPositions[i] = joint->jointPos[dofId];
jVelocites[i] = joint->jointVel[dofId];
jTargetPositions[i] = joint->targetP[dofId];
jTargetVelocities[i] = joint->targetV[dofId];
}
}
PxU32 flags = (Dy::ArticulationDirtyFlag::eDIRTY_POSITIONS |
Dy::ArticulationDirtyFlag::eDIRTY_VELOCITIES |
Dy::ArticulationDirtyFlag::eDIRTY_JOINT_TARGET_POS |
Dy::ArticulationDirtyFlag::eDIRTY_JOINT_TARGET_VEL);
raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::Enum(flags));
initPathToRoot();
}
void Sc::ArticulationSim::updateKinematic(PxArticulationKinematicFlags flags)
{
Dy::ArticulationData& data = getArticulationData();
if (mJcalcDirty)
{
jcalc(data);
mJcalcDirty = false;
}
if ((flags & PxArticulationKinematicFlag::ePOSITION))
{
raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::eDIRTY_POSITIONS);
teleportLinks(data);
}
if ((flags & PxArticulationKinematicFlag::ePOSITION) ||
(flags & PxArticulationKinematicFlag::eVELOCITY))
{
raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::eDIRTY_VELOCITIES);
computeLinkVelocities(data);
}
}
void Sc::ArticulationSim::copyJointStatus(const PxU32 linkID)
{
const Dy::ArticulationData& data = getArticulationData();
const Dy::ArticulationLink* PX_RESTRICT links = data.getLinks();
const Dy::ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();
const Dy::ArticulationLink& link = links[linkID];
Dy::ArticulationJointCore* PX_RESTRICT joint = link.inboundJoint;
const Dy::ArticulationJointCoreData& jointDatum = jointData[linkID];
const PxReal* PX_RESTRICT jointVelocites = data.getJointVelocities();
const PxReal* PX_RESTRICT jointPositions = data.getJointPositions();
const PxReal* PX_RESTRICT jVelocities = &jointVelocites[jointDatum.jointOffset];
const PxReal* PX_RESTRICT jPositions = &jointPositions[jointDatum.jointOffset];
for(PxU8 i = 0; i < jointDatum.nbDof; ++i)
{
const PxU32 dofId = joint->dofIds[i];
joint->jointPos[dofId] = jPositions[i];
joint->jointVel[dofId] = jVelocities[i];
}
}
void Sc::ArticulationSim::putToSleep()
{
for (PxU32 i = 0; i < mLinks.size(); i++)
{
BodySim* bodySim = mBodies[i];
PxsRigidBody& rigid = bodySim->getLowLevelBody();
PxsBodyCore& bodyCore = bodySim->getBodyCore().getCore();
//rigid.setPose(rigid.getLastCCDTransform());
//KS - the IG deactivates bodies in parallel with the solver. It appears that under certain circumstances, the solver's integration (which performs
//sleep checks) could decide that the body is no longer a candidate for sleeping on the same frame that the island gen decides to deactivate the island
//that the body is contained in. This is a rare occurrence but the behavior we want to emulate is that of IG running before solver so we should therefore
//permit the IG to make the authoritative decision over whether the body should be active or inactive.
bodyCore.wakeCounter = 0.0f;
bodyCore.linearVelocity = PxVec3(0.0f);
bodyCore.angularVelocity = PxVec3(0.0f);
rigid.clearAllFrameFlags();
//Force update
}
// Zero joint velocities as documented in PxArticulationReducedCoordinate::putToSleep()
// First zero them in the Sc::ArticulationJointCore objects via the joints array
// This is what getJointVelocity() reads from when LL articulation is not initialized
for (PxU32 i = 1; i < mJoints.size(); ++i) // Start from 1 since root has no joint
{
ArticulationJointSim* jointSim = mJoints[i];
if (jointSim)
{
Sc::ArticulationJointCore& jointCore = jointSim->getCore();
// Zero all the cached joint velocities in the Sc layer
for (PxU32 axis = 0; axis < PxArticulationAxis::eCOUNT; ++axis)
{
jointCore.getCore().jointVel[axis] = 0.0f;
}
}
}
// Only access low-level articulation data if it's initialized
if (mIsLLArticulationInitialized)
{
Dy::ArticulationData& articulationData = getArticulationData();
const PxU32 dofs = articulationData.getDofs();
if (dofs > 0)
{
const PxU32 jointDataSize = sizeof(PxReal) * dofs;
// Zero the joint velocities
PxReal* jointVelocities = articulationData.getJointVelocities();
if (jointVelocities)
PxMemZero(jointVelocities, jointDataSize);
// Also zero the new joint velocities (used for solver updates)
PxReal* jointNewVelocities = articulationData.getJointNewVelocities();
if (jointNewVelocities)
PxMemZero(jointNewVelocities, jointDataSize);
// Also zero position iteration joint velocities to be thorough
PxReal* posIterJointVelocities = articulationData.getPosIterJointVelocities();
if (posIterJointVelocities)
PxMemZero(posIterJointVelocities, jointDataSize);
}
}
mScene.getSimulationController()->updateArticulation(this, mIslandNodeIndex);
}
void Sc::ArticulationSim::sleepCheck(PxReal dt, PxMutex& articulationSleepLock)
{
if(!mBodies.size())
return;
#if PX_CHECKED
{
PxReal maxTimer = 0.0f, minTimer = PX_MAX_F32;
bool allActive = true, noneActive = true;
PX_UNUSED(allActive);
PX_UNUSED(noneActive);
for(PxU32 i=0;i<mLinks.size();i++)
{
PxReal timer = mBodies[i]->getBodyCore().getWakeCounter();
maxTimer = PxMax(maxTimer, timer);
minTimer = PxMin(minTimer, timer);
bool active = mBodies[i]->isActive();
allActive &= active;
noneActive &= !active;
}
// either all links are asleep, or no links are asleep
PX_ASSERT(maxTimer==0 || minTimer!=0);
PX_ASSERT(allActive || noneActive);
}
#endif
if(!mBodies[0]->isActive())
return;
const PxReal sleepThreshold = getCore().getCore().sleepThreshold;
PxReal maxTimer = 0.0f , minTimer = PX_MAX_F32;
const PxU32 nbLinks = mLinks.size();
for(PxU32 i=0; i<nbLinks; i++)
{
const Cm::SpatialVector& motionVelocity = getMotionVelocity(i);
const PxReal timer = mBodies[i]->updateWakeCounter(dt, sleepThreshold, motionVelocity);
maxTimer = PxMax(maxTimer, timer);
minTimer = PxMin(minTimer, timer);
}
mCore.setWakeCounterInternal(maxTimer);
if(maxTimer != 0.0f)
{
if(minTimer == 0.0f)
{
// make sure nothing goes to sleep unless everything does
for(PxU32 i=0; i<nbLinks; i++)
mBodies[i]->getBodyCore().setWakeCounterFromSim(PxMax(1e-6f, mBodies[i]->getBodyCore().getWakeCounter()));
}
return;
}
// PT: we reach this when the articulation goes to sleep
for(PxU32 i=0; i<nbLinks; i++)
{
//mBodies[i]->notifyReadyForSleeping(); // PT: TODO: this seems unnecessary
mBodies[i]->getLowLevelBody().resetSleepFilter();
}
// /PT: we are calling this function from multiple threads but deactivateNode() is not thread-safe. Using a lock as an initial fix.
articulationSleepLock.lock();
mScene.getSimpleIslandManager()->deactivateNode(mIslandNodeIndex);
articulationSleepLock.unlock();
}
bool Sc::ArticulationSim::isSleeping() const
{
return (mBodies.size() > 0) ? (!mBodies[0]->isActive()) : true;
}
void Sc::ArticulationSim::internalWakeUp(PxReal wakeCounter)
{
if(mCore.getWakeCounter() < wakeCounter)
{
mCore.setWakeCounterInternal(wakeCounter);
for(PxU32 i=0;i<mBodies.size();i++)
mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
}
}
void Sc::ArticulationSim::updateForces(PxReal dt)
{
PxU32 count = 0;
bool anyForcesApplied = false;
for(PxU32 i=0;i<mBodies.size();i++)
{
if (i+1 < mBodies.size())
{
PxPrefetchLine(mBodies[i+1],128);
PxPrefetchLine(mBodies[i+1],256);
}
Cm::SpatialVector* accelerations = mAcceleration.begin();
anyForcesApplied |= mBodies[i]->updateForces(dt, NULL, NULL, count, &accelerations[i], NULL);
}
if(anyForcesApplied)
raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::eDIRTY_EXT_ACCEL);
}
void Sc::ArticulationSim::clearAcceleration(PxReal dt)
{
PxU32 count = 0;
bool anyBodyRetains = false;
for (PxU32 i = 0; i < mBodies.size(); i++)
{
if (i + 1 < mBodies.size())
{
PxPrefetchLine(mBodies[i + 1], 128);
PxPrefetchLine(mBodies[i + 1], 256);
}
const bool accDirty = mBodies[i]->readVelocityModFlag(VMF_ACC_DIRTY);
// the code restores the pre-impulse state:
// if we only applied an impulse and no acceleration, we clear the acceleration here.
// if we applied an acceleration, we re-apply the acceleration terms we have in the velMod.
// we cleared out the impulse here when we pushed the data at the start of the sim.
Cm::SpatialVector* accelerations = mAcceleration.begin();
if (!accDirty)
{
accelerations[i].linear = PxVec3(0.f);
accelerations[i].angular = PxVec3(0.f);
}
else
{
mBodies[i]->updateForces(dt, NULL, NULL, count, &accelerations[i], NULL);
}
// we need to raise the dirty flag if retain accelerations is on
// because in that case we need to restore the acceleration without impulses. We
// can only do that using the CPU->GPU codepath because we don't distinguish between
// acceleration and impulses on the GPU.
// The flag must be raised here because we don't know at the start of the next sim step
// that the data in velMod is actually valid and the articulation would not be added
// to the dirty list.
// without retain accelerations, the accelerations are cleared directly on the GPU.
if (mBodies[i]->getFlagsFast() & PxRigidBodyFlag::eRETAIN_ACCELERATIONS)
anyBodyRetains = true;
}
if (anyBodyRetains)
{
mScene.getSimulationController()->updateArticulationExtAccel(this, mIslandNodeIndex);
}
}
void Sc::ArticulationSim::saveLastCCDTransform()
{
for(PxU32 i=0;i<mBodies.size();i++)
{
if (i+1 < mBodies.size())
{
PxPrefetchLine(mBodies[i+1],128);
PxPrefetchLine(mBodies[i+1],256);
}
mBodies[i]->getLowLevelBody().saveLastCCDTransform();
}
}
void Sc::ArticulationSim::setFixedBaseLink(bool value)
{
const PxU32 linkCount = mLinks.size();
if(linkCount > 0)
mLinks[0].bodyCore->fixedBaseLink = PxU8(value);
}
PX_COMPILE_TIME_ASSERT(sizeof(Cm::SpatialVector)==sizeof(PxSpatialForce));
PxArticulationCache* Sc::ArticulationSim::createCache()
{
return FeatherstoneArticulation::createCache(getDofs(), mLinks.size());
}
PxU32 Sc::ArticulationSim::getCacheDataSize() const
{
return FeatherstoneArticulation::getCacheDataSize(getDofs(), mLinks.size());
}
void Sc::ArticulationSim::zeroCache(PxArticulationCache& cache) const
{
const PxU32 cacheDataSize = getCacheDataSize();
PxMemZero(cache.externalForces, cacheDataSize);
}
//copy external data to internal data
bool Sc::ArticulationSim::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag)
{
//checkResize();
bool shouldWake = false;
if (FeatherstoneArticulation::applyCache(cache, flag, shouldWake))
{
mScene.getSimulationController()->updateArticulation(this, mIslandNodeIndex);
}
return shouldWake;
}
void Sc::ArticulationSim::computeGeneralizedGravityForce(PxArticulationCache& cache)
{
getGeneralizedGravityForce(mScene.getGravity(), cache);
}
void Sc::ArticulationSim::computeJointAcceleration(PxArticulationCache& cache)
{
getJointAcceleration(mScene.getGravity(), cache);
}
bool Sc::ArticulationSim::computeLambda_Deprecated(PxArticulationCache& cache, PxArticulationCache& initialState,
const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter)
{
const PxReal invLengthScale = 1.f / mScene.getLengthScale();
return getLambda_Deprecated(mScene.getDynamicsContext()->getConstraintWriteBackPool(), mLoopConstraints.begin(), mLoopConstraints.size(), cache, initialState, jointTorque, gravity, maxIter, invLengthScale);
}
PxU32 Sc::ArticulationSim::getCoefficientMatrixSize_Deprecated() const
{
const PxU32 size = mLoopConstraints.size();
const PxU32 totalDofs = getDofs();
return size * totalDofs;
}
void Sc::ArticulationSim::setJointDirty(Dy::ArticulationJointCore& jointCore)
{
PX_UNUSED(jointCore);
mScene.getSimulationController()->updateArticulationJoint(this, mIslandNodeIndex);
}
void Sc::ArticulationSim::setArticulationDirty(PxU32 flag)
{
raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::Enum(flag));
mScene.getSimulationController()->updateArticulation(this, mIslandNodeIndex);
}
void Sc::ArticulationSim::debugCheckWakeCounterOfLinks(PxReal wakeCounter) const
{
PX_UNUSED(wakeCounter);
#if PX_DEBUG
// make sure the links are in sync with the articulation
for(PxU32 i=0; i < mBodies.size(); i++)
{
PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == wakeCounter);
}
#endif
}
void Sc::ArticulationSim::debugCheckSleepStateOfLinks(bool isSleeping) const
{
PX_UNUSED(isSleeping);
#if PX_DEBUG
// make sure the links are in sync with the articulation
for(PxU32 i=0; i < mBodies.size(); i++)
{
if (isSleeping)
{
PX_ASSERT(!mBodies[i]->isActive());
PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == 0.0f);
PX_ASSERT(mBodies[i]->checkSleepReadinessBesidesWakeCounter());
}
else
PX_ASSERT(mBodies[i]->isActive());
}
#endif
}
PxU32 Sc::ArticulationSim::getRootActorIndex() const
{
return mBodies[0]->getActorID();
}