private void applyRotationalSlip(double percentOfDelta)
{
FrameQuaternion identity = new FrameQuaternion(ReferenceFrame.getWorldFrame());
FrameQuaternion desired = slipRotation.getFrameOrientationCopy();
FrameQuaternion delta = new FrameQuaternion();
delta.interpolate(identity, desired, percentOfDelta);
desired.interpolate(identity, desired, 1.0-percentOfDelta);
slipRotation.set(desired);
Point3D touchdownCoM = computeTouchdownCoM();
RotationMatrix deltaRotation = new RotationMatrix(delta);
Point3D touchdownLocation = new Point3D();
for (int i = 0; i < groundContactPointsToSlip.size(); i++)
{
GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i);
boolean touchedDown = (groundContactPointToSlip.isInContact());
if (touchedDown)
{
groundContactPointToSlip.getTouchdownLocation(touchdownLocation);
touchdownLocation.sub(touchdownCoM);
deltaRotation.transform(touchdownLocation);
touchdownLocation.add(touchdownCoM);
groundContactPointToSlip.setTouchdownLocation(touchdownLocation);
}
}
}