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;
}
bool Sc::ShapeSim::destroyLowLevelVolume()
{
//Need to test that shape has entry in bp. The shape might not have a bp
//entry because it has its simulation flag set to false.
const PxcBpHandle actorHandle = getAABBMgrId().mActorHandle;
if(PX_INVALID_BP_HANDLE!=actorHandle)
{
getInteractionScene().getLowLevelContext()->unMarkShape(actorHandle);
}
bool removingLastShape=Element::destroyLowLevelVolume();
if(removingLastShape)
{
BodySim* b = getBodySim();
if(b)
b->getLowLevelBody().resetAABBMgrId();
}
return removingLastShape;
}