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