[wpimath] ApplyDeadband: add a scale param (#3865)

Also templates it in C++ so it can work with both doubles and units.

wpimath/src/main/native/include/frc/MathUtil.h

@@ -8,18 +8,79 @@
 #include <wpi/numbers>
 
 #include "units/angle.h"
+#include "units/base.h"
+#include "units/math.h"
 
 namespace frc {
 
 /**
- * Returns 0.0 if the given value is within the specified range around zero.
- * The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ * Returns 0.0 if the given value is within the specified range around zero. The
+ * remaining range between the deadband and the maximum magnitude is scaled from
+ * 0.0 to the maximum magnitude.
  *
- * @param value    Value to clip.
+ * @param value Value to clip.
  * @param deadband Range around zero.
+ * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can
+ * be infinite.
+ * @return The value after the deadband is applied.
  */
-WPILIB_DLLEXPORT
-double ApplyDeadband(double value, double deadband);
+template <typename T,
+          typename = std::enable_if_t<std::disjunction_v<
+              std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
+  T magnitude;
+  if constexpr (std::is_floating_point_v<T>) {
+    magnitude = std::abs(value);
+  } else {
+    magnitude = units::math::abs(value);
+  }
+
+  if (magnitude > deadband) {
+    if (maxMagnitude / deadband > 1.0E12) {
+      // If max magnitude is sufficiently large, the implementation encounters
+      // roundoff error.  Implementing the limiting behavior directly avoids
+      // the problem.
+      return value > T{0.0} ? value - deadband : value + deadband;
+    }
+    if (value > T{0.0}) {
+      // Map deadband to 0 and map max to max.
+      //
+      // y - y₁ = m(x - x₁)
+      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+      // x₁ = deadband
+      // y₁ = 0
+      // x₂ = max
+      // y₂ = max
+      //
+      // y = (max - 0)/(max - deadband) (x - deadband) + 0
+      // y = max/(max - deadband) (x - deadband)
+      // y = max (x - deadband)/(max - deadband)
+      return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
+    } else {
+      // Map -deadband to 0 and map -max to -max.
+      //
+      // y - y₁ = m(x - x₁)
+      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+      // x₁ = -deadband
+      // y₁ = 0
+      // x₂ = -max
+      // y₂ = -max
+      //
+      // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+      // y = max/(max - deadband) (x + deadband)
+      // y = max (x + deadband)/(max - deadband)
+      return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
+    }
+  } else {
+    return T{0.0};
+  }
+}
 
 /**
  * Returns modulus of input.