private static int determineSize(RigidBody[] rigidBodiesInOrder) {
int ret = 0;
for (RigidBody rigidBody : rigidBodiesInOrder) {
ret += rigidBody.getParentJoint().getDegreesOfFreedom();
}
return ret;
}
private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder) {
int[] ret = new int[rigidBodiesInOrder.length];
int currentIndex = 0;
for (int i = 0; i < rigidBodiesInOrder.length; i++) {
RigidBody rigidBody = rigidBodiesInOrder[i];
ret[i] = currentIndex;
currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom();
}
return ret;
}
public void compute() {
MatrixTools.setToZero(massMatrix);
for (int i = 0; i < allRigidBodiesInOrder.length; i++) {
crbInertiasInOrder[i].set(allRigidBodiesInOrder[i].getInertia());
}
for (int i = allRigidBodiesInOrder.length - 1; i >= 0; i--) {
RigidBody currentBody = allRigidBodiesInOrder[i];
InverseDynamicsJoint parentJoint = currentBody.getParentJoint();
CompositeRigidBodyInertia currentBodyInertia = crbInertiasInOrder[i];
GeometricJacobian motionSubspace = parentJoint.getMotionSubspace();
setUnitMomenta(currentBodyInertia, motionSubspace);
setDiagonalTerm(i, motionSubspace);
setOffDiagonalTerms(i);
buildCrbInertia(i);
}
}