826 lines
24 KiB
C++
826 lines
24 KiB
C++
// 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 "ScArticulationSim.h"
|
|
#include "ScArticulationCore.h"
|
|
#include "ScArticulationJointSim.h"
|
|
#include "ScArticulationJointCore.h"
|
|
#include "ScBodySim.h"
|
|
#include "ScConstraintSim.h"
|
|
#include "ScArticulationTendonSim.h"
|
|
#include "ScArticulationMimicJointSim.h"
|
|
#include "ScScene.h"
|
|
|
|
#include "DyConstraint.h"
|
|
#include "DyFeatherstoneArticulation.h"
|
|
#include "PxsContext.h"
|
|
#include "CmSpatialVector.h"
|
|
#include "foundation/PxVecMath.h"
|
|
#include "PxsSimpleIslandManager.h"
|
|
#include "ScShapeSim.h"
|
|
#include "PxsSimulationController.h"
|
|
#include "DyIslandManager.h"
|
|
|
|
using namespace physx;
|
|
using namespace Dy;
|
|
using namespace Sc;
|
|
using namespace IG;
|
|
|
|
ArticulationSim* Sc::getArticulationSim(const IslandSim& islandSim, PxNodeIndex nodeIndex)
|
|
{
|
|
void* userData = getArticulationFromIG(islandSim, nodeIndex)->getUserData();
|
|
return reinterpret_cast<ArticulationSim*>(userData);
|
|
}
|
|
|
|
Sc::ArticulationSim::ArticulationSim(ArticulationCore& core, Scene& scene, BodyCore& root) :
|
|
mLLArticulation (NULL),
|
|
mScene (scene),
|
|
mCore (core),
|
|
mLinks ("ScArticulationSim::links"),
|
|
mBodies ("ScArticulationSim::bodies"),
|
|
mJoints ("ScArticulationSim::joints"),
|
|
mMaxDepth (0),
|
|
mIsLLArticulationInitialized(false)
|
|
{
|
|
mLinks.reserve(16);
|
|
mJoints.reserve(16);
|
|
mBodies.reserve(16);
|
|
|
|
mLLArticulation = mScene.createLLArticulation(this);
|
|
|
|
mIslandNodeIndex = scene.getSimpleIslandManager()->addNode(false, false, IG::Node::eARTICULATION_TYPE, mLLArticulation);
|
|
|
|
if(!mLLArticulation)
|
|
{
|
|
PxGetFoundation().error(PxErrorCode::eINTERNAL_ERROR, PX_FL, "Articulation: could not allocate low-level resources.");
|
|
return;
|
|
}
|
|
|
|
PX_ASSERT(root.getSim());
|
|
|
|
addBody(*root.getSim(), NULL, NULL);
|
|
|
|
mCore.setSim(this);
|
|
|
|
mLLArticulation->setDyContext(mScene.getDynamicsContext());
|
|
mLLArticulation->getSolverDesc().initData(&core.getCore(), NULL);
|
|
|
|
//mLLArticulation->onUpdateSolverDesc();
|
|
}
|
|
|
|
Sc::ArticulationSim::~ArticulationSim()
|
|
{
|
|
if (!mLLArticulation)
|
|
return;
|
|
|
|
mScene.destroyLLArticulation(*mLLArticulation);
|
|
|
|
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::updateCached(PxBitMapPinned* shapeChangedMap)
|
|
{
|
|
for(PxU32 i=0; i<mBodies.size(); i++)
|
|
mBodies[i]->updateCached(shapeChangedMap);
|
|
}
|
|
|
|
void Sc::ArticulationSim::markShapesUpdated(PxBitMapPinned* shapeChangedMap)
|
|
{
|
|
for (PxU32 a = 0; a < mBodies.size(); ++a)
|
|
{
|
|
PxU32 nbElems = mBodies[a]->getNbElements();
|
|
ElementSim** elems = mBodies[a]->getElements();
|
|
while (nbElems--)
|
|
{
|
|
ShapeSim* sim = static_cast<ShapeSim*>(*elems++);
|
|
if (sim->isInBroadPhase())
|
|
shapeChangedMap->growAndSet(sim->getElementID());
|
|
}
|
|
}
|
|
}
|
|
|
|
void Sc::ArticulationSim::addBody(BodySim& body, BodySim* parent, ArticulationJointSim* joint)
|
|
{
|
|
mBodies.pushBack(&body);
|
|
mJoints.pushBack(joint);
|
|
mLLArticulation->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.bodyCore = &body.getBodyCore().getCore();
|
|
link.mPathToRootStartIndex = 0;
|
|
link.mPathToRootCount = 0;
|
|
link.mChildrenStartIndex = 0xffffffff;
|
|
link.mNumChildren = 0;
|
|
bool shouldSleep;
|
|
bool currentlyAsleep;
|
|
const bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter();
|
|
const PxReal wakeCounter = getCore().getWakeCounter();
|
|
|
|
if(parent)
|
|
{
|
|
currentlyAsleep = !mBodies[0]->isActive();
|
|
shouldSleep = currentlyAsleep && bodyReadyForSleep;
|
|
|
|
PxU32 parentIndex = findBodyIndex(*parent);
|
|
link.parent = parentIndex;
|
|
ArticulationLink& parentLink = mLinks[parentIndex];
|
|
link.inboundJoint = &joint->getCore().getCore();
|
|
|
|
if (parentLink.mChildrenStartIndex == 0xffffffff)
|
|
parentLink.mChildrenStartIndex = index;
|
|
|
|
parentLink.mNumChildren++;
|
|
|
|
}
|
|
else
|
|
{
|
|
currentlyAsleep = (wakeCounter == 0.0f);
|
|
shouldSleep = currentlyAsleep && bodyReadyForSleep;
|
|
|
|
link.parent = DY_ARTICULATION_LINK_NONE;
|
|
link.inboundJoint = NULL;
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
mLLArticulation->setupLinks(mLinks.size(), mLinks.begin());
|
|
|
|
mLLArticulation->assignTendons(mSpatialTendons.size(), mSpatialTendons.begin());
|
|
|
|
mLLArticulation->assignTendons(mFixedTendons.size(), mFixedTendons.begin());
|
|
|
|
mLLArticulation->assignMimicJoints(mMimicJoints.size(), mMimicJoints.begin());
|
|
|
|
mIsLLArticulationInitialized = true;
|
|
}
|
|
|
|
void Sc::ArticulationSim::initializeConfiguration()
|
|
{
|
|
Dy::ArticulationData& data = mLLArticulation->getArticulationData();
|
|
mLLArticulation->jcalc(data);
|
|
mLLArticulation->mJcalcDirty = false;
|
|
|
|
Dy::ArticulationLink* links = data.getLinks();
|
|
Dy::ArticulationJointCoreData* jointData = data.getJointData();
|
|
const PxU32 linkCount = data.getLinkCount();
|
|
|
|
PxReal* jointVelocites = data.getJointVelocities();
|
|
PxReal* jointPositions = data.getJointPositions();
|
|
PxReal* jointTargetPositions = data.getJointTargetPositions();
|
|
PxReal* jointTargetVelocities = data.getJointTargetVelocities();
|
|
|
|
for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
|
|
{
|
|
Dy::ArticulationLink& link = links[linkID];
|
|
|
|
Dy::ArticulationJointCore* joint = link.inboundJoint;
|
|
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);
|
|
|
|
mLLArticulation->raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::Enum(flags));
|
|
|
|
mLLArticulation->initPathToRoot();
|
|
}
|
|
|
|
void Sc::ArticulationSim::updateKinematic(PxArticulationKinematicFlags flags)
|
|
{
|
|
Dy::ArticulationData& data = mLLArticulation->getArticulationData();
|
|
if (mLLArticulation->mJcalcDirty)
|
|
{
|
|
mLLArticulation->jcalc(data);
|
|
mLLArticulation->mJcalcDirty = false;
|
|
}
|
|
|
|
if ((flags & PxArticulationKinematicFlag::ePOSITION))
|
|
{
|
|
mLLArticulation->raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::eDIRTY_POSITIONS);
|
|
mLLArticulation->teleportLinks(data);
|
|
}
|
|
|
|
if ((flags & PxArticulationKinematicFlag::ePOSITION) ||
|
|
(flags & PxArticulationKinematicFlag::eVELOCITY))
|
|
{
|
|
mLLArticulation->raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::eDIRTY_VELOCITIES);
|
|
mLLArticulation->computeLinkVelocities(data);
|
|
}
|
|
}
|
|
|
|
void Sc::ArticulationSim::copyJointStatus(const PxU32 linkID)
|
|
{
|
|
Dy::ArticulationData& data = mLLArticulation->getArticulationData();
|
|
Dy::ArticulationLink* links = data.getLinks();
|
|
Dy::ArticulationJointCoreData* jointData = data.getJointData();
|
|
|
|
Dy::ArticulationLink& link = links[linkID];
|
|
Dy::ArticulationJointCore* joint = link.inboundJoint;
|
|
Dy::ArticulationJointCoreData& jointDatum = jointData[linkID];
|
|
|
|
PxReal* jointVelocites = data.getJointVelocities();
|
|
PxReal* jointPositions = data.getJointPositions();
|
|
|
|
PxReal* jVelocities = &jointVelocites[jointDatum.jointOffset];
|
|
PxReal* 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::updateCCDLinks(PxArray<BodySim*>& sims)
|
|
{
|
|
|
|
for (PxU32 a = 0; a < mBodies.size(); ++a)
|
|
{
|
|
if (mBodies[a]->getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_CCD)
|
|
{
|
|
sims.pushBack(mBodies[a]);
|
|
}
|
|
}
|
|
}
|
|
|
|
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
|
|
}
|
|
mScene.getSimulationController()->updateArticulation(mLLArticulation, mIslandNodeIndex);
|
|
}
|
|
|
|
void Sc::ArticulationSim::sleepCheck(PxReal dt)
|
|
{
|
|
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;
|
|
|
|
for(PxU32 i=0;i<mLinks.size();i++)
|
|
{
|
|
const Cm::SpatialVector& motionVelocity = mLLArticulation->getMotionVelocity(i);
|
|
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<mLinks.size();i++)
|
|
mBodies[i]->getBodyCore().setWakeCounterFromSim(PxMax(1e-6f, mBodies[i]->getBodyCore().getWakeCounter()));
|
|
}
|
|
return;
|
|
}
|
|
|
|
for(PxU32 i=0;i<mLinks.size();i++)
|
|
{
|
|
mBodies[i]->notifyReadyForSleeping();
|
|
mBodies[i]->getLowLevelBody().resetSleepFilter();
|
|
}
|
|
|
|
mScene.getSimpleIslandManager()->deactivateNode(mIslandNodeIndex);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
anyForcesApplied |= mBodies[i]->updateForces(dt, NULL, NULL, count, &mLLArticulation->getSolverDesc().acceleration[i], NULL);
|
|
}
|
|
if(anyForcesApplied)
|
|
mLLArticulation->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.
|
|
|
|
if (!accDirty)
|
|
{
|
|
mLLArticulation->getSolverDesc().acceleration[i].linear = PxVec3(0.f);
|
|
mLLArticulation->getSolverDesc().acceleration[i].angular = PxVec3(0.f);
|
|
}
|
|
else
|
|
{
|
|
mBodies[i]->updateForces(dt, NULL, NULL, count, &mLLArticulation->getSolverDesc().acceleration[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(mLLArticulation, 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);
|
|
}
|
|
|
|
PxU32 Sc::ArticulationSim::getDofs() const
|
|
{
|
|
return mLLArticulation->getDofs();
|
|
}
|
|
|
|
PxU32 Sc::ArticulationSim::getDof(const PxU32 linkID) const
|
|
{
|
|
return mLLArticulation->getDof(linkID);
|
|
}
|
|
|
|
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) const
|
|
{
|
|
//checkResize();
|
|
bool shouldWake = false;
|
|
if (mLLArticulation->applyCache(cache, flag, shouldWake))
|
|
{
|
|
mScene.getSimulationController()->updateArticulation(mLLArticulation, mIslandNodeIndex);
|
|
}
|
|
return shouldWake;
|
|
}
|
|
|
|
//copy internal data to external data
|
|
void Sc::ArticulationSim::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, const bool isGpuSimEnabled) const
|
|
{
|
|
mLLArticulation->copyInternalStateToCache(cache, flag, isGpuSimEnabled);
|
|
}
|
|
|
|
void Sc::ArticulationSim::packJointData(const PxReal* maximum, PxReal* reduced) const
|
|
{
|
|
mLLArticulation->packJointData(maximum, reduced);
|
|
}
|
|
|
|
void Sc::ArticulationSim::unpackJointData(const PxReal* reduced, PxReal* maximum) const
|
|
{
|
|
mLLArticulation->unpackJointData(reduced, maximum);
|
|
}
|
|
|
|
void Sc::ArticulationSim::commonInit()
|
|
{
|
|
mLLArticulation->initializeCommonData();
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeGeneralizedGravityForce(PxArticulationCache& cache, const bool rootMotion)
|
|
{
|
|
mLLArticulation->getGeneralizedGravityForce(mScene.getGravity(), cache, rootMotion);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeCoriolisAndCentrifugalForce(PxArticulationCache& cache, const bool rootMotion)
|
|
{
|
|
mLLArticulation->getCoriolisAndCentrifugalForce(cache, rootMotion);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeGeneralizedExternalForce(PxArticulationCache& cache)
|
|
{
|
|
mLLArticulation->getGeneralizedExternalForce(cache);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeJointAcceleration(PxArticulationCache& cache)
|
|
{
|
|
mLLArticulation->getJointAcceleration(mScene.getGravity(), cache);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeJointForce(PxArticulationCache& cache)
|
|
{
|
|
mLLArticulation->getJointForce(cache);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols)
|
|
{
|
|
mLLArticulation->getDenseJacobian(cache, nRows, nCols);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeCoefficientMatrix(PxArticulationCache& cache)
|
|
{
|
|
mLLArticulation->getCoefficientMatrixWithLoopJoints(mLoopConstraints.begin(), mLoopConstraints.size(), cache);
|
|
}
|
|
|
|
bool Sc::ArticulationSim::computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState,
|
|
const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter)
|
|
{
|
|
const PxReal invLengthScale = 1.f / mScene.getLengthScale();
|
|
return mLLArticulation->getLambda(mLoopConstraints.begin(), mLoopConstraints.size(), cache, initialState, jointTorque, gravity, maxIter, invLengthScale);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeGeneralizedMassMatrix(PxArticulationCache& cache, const bool rootMotion)
|
|
{
|
|
mLLArticulation->getGeneralizedMassMatrixCRB(cache, rootMotion);
|
|
|
|
/*const PxU32 totalDofs = mLLArticulation->getDofs();
|
|
|
|
PxReal* massMatrix = reinterpret_cast<PxReal*>(PX_ALLOC(sizeof(PxReal) * totalDofs * totalDofs, "MassMatrix"));
|
|
PxMemCopy(massMatrix, cache.massMatrix, sizeof(PxReal)*totalDofs * totalDofs);
|
|
|
|
mLLArticulation->getGeneralizedMassMatrix(cache);
|
|
|
|
PxReal* massMatrix1 = cache.massMatrix;
|
|
for (PxU32 i = 0; i < totalDofs; ++i)
|
|
{
|
|
PxReal* row = &massMatrix1[i * totalDofs];
|
|
|
|
for (PxU32 j = 0; j < totalDofs; ++j)
|
|
{
|
|
const PxReal dif = row[j] - massMatrix[j*totalDofs + i];
|
|
PX_ASSERT (PxAbs(dif) < 2e-4f)
|
|
}
|
|
}
|
|
|
|
PX_FREE(massMatrix);*/
|
|
}
|
|
|
|
PxVec3 Sc::ArticulationSim::computeArticulationCOM(const bool rootFrame)
|
|
{
|
|
return mLLArticulation->getArticulationCOM(rootFrame);
|
|
}
|
|
|
|
void Sc::ArticulationSim::computeCentroidalMomentumMatrix(PxArticulationCache& cache)
|
|
{
|
|
mLLArticulation->getCentroidalMomentumMatrix(cache);
|
|
}
|
|
|
|
PxU32 Sc::ArticulationSim::getCoefficientMatrixSize() const
|
|
{
|
|
const PxU32 size = mLoopConstraints.size();
|
|
const PxU32 totalDofs = mLLArticulation->getDofs();
|
|
return size * totalDofs;
|
|
}
|
|
|
|
void Sc::ArticulationSim::setRootLinearVelocity(const PxVec3& velocity)
|
|
{
|
|
mLLArticulation->setRootLinearVelocity(velocity);
|
|
}
|
|
|
|
void Sc::ArticulationSim::setRootAngularVelocity(const PxVec3& velocity)
|
|
{
|
|
mLLArticulation->setRootAngularVelocity(velocity);
|
|
}
|
|
|
|
PxSpatialVelocity Sc::ArticulationSim::getLinkVelocity(const PxU32 linkId) const
|
|
{
|
|
Cm::SpatialVector vel = mLLArticulation->getLinkScalarVelocity(linkId);
|
|
return reinterpret_cast<PxSpatialVelocity&>(vel);
|
|
}
|
|
|
|
PxSpatialVelocity Sc::ArticulationSim::getLinkAcceleration(const PxU32 linkId, const bool isGpuSimEnabled) const
|
|
{
|
|
Cm::SpatialVector accel = mLLArticulation->getMotionAcceleration(linkId, isGpuSimEnabled);
|
|
return reinterpret_cast<PxSpatialVelocity&>(accel);
|
|
}
|
|
|
|
// This method allows user teleport the root links and the articulation
|
|
//system update all other links pose
|
|
void Sc::ArticulationSim::setGlobalPose()
|
|
{
|
|
mLLArticulation->teleportRootLink();
|
|
}
|
|
|
|
void Sc::ArticulationSim::setJointDirty(Dy::ArticulationJointCore& jointCore)
|
|
{
|
|
PX_UNUSED(jointCore);
|
|
mScene.getSimulationController()->updateArticulationJoint(mLLArticulation, mIslandNodeIndex);
|
|
}
|
|
|
|
void Sc::ArticulationSim::setArticulationDirty(PxU32 flag)
|
|
{
|
|
Dy::FeatherstoneArticulation* featherstoneArtic = static_cast<Dy::FeatherstoneArticulation*>(mLLArticulation);
|
|
featherstoneArtic->raiseGPUDirtyFlag(Dy::ArticulationDirtyFlag::Enum(flag));
|
|
mScene.getSimulationController()->updateArticulation(mLLArticulation, 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();
|
|
}
|