Vector2f p1 = new Vector2f(body1.getPosition());
p1.add(r1);
Vector2f p2 = new Vector2f(body2.getPosition());
p2.add(r2);
Vector2f dp = new Vector2f(p2);
dp.sub(p1);
Vector2f dv = new Vector2f(body2.getVelocity());
dv.add(MathUtil.cross(body2.getAngularVelocity(),r2));
dv.sub(body1.getVelocity());
dv.sub(MathUtil.cross(body1.getAngularVelocity(),r1));