[wpimath] Deduplicate angle modulus functions (#2998)

frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().

They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.controller;
+
+public final class ControllerUtil {
+  /**
+   * Returns modulus of error where error is the difference between the reference and a measurement.
+   *
+   * @param reference Reference input of a controller.
+   * @param measurement The current measurement.
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public static double getModulusError(
+      double reference, double measurement, double minimumInput, double maximumInput) {
+    double error = reference - measurement;
+    double modulus = maximumInput - minimumInput;
+
+    // Wrap error above maximum input
+    int numMax = (int) ((error + maximumInput) / modulus);
+    error -= numMax * modulus;
+
+    // Wrap error below minimum input
+    int numMin = (int) ((error + minimumInput) / modulus);
+    error -= numMin * modulus;
+
+    return error;
+  }
+
+  private ControllerUtil() {}
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.estimator;
 
+import edu.wpi.first.wpiutil.math.MathUtil;
 import edu.wpi.first.wpiutil.math.Matrix;
 import edu.wpi.first.wpiutil.math.Num;
 import edu.wpi.first.wpiutil.math.numbers.N1;
@@ -26,7 +27,7 @@ public final class AngleStatistics {
   public static <S extends Num> Matrix<S, N1> angleResidual(
       Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
     Matrix<S, N1> ret = a.minus(b);
-    ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
 
     return ret;
   }
@@ -52,7 +53,7 @@ public final class AngleStatistics {
   public static <S extends Num> Matrix<S, N1> angleAdd(
       Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
     Matrix<S, N1> ret = a.plus(b);
-    ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
 
     return ret;
   }
@@ -68,16 +69,6 @@ public final class AngleStatistics {
     return (a, b) -> angleAdd(a, b, angleStateIdx);
   }
 
-  static double normalizeAngle(double angle) {
-    final double tau = 2 * Math.PI;
-
-    angle -= Math.floor(angle / tau) * tau;
-    if (angle > Math.PI) {
-      angle -= tau;
-    }
-    return angle;
-  }
-
   /**
    * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a
    * select row.

wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java

@@ -32,22 +32,32 @@ public final class MathUtil {
   }
 
   /**
-   * Constrains theta to within the range (-pi, pi].
+   * Returns modulus of input.
    *
-   * @param theta The angle to normalize.
-   * @return The normalized angle.
+   * @param input Input value to wrap.
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
    */
-  @SuppressWarnings("LocalVariableName")
-  public static double normalizeAngle(double theta) {
-    // Constraint theta to within (-3pi, pi)
-    int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
-    theta -= nPiPos * 2.0 * Math.PI;
+  public static double inputModulus(double input, double minimumInput, double maximumInput) {
+    double modulus = maximumInput - minimumInput;
 
-    // Cut off the bottom half of the above range to constrain within
-    // (-pi, pi]
-    int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
-    theta -= nPiNeg * 2.0 * Math.PI;
+    // Wrap input if it's above the maximum input
+    int numMax = (int) ((input - minimumInput) / modulus);
+    input -= numMax * modulus;
 
-    return theta;
+    // Wrap input if it's below the minimum input
+    int numMin = (int) ((input - maximumInput) / modulus);
+    input -= numMin * modulus;
+
+    return input;
+  }
+
+  /**
+   * Wraps an angle to the range -pi to pi radians.
+   *
+   * @param angleRadians Angle to wrap in radians.
+   */
+  public static double angleModulus(double angleRadians) {
+    return inputModulus(angleRadians, -Math.PI, Math.PI);
   }
 }