Matrix2f K3 = new Matrix2f();
K3.col1.x = body2.getInvI() * r2.y * r2.y; K3.col2.x = -body2.getInvI() * r2.x * r2.y;
K3.col1.y = -body2.getInvI() * r2.x * r2.y; K3.col2.y = body2.getInvI() * r2.x * r2.x;
Matrix2f K = MathUtil.add(MathUtil.add(K1,K2),K3);
M = K.invert();
Vector2f p1 = new Vector2f(body1.getPosition());
p1.add(r1);
Vector2f p2 = new Vector2f(body2.getPosition());
p2.add(r2);