void Sc::ArticulationSim::addBody(BodySim& body,
BodySim* parent,
ArticulationJointSim* joint)
{
mBodies.pushBack(&body);
mJoints.pushBack(joint);
mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));
PxU32 index = mLinks.size();
PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) ||
(((index!=0) && joint) && (parent && (parent->getArticulation() == this))));
ArticulationLink &link = mLinks.insert();
link.body = &body.getLowLevelBody();
link.bodyCore = &body.getBodyCore().getCore();
link.children = 0;
bool shouldSleep;
bool currentlyAsleep;
bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter();
PxReal wakeCounter = getCore().getWakeCounter();
if(parent)
{
currentlyAsleep = !mBodies[0]->isActive();
shouldSleep = currentlyAsleep && bodyReadyForSleep;
PxU32 parentIndex = findBodyIndex(*parent);
link.parent = parentIndex;
link.pathToRoot = mLinks[parentIndex].pathToRoot | ArticulationBitField(1)<<index;
link.inboundJoint = &joint->getCore().getCore();
mLinks[parentIndex].children |= ArticulationBitField(1)<<index;
}
else
{
currentlyAsleep = (wakeCounter == 0.0f);
shouldSleep = currentlyAsleep && bodyReadyForSleep;
link.parent = DY_ARTICULATION_LINK_NONE;
link.pathToRoot = 1;
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);
mUpdateSolverData = true;
}
void Sc::ArticulationSim::removeBody(BodySim &body)
{
PX_ASSERT(body.getArticulation() == this);
PxU32 index = findBodyIndex(body);
body.setArticulation(NULL, 0.0f, true, 0);
ArticulationLink &link0 = mLinks[index];
PX_ASSERT(link0.children == 0);
PX_UNUSED(link0);
// copy all the later links down by one
for(PxU32 i=index+1;i<mLinks.size();i++)
{
mLinks[i-1] = mLinks[i];
mBodies[i-1] = mBodies[i];
mJoints[i-1] = mJoints[i];
//setIslandHandle(*mBodies[i-1], i-1);
}
// adjust parent/child indices
ArticulationBitField fixedIndices = (ArticulationBitField(1)<<index)-1;
ArticulationBitField shiftIndices = ~(fixedIndices|(ArticulationBitField(1)<<index));
for(PxU32 i=0;i<mLinks.size();i++)
{
ArticulationLink &link = mLinks[i];
if(link.parent != DY_ARTICULATION_LINK_NONE && link.parent>index)
link.pathToRoot = (link.pathToRoot&fixedIndices) | (link.pathToRoot&shiftIndices)>>1;
link.children = (link.children&fixedIndices) | (link.children&shiftIndices)>>1;
}
mLinks.popBack();
mUpdateSolverData = true;
}