private void setUnitMomenta(
CompositeRigidBodyInertia currentBodyInertia, GeometricJacobian motionSubspace) {
nMomentaInUse = motionSubspace.getNumberOfColumns();
for (int i = 0; i < nMomentaInUse; i++) {
tempTwist.set(motionSubspace.getAllUnitTwists().get(i));
tempTwist.changeFrame(currentBodyInertia.getExpressedInFrame());
unitMomenta[i].compute(currentBodyInertia, tempTwist);
}
}
private void setMassMatrixPart(int i, int j, GeometricJacobian motionSubspace) {
int rowStart = massMatrixIndices[i];
int colStart = massMatrixIndices[j];
for (int m = 0; m < nMomentaInUse; m++) {
Momentum unitMomentum = unitMomenta[m];
int massMatrixRow = rowStart + m;
for (int n = 0; n < motionSubspace.getNumberOfColumns(); n++) {
Twist unitRelativeTwist = motionSubspace.getAllUnitTwists().get(n);
unitRelativeTwist.changeFrame(unitMomentum.getExpressedInFrame());
double entry = unitMomentum.computeKineticCoEnergy(unitRelativeTwist);
int massMatrixCol = colStart + n;
setMassMatrixSymmetrically(massMatrixRow, massMatrixCol, entry);
}
}
}