final Vector3 nB3 = M2.multiply(nJ3);
final Vector3 nB4 = I2.multiply(nJ4);
// clear out B's if mass is "infinity"
if (b1.isFixed()) { nB1.assignZero(); nB2.assignZero(); }
if (b2.isFixed()) { nB3.assignZero(); nB4.assignZero(); }
//external forces acing at contact (obsolete, external forces are modelled using the delta velocities)
//double Fext = B1.dot(b1.state.force) + B2.dot(b1.state.torque) + B3.dot(b2.state.force) + B4.dot(b2.state.torque);
double correction = depth*(1/dt); //the true correction velocity. This velocity corrects the contact in the next timestep.
final double escape = (cp.envelope-cp.distance)*(1/dt);