Add MathUtil tests

Author: Gold856

wpimath/src/main/native/include/wpi/math/util/MathUtil.hpp

@@ -35,7 +35,7 @@ namespace wpi::math {
  * @return The value after the deadband is applied.
  */
 template <typename T>
-  requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_v<T>
+  requires std::is_arithmetic_v<T> || wpi::units::DimensionedUnitType<T>
 constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   T magnitude;
   if constexpr (std::is_arithmetic_v<T>) {
@@ -95,6 +95,24 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{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 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.
+ */
+template <typename T>
+  requires wpi::units::DimensionlessUnitType<T>
+constexpr T ApplyDeadband(T value, T deadband,
+                          T maxMagnitude = wpi::units::dimensionless<>{1.0}) {
+  return ApplyDeadband(value.raw(), deadband.raw(), maxMagnitude.raw());
+}
+
 /**
  * Returns a zero vector if the given vector is within the specified
  * distance from the origin. The remaining distance between the deadband and the
@@ -107,7 +125,7 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
  * @return The value after the deadband is applied.
  */
 template <typename T, int N>
-  requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_v<T>
+  requires std::is_arithmetic_v<T> || wpi::units::DimensionedUnitType<T>
 Eigen::Vector<T, N> ApplyDeadband(const Eigen::Vector<T, N>& value, T deadband,
                                   T maxMagnitude = T{1.0}) {
   if constexpr (std::is_arithmetic_v<T>) {
@@ -143,7 +161,7 @@ Eigen::Vector<T, N> ApplyDeadband(const Eigen::Vector<T, N>& value, T deadband,
  * range.
  */
 template <typename T>
-  requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_v<T>
+  requires std::is_arithmetic_v<T> || wpi::units::DimensionedUnitType<T>
 constexpr T CopyDirectionPow(T value, double exponent,
                              T maxMagnitude = T{1.0}) {
   if constexpr (std::is_arithmetic_v<T>) {
@@ -152,12 +170,37 @@ constexpr T CopyDirectionPow(T value, double exponent,
         value);
   } else {
     return wpi::units::copysign(
-        gcem::pow((wpi::units::abs(value) / maxMagnitude).value(), exponent) *
+        gcem::pow((wpi::units::abs(value) / maxMagnitude).raw(), exponent) *
             maxMagnitude,
         value);
   }
 }
 
+/**
+ * Raises the input to the power of the given exponent while preserving its
+ * sign.
+ *
+ * The function normalizes the input value to the range [0, 1] based on the
+ * maximum magnitude so that the output stays in the range.
+ *
+ * This is useful for applying smoother or more aggressive control response
+ * curves (e.g. joystick input shaping).
+ *
+ * @param value The input value to transform.
+ * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
+ * curve). Must be positive.
+ * @param maxMagnitude The maximum expected absolute value of input (defaults to
+ * 1). Must be positive.
+ * @return The transformed value with the same sign and scaled to the input
+ * range.
+ */
+template <typename T>
+  requires wpi::units::DimensionlessUnitType<T>
+constexpr T CopyDirectionPow(T value, double exponent,
+                             T maxMagnitude = wpi::units::dimensionless{1.0}) {
+  return CopyDirectionPow(value.raw(), exponent, maxMagnitude.raw());
+}
+
 /**
  * Raises the norm of the input to the power of the given exponent while
  * preserving its direction.
@@ -177,7 +220,7 @@ constexpr T CopyDirectionPow(T value, double exponent,
  * the input range.
  */
 template <typename T, int N>
-  requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_v<T>
+  requires std::is_arithmetic_v<T> || wpi::units::DimensionedUnitType<T>
 Eigen::Vector<T, N> CopyDirectionPow(const Eigen::Vector<T, N>& value,
                                      double exponent, T maxMagnitude = T{1.0}) {
   if constexpr (std::is_arithmetic_v<T>) {

wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -15,6 +15,7 @@
 #include "wpi/math/geometry/Translation2d.hpp"
 #include "wpi/math/geometry/Translation3d.hpp"
 #include "wpi/units/angle.hpp"
+#include "wpi/units/concentration.hpp"
 #include "wpi/units/length.hpp"
 #include "wpi/units/time.hpp"
 #include "wpi/units/velocity.hpp"
@@ -59,6 +60,17 @@ TEST_CASE("MathUtilTest ApplyDeadbandUnits", "[wpimath]") {
                               -20_rad, 1_rad, 20_rad));
 }
 
+TEST_CASE("MathUtilTest ApplyDeadbandDimensionlessUnits", "[wpimath]") {
+  // < 0
+  CHECK_UNITS_EQ(-100_pct, wpi::math::ApplyDeadband<wpi::units::percent<>>(
+                               -100_pct, 1_pct, 100_pct));
+  CHECK_UNITS_EQ(-100_pct, wpi::math::ApplyDeadband<wpi::units::percent<>>(
+                               -100_pct, 1_pct));
+
+  CHECK_UNITS_EQ(25_pct, wpi::math::ApplyDeadband<wpi::units::percent<>>(
+                             25_pct, 2_pct, 25_pct));
+}
+
 TEST_CASE("MathUtilTest ApplyDeadbandLargeMaxMagnitude", "[wpimath]") {
   CHECK_DOUBLE_EQ(
       80.0, wpi::math::ApplyDeadband(100.0, 20.0,
@@ -194,6 +206,22 @@ TEST_CASE("MathUtilTest CopyDirectionPowWithUnits", "[wpimath]") {
                      -5_mps, 2.0, 10_mps));
 }
 
+TEST_CASE("MathUtilTest CopyDirectionPowWithDimensionUnits", "[wpimath]") {
+  CHECK_UNITS_EQ(
+      0_pct, wpi::math::CopyDirectionPow<wpi::units::percent<>>(0_pct, 2.0));
+  CHECK_UNITS_EQ(100_pct, wpi::math::CopyDirectionPow<wpi::units::percent<>>(
+                              100_pct, 2.0));
+  CHECK_UNITS_EQ(-100_pct, wpi::math::CopyDirectionPow<wpi::units::percent<>>(
+                               -100_pct, 2.0));
+
+  CHECK_UNITS_EQ(
+      wpi::units::percent<>{0.5 * 0.5 * 10},
+      wpi::math::CopyDirectionPow<wpi::units::percent<>>(5_pct, 2.0, 10_pct));
+  CHECK_UNITS_EQ(
+      wpi::units::percent<>{-0.5 * 0.5 * 10},
+      wpi::math::CopyDirectionPow<wpi::units::percent<>>(-5_pct, 2.0, 10_pct));
+}
+
 TEST_CASE("MathUtilTest CopyDirectionPow2d", "[wpimath]") {
   CHECK((Eigen::Vector2d{{0.5}, {0.0}}) ==
         wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 1.0));