Fix TwoPointAngle() and readability fixes