[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

wpimath/src/main/native/include/units/math.h

@@ -751,24 +751,4 @@ auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
       "Unit types are not compatible.");
   return resultType(std::fma(x(), y(), resultType(z)()));
 }
-
-/**
- * Constrains theta to within the range (-pi, pi].
- *
- * @param theta Angle to normalize.
- */
-constexpr units::radian_t NormalizeAngle(units::radian_t theta) {
-  units::radian_t pi(wpi::math::pi);
-
-  // Constrain theta to within (-3pi, pi)
-  const int n_pi_pos = (theta + pi) / 2.0 / pi;
-  theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi};
-
-  // Cut off the bottom half of the above range to constrain within
-  // (-pi, pi]
-  const int n_pi_neg = (theta - pi) / 2.0 / pi;
-  theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi};
-
-  return theta;
-}
 }  // namespace units::math