Vector2f v1 = new Vector2f(ndp);
v1.scale(-body2.getInvMass() - body1.getInvMass());
Vector2f v2 = MathUtil.cross(MathUtil.cross(r2, ndp), r2);
v2.scale(-body2.getInvI());
Vector2f v3 = MathUtil.cross(MathUtil.cross(r1, ndp),r1);
v3.scale(-body1.getInvI());
Vector2f K1 = new Vector2f(v1);