// Vector3 posError = b1.state.rCm.add(b1.state.q.rotate(p1)).minus(b2.state.rCm).minus(b2.state.q.rotate(p2)).multiply(Kcor);
Vector3 posError = b1.state.position.add(ri).sub(b2.state.position).sub(rj).multiply(1.0/dt);
// correction velocity limit
if ( posError.norm() > velocitylimit) {
posError.assign( posError.normalize().multiply(velocitylimit));
}
u.assign( u.add(posError));