Method addBody

physx/source/simulationcontroller/src/ScArticulationSim.cpp:197–254

}

void Sc::ArticulationSim::addBody(BodySim& body, 
								  BodySim* parent, 
								  ArticulationJointSim* joint)
{
	mBodies.pushBack(&body);
	mJoints.pushBack(joint);
	mLLArticulation->addBody();

	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;
	}
	
	const PxU32 low = PxU32(link.pathToRoot & 0xffffffff);
	const PxU32 high = PxU32(link.pathToRoot >> 32);
	const PxU32 depth = Ps::bitCount(low) + Ps::bitCount(high);
	mMaxDepth = PxMax(depth, mMaxDepth);

	mLLArticulation->setMaxDepth(mMaxDepth);

	if (currentlyAsleep && (!shouldSleep))
	{
		for(PxU32 i=0; i < (mBodies.size() - 1); i++)
			mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
	}

	body.setArticulation(this, wakeCounter, shouldSleep, index);
}