code: public static double wrapAngleRad(double angle) {
          angle %= TrigMath.TWO_PI;
          if (angle <= -TrigMath.PI) {
              return angle + TrigMath.TWO_PI;
          }
          if (angle > TrigMath.PI) {
              return angle - TrigMath.TWO_PI;
          }
          return angle;
      }
docs: Wraps the radian between -PI and PI
  
  @param angle to wrap
  @return -PI < radian <= PI