[wpimath] Add copySignPow to MathUtil for joystick input shaping (#8013)

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

@@ -94,6 +94,42 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   }
 }
 
+/**
+ * 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, raises it to the power of the exponent, then scales the
+ * result back to the original range and copying the sign. This keeps the value
+ * in the original range and gives consistent curve behavior regardless of the
+ * input value's scale.
+ *
+ * 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. Must be
+ * positive.
+ * @return The transformed value with the same sign and scaled to the input
+ * range.
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
+  if constexpr (std::is_arithmetic_v<T>) {
+    return gcem::copysign(
+        gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude,
+        value);
+  } else {
+    return units::math::copysign(
+        gcem::pow((units::math::abs(value) / maxMagnitude).value(), exponent) *
+            maxMagnitude,
+        value);
+  }
+}
+
 /**
  * Returns modulus of input.
  *