Normalizes an angle in radians to the range -pi..pi.
-pi..pi
double normalizeRadians(double angle) { return angle - 2 * math.pi * ((angle + math.pi) / (2 * math.pi)).floor(); }