From b56b843c8a9abcddaaea3a57804345ebfdb74a3d Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Tue, 7 Oct 2025 22:00:04 -0700 Subject: [PATCH 1/6] Update frcYear in vendordeps (#8276) --- romiVendordep/RomiVendordep.json | 2 +- wpilibNewCommands/WPILibNewCommands.json | 2 +- xrpVendordep/XRPVendordep.json | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/romiVendordep/RomiVendordep.json b/romiVendordep/RomiVendordep.json index 7f6eda70c8..90cdc120ea 100644 --- a/romiVendordep/RomiVendordep.json +++ b/romiVendordep/RomiVendordep.json @@ -3,7 +3,7 @@ "name": "Romi-Vendordep", "version": "1.0.0", "uuid": "1010372a-b446-46f4-b229-61e53a26a7dc", - "frcYear": "2025", + "frcYear": "2026beta", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/wpilibNewCommands/WPILibNewCommands.json b/wpilibNewCommands/WPILibNewCommands.json index cf4ff02435..7ebc95424b 100644 --- a/wpilibNewCommands/WPILibNewCommands.json +++ b/wpilibNewCommands/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026beta", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/xrpVendordep/XRPVendordep.json b/xrpVendordep/XRPVendordep.json index 28401fb933..1498cac0ec 100644 --- a/xrpVendordep/XRPVendordep.json +++ b/xrpVendordep/XRPVendordep.json @@ -3,7 +3,7 @@ "name": "XRP-Vendordep", "version": "1.0.0", "uuid": "1571a1a5-ed3f-4f07-b7eb-b2beb17394e0", - "frcYear": "2025", + "frcYear": "2026beta", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ From f5990e8b400ad69c7d5797d8d80545d724757f21 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 9 Oct 2025 21:50:55 -0700 Subject: [PATCH 2/6] [upstream_utils] Fix Eigen tag (#8283) Upstream no longer seems to have the commit we were pointing to. We'll just use the tag since that hasn't changed since the official release announcement. --- upstream_utils/eigen.py | 3 +-- .../thirdparty/eigen/include/Eigen/Version | 4 +-- .../include/Eigen/src/Core/CoreEvaluators.h | 27 +++++++++++++------ .../Eigen/src/Core/ProductEvaluators.h | 8 ++++++ .../Eigen/src/Core/functors/BinaryFunctors.h | 25 +++++------------ .../Eigen/src/SparseCore/SparseMatrixBase.h | 3 --- 6 files changed, 37 insertions(+), 33 deletions(-) diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py index 222b45e5f2..e343251160 100755 --- a/upstream_utils/eigen.py +++ b/upstream_utils/eigen.py @@ -145,8 +145,7 @@ def copy_upstream_src(wpilib_root: Path): def main(): name = "eigen" url = "https://gitlab.com/libeigen/eigen.git" - # 5.0.0 release as of 2025-09-23 - tag = "d65cda87c1a673047b59b20a9f9e165a452f91e9" + tag = "5.0.0" eigen = Lib(name, url, tag, copy_upstream_src) eigen.main() diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version index db0f3bf2aa..91936c2e21 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version @@ -7,8 +7,8 @@ #define EIGEN_MAJOR_VERSION 5 #define EIGEN_MINOR_VERSION 0 #define EIGEN_PATCH_VERSION 0 -#define EIGEN_PRERELEASE_VERSION -#define EIGEN_BUILD_VERSION +#define EIGEN_PRERELEASE_VERSION "" +#define EIGEN_BUILD_VERSION "" #define EIGEN_VERSION_STRING "5.0.0" #endif // EIGEN_VERSION_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h index c9b2d2d282..60857e2cc4 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h @@ -1048,22 +1048,33 @@ struct ternary_evaluator, IndexBased Data m_d; }; -// specialization for expressions like (a < b).select(c, d) to enable full vectorization template -struct evaluator, Arg1, Arg2, - CwiseBinaryOp, CmpLhsType, CmpRhsType>>> - : public ternary_evaluator< - CwiseTernaryOp, Arg1, Arg2, - CwiseBinaryOp, CmpLhsType, CmpRhsType>>> { +struct scalar_boolean_select_spec { using DummyTernaryOp = scalar_boolean_select_op; using DummyArg3 = CwiseBinaryOp, CmpLhsType, CmpRhsType>; using DummyXprType = CwiseTernaryOp; - using TernaryOp = scalar_boolean_select_op; - using Arg3 = CwiseBinaryOp, CmpLhsType, CmpRhsType>; + // only use the typed comparison if it is vectorized + static constexpr bool UseTyped = functor_traits>::PacketAccess; + using CondScalar = std::conditional_t; + + using TernaryOp = scalar_boolean_select_op; + using Arg3 = CwiseBinaryOp, CmpLhsType, CmpRhsType>; using XprType = CwiseTernaryOp; using Base = ternary_evaluator; +}; + +// specialization for expressions like (a < b).select(c, d) to enable full vectorization +template +struct evaluator, Arg1, Arg2, + CwiseBinaryOp, CmpLhsType, CmpRhsType>>> + : public scalar_boolean_select_spec::Base { + using Helper = scalar_boolean_select_spec; + using Base = typename Helper::Base; + using DummyXprType = typename Helper::DummyXprType; + using Arg3 = typename Helper::Arg3; + using XprType = typename Helper::XprType; EIGEN_DEVICE_FUNC explicit evaluator(const DummyXprType& xpr) : Base(XprType(xpr.arg1(), xpr.arg2(), Arg3(xpr.arg3().lhs(), xpr.arg3().rhs()))) {} diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h index 5955e496f6..be55be5e89 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h @@ -1272,6 +1272,14 @@ template struct generic_product_impl : generic_product_impl {}; +template +struct generic_product_impl + : generic_product_impl {}; + +template +struct generic_product_impl + : generic_product_impl {}; + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h index b6ecfb5d55..85e1584ea8 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h @@ -207,20 +207,9 @@ struct functor_traits -struct typed_cmp_helper { - static constexpr bool SameType = is_same::value; - static constexpr bool IsNumeric = is_arithmetic::Real>::value; - static constexpr bool UseTyped = UseTypedComparators && SameType && IsNumeric; - using type = typename conditional::type; -}; - -template -using cmp_return_t = typename typed_cmp_helper::type; - template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a == b ? result_type(1) : result_type(0); } @@ -233,7 +222,7 @@ struct scalar_cmp_op : binary template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a < b ? result_type(1) : result_type(0); } @@ -246,7 +235,7 @@ struct scalar_cmp_op : binary template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a <= b ? result_type(1) : result_type(0); } @@ -259,7 +248,7 @@ struct scalar_cmp_op : binary template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a > b ? result_type(1) : result_type(0); } @@ -272,7 +261,7 @@ struct scalar_cmp_op : binary template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a >= b ? result_type(1) : result_type(0); } @@ -285,7 +274,7 @@ struct scalar_cmp_op : binary template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return !(a <= b || b <= a) ? result_type(1) : result_type(0); } @@ -298,7 +287,7 @@ struct scalar_cmp_op : bin template struct scalar_cmp_op : binary_op_base { - using result_type = cmp_return_t; + using result_type = std::conditional_t; EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const { return a != b ? result_type(1) : result_type(0); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h index fbf1313d54..ccbbe98da0 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h @@ -227,9 +227,6 @@ class SparseMatrixBase : public EigenBase { using Nested = typename Derived::Nested; using NestedCleaned = typename internal::remove_all::type; - /// For converting `0's` to the matrices numerical type - using Scalar = typename Derived::Scalar; - if (Flags & RowMajorBit) { Nested nm(m.derived()); internal::evaluator thisEval(nm); From cfbd7a5af29c2533b30f281b36416887c426b8b0 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Fri, 10 Oct 2025 15:42:02 -0400 Subject: [PATCH 3/6] [build] Fix doxygen builds on apple CPUs (#8282) Caused by the doxygen gradle plugin attempting to download 1.10.0 (presumably its default version) from artifactory because the 1.12.0 config is only applied on x86_64 platforms. Just fixing that isn't enough, however; on mac, the plugin would fail to extract the dmg. We need to fall back to a global installation on the PATH for the plugin to find, preferentially using that instead of a failed attempt to download and extract the dmg. --- docs/build.gradle | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/docs/build.gradle b/docs/build.gradle index 423dc96cb2..0e562b4f84 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -52,12 +52,16 @@ doxygen { // See below maven and https://doxygen.nl/download.html for provided binaries // Ensure theme.css (from https://github.com/jothepro/doxygen-awesome-css) is compatible with // doxygen version when updating + executables { + doxygen { + // Note: has no effect if not on an x86_64 platform - you need to have a global install available on your + // PATH for the doxygen plugin to run + executableByVersion('1.12.0') - String arch = System.getProperty("os.arch"); - if (arch.equals("x86_64") || arch.equals("amd64")) { - executables { - doxygen { - executableByVersion('1.12.0') + String arch = System.getProperty("os.arch"); + if (!(arch.equals("x86_64") || arch.equals("amd64"))) { + // Search for a local doxygen install + executableBySearchPath('doxygen') } } } From 4c4996e6387e7dd29bcdc504e4db1f7826b5465f Mon Sep 17 00:00:00 2001 From: arbessette <115647688+arbessette@users.noreply.github.com> Date: Fri, 10 Oct 2025 15:43:02 -0400 Subject: [PATCH 4/6] [docs] Remove Private Message language (#8202) Removing private written message for safety of all users and contributors. --- CODE_OF_CONDUCT.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md index 4e55a063a2..9a72ae63cd 100644 --- a/CODE_OF_CONDUCT.md +++ b/CODE_OF_CONDUCT.md @@ -56,7 +56,7 @@ the consequences for any action they deem in violation of this Code of Conduct: **Community Impact**: Use of inappropriate language or other behavior deemed unprofessional or unwelcome in the community. -**Consequence**: A private, written warning from community leaders, providing +**Consequence**: A warning from community leaders, providing clarity around the nature of the violation and an explanation of why the behavior was inappropriate. A public apology may be requested. From 2b43541b94f9bcba71cda32dbcab47449a8c5586 Mon Sep 17 00:00:00 2001 From: Michael Lesirge <100492377+MichaelLesirge@users.noreply.github.com> Date: Fri, 10 Oct 2025 13:44:12 -0700 Subject: [PATCH 5/6] [wpimath] MathUtil: Add 2D variants of applyDeadband and copySignPow (#8057) --- .../java/edu/wpi/first/math/MathUtil.java | 69 +++++++ .../src/main/native/include/frc/MathUtil.h | 76 +++++++- .../java/edu/wpi/first/math/MathUtilTest.java | 125 ++++++++++++ wpimath/src/test/native/cpp/MathUtilTest.cpp | 183 ++++++++++++++++-- 4 files changed, 432 insertions(+), 21 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 239a6a31fb..01a4f0337c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -107,6 +107,39 @@ public final class MathUtil { return applyDeadband(value, deadband, 1); } + /** + * Returns a zero vector if the given vector is within the specified distance from the origin. The + * remaining distance between the deadband and the maximum distance is scaled from the origin to + * the maximum distance. + * + * @param value Value to clip. + * @param deadband Distance from origin. + * @param maxMagnitude The maximum distance from the origin of the input. Can be infinite. + * @param The number of rows in the vector. + * @return The value after the deadband is applied. + */ + public static Vector applyDeadband( + Vector value, double deadband, double maxMagnitude) { + if (value.norm() < 1e-9) { + return value.times(0); + } + return value.unit().times(applyDeadband(value.norm(), deadband, maxMagnitude)); + } + + /** + * Returns a zero vector if the given vector is within the specified distance from the origin. The + * remaining distance between the deadband and a distance of 1.0 is scaled from the origin to a + * distance of 1.0. + * + * @param value Value to clip. + * @param deadband Distance from origin. + * @param The number of rows in the vector. + * @return The value after the deadband is applied. + */ + public static Vector applyDeadband(Vector value, double deadband) { + return applyDeadband(value, deadband, 1); + } + /** * Raises the input to the power of the given exponent while preserving its sign. * @@ -143,6 +176,42 @@ public final class MathUtil { return copySignPow(value, exponent, 1); } + /** + * Raises the norm of the input to the power of the given exponent while preserving its direction. + * + *

The function normalizes the norm of the input to the range [0, 1] based on the maximum + * distance, raises it to the power of the exponent, then scales the result back to the original + * range. This keeps the value in the original max distance and gives consistent curve behavior + * regardless of the input norm's scale. + * + * @param value The input vector 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 distance from origin of input. Must be positive. + * @param The number of rows in the vector. + * @return The transformed value with the same direction and norm scaled to the input range. + */ + public static Vector copyDirectionPow( + Vector value, double exponent, double maxMagnitude) { + if (value.norm() < 1e-9) { + return value.times(0); + } + return value.unit().times(copySignPow(value.norm(), exponent, maxMagnitude)); + } + + /** + * Raises the norm of the input to the power of the given exponent while preserving its direction. + * + * @param value The input vector to transform. + * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be + * positive. + * @param The number of rows in the vector. + * @return The transformed value with the same direction. + */ + public static Vector copyDirectionPow(Vector value, double exponent) { + return copyDirectionPow(value, exponent, 1); + } + /** * Returns modulus of input. * diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 97705811f9..52fa05df86 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -94,15 +94,41 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { } } +/** + * Returns a zero vector if the given vector is within the specified + * distance from the origin. The remaining distance between the deadband and the + * maximum distance is scaled from the origin to the maximum distance. + * + * @param value Value to clip. + * @param deadband Distance from origin. + * @param maxMagnitude The maximum distance from the origin of the input + * (defaults to 1). Can be infinite. + * @return The value after the deadband is applied. + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +Eigen::Vector ApplyDeadband(const Eigen::Vector& value, T deadband, + T maxMagnitude = T{1.0}) { + if constexpr (std::is_arithmetic_v) { + if (value.norm() < T{1e-9}) { + return Eigen::Vector::Zero(); + } + return value.normalized() * + ApplyDeadband(value.norm(), deadband, maxMagnitude); + } else { + const Eigen::Vector asDouble = value.template cast(); + const Eigen::Vector processed = + ApplyDeadband(asDouble, deadband.value(), maxMagnitude.value()); + return processed.template cast(); + } +} + /** * 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. + * 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). @@ -110,8 +136,8 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { * @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. + * @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. */ @@ -130,6 +156,42 @@ constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) { } } +/** + * Raises the norm of the input to the power of the given exponent while + * preserving its direction. + * + * 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 vector 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 distance from origin of input + * (defaults to 1). Must be positive. + * @return The transformed value with the same direction and norm scaled to + * the input range. + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +Eigen::Vector CopyDirectionPow(const Eigen::Vector& value, + double exponent, T maxMagnitude = T{1.0}) { + if constexpr (std::is_arithmetic_v) { + if (value.norm() < T{1e-9}) { + return Eigen::Vector::Zero(); + } + return value.normalized() * + CopySignPow(value.norm(), exponent, maxMagnitude); + } else { + const Eigen::Vector asDouble = value.template cast(); + const Eigen::Vector processed = + CopyDirectionPow(asDouble, exponent, maxMagnitude.value()); + return processed.template cast(); + } +} + /** * Returns modulus of input. * @@ -279,6 +341,7 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current, } if (dist > maxVelocity * dt) { // Move maximum allowed amount in direction of the difference + // NOLINTNEXTLINE(bugprone-integer-division) return current + diff * (maxVelocity * dt / dist); } return next; @@ -309,6 +372,7 @@ constexpr Translation3d SlewRateLimit(const Translation3d& current, } if (dist > maxVelocity * dt) { // Move maximum allowed amount in direction of the difference + // NOLINTNEXTLINE(bugprone-integer-division) return current + diff * (maxVelocity * dt / dist); } return next; diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index 1fcef74d1f..1c6d725023 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -72,6 +72,60 @@ class MathUtilTest extends UtilityClassTest { assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY)); } + @Test + void testApplyDeadband2dUnityScale() { + assertEquals( + VecBuilder.fill(0.0, 1.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 1.0), 0.02)); + assertEquals( + VecBuilder.fill(0.0, -1.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, -1.0), 0.02)); + assertEquals( + VecBuilder.fill(-1.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(-1.0, 0.0), 0.02)); + + // == 0 + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02)); + + // > 0 + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.01, 0.0), 0.02)); + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.02, 0.0), 0.02)); + assertEquals( + VecBuilder.fill((0.03 - 0.02) / (1.0 - 0.02), 0.0), + MathUtil.applyDeadband(VecBuilder.fill(0.03, 0.0), 0.02)); + assertEquals( + VecBuilder.fill(1.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(1.0, 0.0), 0.02)); + } + + @Test + void testApplyDeadband2dArbitraryScale() { + assertEquals( + VecBuilder.fill(0.0, 2.5), MathUtil.applyDeadband(VecBuilder.fill(0.0, 2.5), 0.02, 2.5)); + assertEquals( + VecBuilder.fill(0.0, -2.5), MathUtil.applyDeadband(VecBuilder.fill(0.0, -2.5), 0.02, 2.5)); + assertEquals( + VecBuilder.fill(-2.5, 0.0), MathUtil.applyDeadband(VecBuilder.fill(-2.5, 0.0), 0.02, 2.5)); + + // == 0 + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02, 2.5)); + + // > 0 + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.01, 0.0), 0.02, 2.5)); + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.02, 0.0), 0.02, 2.5)); + assertEquals( + VecBuilder.fill(2.5, 0.0), MathUtil.applyDeadband(VecBuilder.fill(2.5, 0.0), 0.02, 2.5)); + } + + @Test + void testApplyDeadband2dLargeMaxMagnitude() { + assertEquals( + VecBuilder.fill(80.0, 0.0), + MathUtil.applyDeadband(VecBuilder.fill(100.0, 0.0), 20, Double.POSITIVE_INFINITY)); + } + @Test void testCopySignPow() { assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0)); @@ -110,6 +164,77 @@ class MathUtilTest extends UtilityClassTest { assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0)); } + @Test + void testCopyDirectionPow2d() { + assertEquals( + VecBuilder.fill(0.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 1.0)); + assertEquals( + VecBuilder.fill(-0.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 1.0)); + + assertEquals( + VecBuilder.fill(0.25, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 2.0)); + assertEquals( + VecBuilder.fill(-0.25, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 2.0)); + + assertEquals( + VecBuilder.fill(Math.sqrt(0.5), 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 0.5)); + assertEquals( + VecBuilder.fill(-Math.sqrt(0.5), 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 0.5)); + + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.0), 2.0)); + assertEquals( + VecBuilder.fill(1.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(1.0, 0.0), 2.0)); + assertEquals( + VecBuilder.fill(-1.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-1.0, 0.0), 2.0)); + + assertEquals( + VecBuilder.fill(0.0, Math.pow(0.8, 0.3)), + MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.8), 0.3)); + assertEquals( + VecBuilder.fill(0.0, -Math.pow(0.8, 0.3)), + MathUtil.copyDirectionPow(VecBuilder.fill(0.0, -0.8), 0.3)); + } + + @Test + void testCopyDirectionPow2dMaxDistance() { + assertEquals( + VecBuilder.fill(5.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 1.0, 10.0)); + assertEquals( + VecBuilder.fill(-5.0, 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 1.0, 10.0)); + + assertEquals( + VecBuilder.fill(2.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 2.0, 10.0)); + assertEquals( + VecBuilder.fill(-2.5, 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 2.0, 10.0)); + + assertEquals( + VecBuilder.fill(Math.sqrt(0.5) * 10, 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 0.5, 10.0)); + assertEquals( + VecBuilder.fill(-Math.sqrt(0.5) * 10, 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 0.5, 10.0)); + + assertEquals( + VecBuilder.fill(0.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.0), 2.0, 5.0)); + assertEquals( + VecBuilder.fill(5.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 2.0, 5.0)); + assertEquals( + VecBuilder.fill(-5.0, 0.0), + MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 2.0, 5.0)); + + assertEquals( + VecBuilder.fill(0.0, Math.pow(0.8, 0.3) * 100), + MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 80.0), 0.3, 100.0)); + assertEquals( + VecBuilder.fill(0.0, -Math.pow(0.8, 0.3) * 100), + MathUtil.copyDirectionPow(VecBuilder.fill(0.0, -80.0), 0.3, 100.0)); + } + @Test void testInputModulus() { // These tests check error wrapping. That is, the result of wrapping the diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index 1479c0d18f..6e493dca56 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -55,8 +55,8 @@ TEST(MathUtilTest, ApplyDeadbandArbitraryScale) { TEST(MathUtilTest, ApplyDeadbandUnits) { // < 0 - EXPECT_DOUBLE_EQ( - -20, frc::ApplyDeadband(-20_rad, 1_rad, 20_rad).value()); + EXPECT_UNITS_EQ(-20_rad, + frc::ApplyDeadband(-20_rad, 1_rad, 20_rad)); } TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) { @@ -65,6 +65,72 @@ TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) { frc::ApplyDeadband(100.0, 20.0, std::numeric_limits::infinity())); } +TEST(MathUtilTest, ApplyDeadband2dUnityScale) { + EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {1.0}}, 0.02)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-1.0}}, 0.02)); + EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{-1.0}, {0.0}}, 0.02)); + + // == 0 + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02)); + + // > 0 + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02)); + EXPECT_EQ((Eigen::Vector2d{{(0.03 - 0.02) / (1.0 - 0.02)}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.03}, {0.0}}, 0.02)); + EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{1.0}, {0.0}}, 0.02)); +} + +TEST(MathUtilTest, ApplyDeadband2dArbitraryScale) { + EXPECT_EQ((Eigen::Vector2d{{0.0}, {2.5}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {2.5}}, 0.02, 2.5)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {-2.5}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5)); + EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5)); + + // == 0 + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5)); + + // > 0 + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5)); + EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}), + frc::ApplyDeadband(Eigen::Vector2d{{2.5}, {0.0}}, 0.02, 2.5)); +} + +TEST(MathUtilTest, ApplyDeadband2dLargeMaxMagnitude) { + EXPECT_EQ((Eigen::Vector2d{{80.0}, {0.0}}), + (frc::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0, + std::numeric_limits::infinity()))); +} + +TEST(MathUtilTest, ApplyDeadband2dUnits) { + EXPECT_EQ((Eigen::Vector{0_mps, 2.5_mps}), + frc::ApplyDeadband( + Eigen::Vector{0_mps, 2.5_mps}, + 0.02_mps, 2.5_mps)); + EXPECT_EQ((Eigen::Vector{1_mps, 0_mps}), + frc::ApplyDeadband( + Eigen::Vector{1_mps, 0_mps}, + 0.02_mps)); + + EXPECT_EQ((Eigen::Vector{0_mps, 0_mps}), + frc::ApplyDeadband( + Eigen::Vector{0_mps, 0_mps}, + 0.02_mps, 2.5_mps)); +} + TEST(MathUtilTest, CopySignPow) { EXPECT_DOUBLE_EQ(0.5, frc::CopySignPow(0.5, 1.0)); EXPECT_DOUBLE_EQ(-0.5, frc::CopySignPow(-0.5, 1.0)); @@ -104,20 +170,107 @@ TEST(MathUtilTest, CopySignPowWithMaxMagnitude) { } TEST(MathUtilTest, CopySignPowWithUnits) { - EXPECT_DOUBLE_EQ( - 0, frc::CopySignPow(0_mps, 2.0).value()); - EXPECT_DOUBLE_EQ( - 1, frc::CopySignPow(1_mps, 2.0).value()); - EXPECT_DOUBLE_EQ( - -1, frc::CopySignPow(-1_mps, 2.0).value()); + EXPECT_UNITS_EQ(0_mps, + frc::CopySignPow(0_mps, 2.0)); + EXPECT_UNITS_EQ(1_mps, + frc::CopySignPow(1_mps, 2.0)); + EXPECT_UNITS_EQ(-1_mps, + frc::CopySignPow(-1_mps, 2.0)); - EXPECT_DOUBLE_EQ( - 0.5 * 0.5 * 10, - frc::CopySignPow(5_mps, 2.0, 10_mps).value()); - EXPECT_DOUBLE_EQ( - -0.5 * 0.5 * 10, - frc::CopySignPow(-5_mps, 2.0, 10_mps) - .value()); + EXPECT_UNITS_EQ( + units::meters_per_second_t{0.5 * 0.5 * 10}, + frc::CopySignPow(5_mps, 2.0, 10_mps)); + EXPECT_UNITS_EQ( + units::meters_per_second_t{-0.5 * 0.5 * 10}, + frc::CopySignPow(-5_mps, 2.0, 10_mps)); +} + +TEST(MathUtilTest, CopyDirectionPow2d) { + EXPECT_EQ((Eigen::Vector2d{{0.5}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 1.0)); + EXPECT_EQ((Eigen::Vector2d{{-0.5}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 1.0)); + + EXPECT_EQ((Eigen::Vector2d{{0.25}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 2.0)); + EXPECT_EQ((Eigen::Vector2d{{-0.25}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 2.0)); + + EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5)}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 0.5)); + EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5)}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 0.5)); + + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0)); + + EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{1.0}, {0.0}}, 2.0)); + EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-1.0}, {0.0}}, 2.0)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {1.0}}, 2.0)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-1.0}}, 2.0)); + + EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3)}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.8}}, 0.3)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3)}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-0.8}}, 0.3)); +} + +TEST(MathUtilTest, CopyDirectionPow2dMaxDistance) { + EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0)); + EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0)); + + EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0)); + EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0)); + + EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5) * 10.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0)); + EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5) * 10.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0)); + + EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0)); + EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0)); + EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0)); + + EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3) * 100.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0)); + EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3) * 100.0}}), + frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0)); +} + +TEST(MathUtilTest, CopyDirectionPow2dUnits) { + EXPECT_EQ( + (Eigen::Vector{1_mps, 0_mps}), + frc::CopyDirectionPow( + Eigen::Vector{1_mps, 0_mps}, 2.0)); + EXPECT_EQ( + (Eigen::Vector{-1_mps, 0_mps}), + frc::CopyDirectionPow( + Eigen::Vector{-1_mps, 0_mps}, 2.0)); + + EXPECT_EQ((Eigen::Vector{0_mps, 0_mps}), + frc::CopyDirectionPow( + Eigen::Vector{0_mps, 0_mps}, 2.0, + 5_mps)); + + EXPECT_EQ((Eigen::Vector{5_mps, 0_mps}), + frc::CopyDirectionPow( + Eigen::Vector{5_mps, 0_mps}, 2.0, + 5_mps)); + EXPECT_EQ((Eigen::Vector{-5_mps, 0_mps}), + frc::CopyDirectionPow( + Eigen::Vector{-5_mps, 0_mps}, + 2.0, 5_mps)); } TEST(MathUtilTest, InputModulus) { From 9e85f3cf5505b1b8b3dcfd8fa0c7a9e3408b2d4d Mon Sep 17 00:00:00 2001 From: Michael Lesirge <100492377+MichaelLesirge@users.noreply.github.com> Date: Sat, 11 Oct 2025 09:24:10 -0700 Subject: [PATCH 6/6] [wpimath] Rename 1D copySignPow to match 2D copyDirectionPow (#8286) --- .../native/cpp/drive/DifferentialDrive.cpp | 8 +-- .../wpilibj/drive/DifferentialDrive.java | 8 +-- .../java/edu/wpi/first/math/MathUtil.java | 8 +-- .../src/main/native/include/frc/MathUtil.h | 5 +- .../java/edu/wpi/first/math/MathUtilTest.java | 48 ++++++------- wpimath/src/test/native/cpp/MathUtilTest.cpp | 67 ++++++++++--------- 6 files changed, 73 insertions(+), 71 deletions(-) diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index 4a65d11c8e..a6f1a3ca5f 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -115,8 +115,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK( // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - xSpeed = CopySignPow(xSpeed, 2); - zRotation = CopySignPow(zRotation, 2); + xSpeed = CopyDirectionPow(xSpeed, 2); + zRotation = CopyDirectionPow(zRotation, 2); } double leftSpeed = xSpeed - zRotation; @@ -170,8 +170,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK( // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - leftSpeed = CopySignPow(leftSpeed, 2); - rightSpeed = CopySignPow(rightSpeed, 2); + leftSpeed = CopyDirectionPow(leftSpeed, 2); + rightSpeed = CopyDirectionPow(rightSpeed, 2); } return {leftSpeed, rightSpeed}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 676a8ea470..10a3b15443 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -265,8 +265,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - xSpeed = MathUtil.copySignPow(xSpeed, 2); - zRotation = MathUtil.copySignPow(zRotation, 2); + xSpeed = MathUtil.copyDirectionPow(xSpeed, 2); + zRotation = MathUtil.copyDirectionPow(zRotation, 2); } double leftSpeed = xSpeed - zRotation; @@ -340,8 +340,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - leftSpeed = MathUtil.copySignPow(leftSpeed, 2); - rightSpeed = MathUtil.copySignPow(rightSpeed, 2); + leftSpeed = MathUtil.copyDirectionPow(leftSpeed, 2); + rightSpeed = MathUtil.copyDirectionPow(rightSpeed, 2); } return new WheelSpeeds(leftSpeed, rightSpeed); diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 01a4f0337c..bab30c4b2c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -157,7 +157,7 @@ public final class MathUtil { * @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. */ - public static double copySignPow(double value, double exponent, double maxMagnitude) { + public static double copyDirectionPow(double value, double exponent, double maxMagnitude) { return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude; } @@ -172,8 +172,8 @@ public final class MathUtil { * positive. * @return The transformed value with the same sign. */ - public static double copySignPow(double value, double exponent) { - return copySignPow(value, exponent, 1); + public static double copyDirectionPow(double value, double exponent) { + return copyDirectionPow(value, exponent, 1); } /** @@ -196,7 +196,7 @@ public final class MathUtil { if (value.norm() < 1e-9) { return value.times(0); } - return value.unit().times(copySignPow(value.norm(), exponent, maxMagnitude)); + return value.unit().times(copyDirectionPow(value.norm(), exponent, maxMagnitude)); } /** diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 52fa05df86..66f4008781 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -143,7 +143,8 @@ Eigen::Vector ApplyDeadband(const Eigen::Vector& value, T deadband, */ template requires std::is_arithmetic_v || units::traits::is_unit_t_v -constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) { +constexpr T CopyDirectionPow(T value, double exponent, + T maxMagnitude = T{1.0}) { if constexpr (std::is_arithmetic_v) { return gcem::copysign( gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude, @@ -183,7 +184,7 @@ Eigen::Vector CopyDirectionPow(const Eigen::Vector& value, return Eigen::Vector::Zero(); } return value.normalized() * - CopySignPow(value.norm(), exponent, maxMagnitude); + CopyDirectionPow(value.norm(), exponent, maxMagnitude); } else { const Eigen::Vector asDouble = value.template cast(); const Eigen::Vector processed = diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index 1c6d725023..7a9e856c11 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -127,41 +127,41 @@ class MathUtilTest extends UtilityClassTest { } @Test - void testCopySignPow() { - assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0)); - assertEquals(-0.5, MathUtil.copySignPow(-0.5, 1.0)); + void testCopyDirectionPow() { + assertEquals(0.5, MathUtil.copyDirectionPow(0.5, 1.0)); + assertEquals(-0.5, MathUtil.copyDirectionPow(-0.5, 1.0)); - assertEquals(0.5 * 0.5, MathUtil.copySignPow(0.5, 2.0)); - assertEquals(-(0.5 * 0.5), MathUtil.copySignPow(-0.5, 2.0)); + assertEquals(0.5 * 0.5, MathUtil.copyDirectionPow(0.5, 2.0)); + assertEquals(-(0.5 * 0.5), MathUtil.copyDirectionPow(-0.5, 2.0)); - assertEquals(Math.sqrt(0.5), MathUtil.copySignPow(0.5, 0.5)); - assertEquals(-Math.sqrt(0.5), MathUtil.copySignPow(-0.5, 0.5)); + assertEquals(Math.sqrt(0.5), MathUtil.copyDirectionPow(0.5, 0.5)); + assertEquals(-Math.sqrt(0.5), MathUtil.copyDirectionPow(-0.5, 0.5)); - assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0)); - assertEquals(1.0, MathUtil.copySignPow(1.0, 2.0)); - assertEquals(-1.0, MathUtil.copySignPow(-1.0, 2.0)); + assertEquals(0.0, MathUtil.copyDirectionPow(0.0, 2.0)); + assertEquals(1.0, MathUtil.copyDirectionPow(1.0, 2.0)); + assertEquals(-1.0, MathUtil.copyDirectionPow(-1.0, 2.0)); - assertEquals(Math.pow(0.8, 0.3), MathUtil.copySignPow(0.8, 0.3)); - assertEquals(-Math.pow(0.8, 0.3), MathUtil.copySignPow(-0.8, 0.3)); + assertEquals(Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(0.8, 0.3)); + assertEquals(-Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(-0.8, 0.3)); } @Test - void testCopySignPowMaxMagnitude() { - assertEquals(5, MathUtil.copySignPow(5.0, 1.0, 10.0)); - assertEquals(-5, MathUtil.copySignPow(-5.0, 1.0, 10.0)); + void testCopyDirectionPowMaxMagnitude() { + assertEquals(5, MathUtil.copyDirectionPow(5.0, 1.0, 10.0)); + assertEquals(-5, MathUtil.copyDirectionPow(-5.0, 1.0, 10.0)); - assertEquals(0.5 * 0.5 * 10, MathUtil.copySignPow(5.0, 2.0, 10.0)); - assertEquals(-0.5 * 0.5 * 10, MathUtil.copySignPow(-5.0, 2.0, 10.0)); + assertEquals(0.5 * 0.5 * 10, MathUtil.copyDirectionPow(5.0, 2.0, 10.0)); + assertEquals(-0.5 * 0.5 * 10, MathUtil.copyDirectionPow(-5.0, 2.0, 10.0)); - assertEquals(Math.sqrt(0.5) * 10, MathUtil.copySignPow(5.0, 0.5, 10.0)); - assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copySignPow(-5.0, 0.5, 10.0)); + assertEquals(Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(5.0, 0.5, 10.0)); + assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(-5.0, 0.5, 10.0)); - assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0, 5.0)); - assertEquals(5.0, MathUtil.copySignPow(5.0, 2.0, 5.0)); - assertEquals(-5.0, MathUtil.copySignPow(-5.0, 2.0, 5.0)); + assertEquals(0.0, MathUtil.copyDirectionPow(0.0, 2.0, 5.0)); + assertEquals(5.0, MathUtil.copyDirectionPow(5.0, 2.0, 5.0)); + assertEquals(-5.0, MathUtil.copyDirectionPow(-5.0, 2.0, 5.0)); - assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(80, 0.3, 100.0)); - assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0)); + assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(80, 0.3, 100.0)); + assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(-80, 0.3, 100.0)); } @Test diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index 6e493dca56..1222bc4073 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -131,58 +131,59 @@ TEST(MathUtilTest, ApplyDeadband2dUnits) { 0.02_mps, 2.5_mps)); } -TEST(MathUtilTest, CopySignPow) { - EXPECT_DOUBLE_EQ(0.5, frc::CopySignPow(0.5, 1.0)); - EXPECT_DOUBLE_EQ(-0.5, frc::CopySignPow(-0.5, 1.0)); +TEST(MathUtilTest, CopyDirectionPow) { + EXPECT_DOUBLE_EQ(0.5, frc::CopyDirectionPow(0.5, 1.0)); + EXPECT_DOUBLE_EQ(-0.5, frc::CopyDirectionPow(-0.5, 1.0)); - EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopySignPow(0.5, 2.0)); - EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopySignPow(-0.5, 2.0)); + EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopyDirectionPow(0.5, 2.0)); + EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopyDirectionPow(-0.5, 2.0)); - EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopySignPow(0.5, 0.5)); - EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopySignPow(-0.5, 0.5)); + EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopyDirectionPow(0.5, 0.5)); + EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopyDirectionPow(-0.5, 0.5)); - EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0)); - EXPECT_DOUBLE_EQ(1.0, frc::CopySignPow(1.0, 2.0)); - EXPECT_DOUBLE_EQ(-1.0, frc::CopySignPow(-1.0, 2.0)); + EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0)); + EXPECT_DOUBLE_EQ(1.0, frc::CopyDirectionPow(1.0, 2.0)); + EXPECT_DOUBLE_EQ(-1.0, frc::CopyDirectionPow(-1.0, 2.0)); - EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopySignPow(0.8, 0.3)); - EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopySignPow(-0.8, 0.3)); + EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopyDirectionPow(0.8, 0.3)); + EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopyDirectionPow(-0.8, 0.3)); } -TEST(MathUtilTest, CopySignPowWithMaxMagnitude) { - EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 1.0, 10.0)); - EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 1.0, 10.0)); +TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) { + EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 1.0, 10.0)); + EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 1.0, 10.0)); - EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopySignPow(5.0, 2.0, 10.0)); - EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopySignPow(-5.0, 2.0, 10.0)); + EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopyDirectionPow(5.0, 2.0, 10.0)); + EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopyDirectionPow(-5.0, 2.0, 10.0)); - EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopySignPow(5.0, 0.5, 10.0)); - EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10, frc::CopySignPow(-5.0, 0.5, 10.0)); + EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopyDirectionPow(5.0, 0.5, 10.0)); + EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10, + frc::CopyDirectionPow(-5.0, 0.5, 10.0)); - EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0, 5.0)); - EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 2.0, 5.0)); - EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 2.0, 5.0)); + EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0, 5.0)); + EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 2.0, 5.0)); + EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 2.0, 5.0)); EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100, - frc::CopySignPow(80.0, 0.3, 100.0)); + frc::CopyDirectionPow(80.0, 0.3, 100.0)); EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100, - frc::CopySignPow(-80.0, 0.3, 100.0)); + frc::CopyDirectionPow(-80.0, 0.3, 100.0)); } -TEST(MathUtilTest, CopySignPowWithUnits) { - EXPECT_UNITS_EQ(0_mps, - frc::CopySignPow(0_mps, 2.0)); - EXPECT_UNITS_EQ(1_mps, - frc::CopySignPow(1_mps, 2.0)); - EXPECT_UNITS_EQ(-1_mps, - frc::CopySignPow(-1_mps, 2.0)); +TEST(MathUtilTest, CopyDirectionPowWithUnits) { + EXPECT_UNITS_EQ( + 0_mps, frc::CopyDirectionPow(0_mps, 2.0)); + EXPECT_UNITS_EQ( + 1_mps, frc::CopyDirectionPow(1_mps, 2.0)); + EXPECT_UNITS_EQ( + -1_mps, frc::CopyDirectionPow(-1_mps, 2.0)); EXPECT_UNITS_EQ( units::meters_per_second_t{0.5 * 0.5 * 10}, - frc::CopySignPow(5_mps, 2.0, 10_mps)); + frc::CopyDirectionPow(5_mps, 2.0, 10_mps)); EXPECT_UNITS_EQ( units::meters_per_second_t{-0.5 * 0.5 * 10}, - frc::CopySignPow(-5_mps, 2.0, 10_mps)); + frc::CopyDirectionPow(-5_mps, 2.0, 10_mps)); } TEST(MathUtilTest, CopyDirectionPow2d) {