RigidBodyNode* MobilizedBody::CustomImpl::createRigidBodyNode(
UIndex& nextUSlot,
USquaredIndex& nextUSqSlot,
QIndex& nextQSlot) const
{
bool noX_MB = (getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1));
bool noR_PF = (getDefaultInboardFrame().R() == Mat33(1));
switch (getImplementation().getImpl().getNU()) {
case 1:
INSTANTIATE_CUSTOM(1, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 2:
INSTANTIATE_CUSTOM(2, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 3:
INSTANTIATE_CUSTOM(3, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 4:
INSTANTIATE_CUSTOM(4, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 5:
INSTANTIATE_CUSTOM(5, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 6:
INSTANTIATE_CUSTOM(6, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
default:
assert(!"Illegal number of degrees of freedom for custom MobilizedBody");
return 0;
}
}
// Ground's "articulated" body inertia is still the infinite mass and
// inertia it started with; no need to look at the children.
void realizeArticulatedBodyInertiasInward(
const SBInstanceCache&,
const SBTreePositionCache&,
SBArticulatedBodyInertiaCache& abc) const
{ ArticulatedInertia& P = updP(abc);
P = ArticulatedInertia(SymMat33(Infinity), Mat33(Infinity),
SymMat33(0));
updPPlus(abc) = P;
}
void realizeInstance(const SBStateDigest& sbs) const {
// Initialize cache entries that will never be changed at later stages.
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
SBDynamicsCache& dc = sbs.updDynamicsCache();
SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
updY(dc) = SpatialMat(Mat33(0));
updA_GB(ac) = 0;
}
