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) {