static double normalizeAngle(double a) { a = (a + pi) % (2 * pi); if (a < 0) a += 2 * pi; return a - pi; }