[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/native/include/frc/MathUtil.h

@@ -0,0 +1,45 @@
+// 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.
+
+#pragma once
+
+#include <wpi/math>
+
+#include "units/angle.h"
+
+namespace frc {
+
+/**
+ * Returns modulus of input.
+ *
+ * @param input        Input value to wrap.
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+template <typename T>
+constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
+  T modulus = maximumInput - minimumInput;
+
+  // Wrap input if it's above the maximum input
+  int numMax = (input - minimumInput) / modulus;
+  input -= numMax * modulus;
+
+  // Wrap input if it's below the minimum input
+  int numMin = (input - maximumInput) / modulus;
+  input -= numMin * modulus;
+
+  return input;
+}
+
+/**
+ * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
+ *
+ * @param angle Angle to wrap.
+ */
+constexpr units::radian_t AngleModulus(units::radian_t angle) {
+  return InputModulus<units::radian_t>(angle, units::radian_t{-wpi::math::pi},
+                                       units::radian_t{wpi::math::pi});
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/AngleStatistics.h

@@ -7,17 +7,9 @@
 #include <wpi/math>
 
 #include "Eigen/Core"
+#include "frc/MathUtil.h"
 
 namespace frc {
-inline double NormalizeAngle(double angle) {
-  static constexpr double tau = 2 * wpi::math::pi;
-
-  angle -= std::floor(angle / tau) * tau;
-  if (angle > wpi::math::pi) {
-    angle -= tau;
-  }
-  return angle;
-}
 
 /**
  * Subtracts a and b while normalizing the resulting value in the selected row
@@ -32,7 +24,8 @@ Eigen::Matrix<double, States, 1> AngleResidual(
     const Eigen::Matrix<double, States, 1>& a,
     const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
   Eigen::Matrix<double, States, 1> ret = a - b;
-  ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
+  ret[angleStateIdx] =
+      AngleModulus(units::radian_t{ret[angleStateIdx]}).to<double>();
   return ret;
 }
 
@@ -65,7 +58,8 @@ Eigen::Matrix<double, States, 1> AngleAdd(
     const Eigen::Matrix<double, States, 1>& a,
     const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
   Eigen::Matrix<double, States, 1> ret = a + b;
-  ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
+  ret[angleStateIdx] =
+      InputModulus(ret[angleStateIdx], -wpi::math::pi, wpi::math::pi);
   return ret;
 }
 
@@ -122,4 +116,5 @@ AngleMean(int angleStateIdx) {
     return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
   };
 }
+
 }  // namespace frc