diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index b57370cfc5..e0e061a707 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -137,8 +137,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable { * @param max maximum voltage percentage (0-1 range) */ public void setVoltagePercentageRange(double min, double max) { - m_sensorMin = MathUtil.clamp(min, 0.0, 1.0); - m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); + m_sensorMin = Math.clamp(min, 0.0, 1.0); + m_sensorMax = Math.clamp(max, 0.0, 1.0); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index be8e5ca1fd..f18ab988c6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -156,8 +156,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { * @param max maximum duty cycle (0-1 range) */ public void setDutyCycleRange(double min, double max) { - m_sensorMin = MathUtil.clamp(min, 0.0, 1.0); - m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); + m_sensorMin = Math.clamp(min, 0.0, 1.0); + m_sensorMax = Math.clamp(max, 0.0, 1.0); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java index 67ac2db26d..78397f2862 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java @@ -5,7 +5,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.DriverStationJNI; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.DriverStation.POVDirection; import edu.wpi.first.wpilibj.event.BooleanEvent; @@ -467,7 +466,7 @@ public class GenericHID { * @param value The normalized value (0 to 1) to set the rumble to */ public void setRumble(RumbleType type, double value) { - value = MathUtil.clamp(value, 0, 1); + value = Math.clamp(value, 0, 1); int rumbleValue = (int) (value * 65535); switch (type) { case kLeftRumble -> this.m_leftRumble = rumbleValue; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java index 902a54c031..0d5ece345e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java @@ -9,7 +9,6 @@ import static edu.wpi.first.units.Units.Microsecond; import static edu.wpi.first.units.Units.Microseconds; import static edu.wpi.first.units.Units.Value; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.collections.LongToObjectHashMap; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.units.measure.Distance; @@ -479,9 +478,9 @@ public interface LEDPattern { writer.setRGB( i, - (int) MathUtil.clamp(r * multiplier, 0, 255), - (int) MathUtil.clamp(g * multiplier, 0, 255), - (int) MathUtil.clamp(b * multiplier, 0, 255)); + (int) Math.clamp(r * multiplier, 0, 255), + (int) Math.clamp(g * multiplier, 0, 255), + (int) Math.clamp(b * multiplier, 0, 255)); }); }; } @@ -526,7 +525,7 @@ public interface LEDPattern { */ static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) { return (reader, writer) -> { - double progress = MathUtil.clamp(progressSupplier.getAsDouble(), 0, 1); + double progress = Math.clamp(progressSupplier.getAsDouble(), 0, 1); int bufLen = reader.getLength(); int max = (int) (bufLen * progress); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java index 4ad3d42f0a..f271ecc369 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java @@ -8,7 +8,6 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -81,7 +80,7 @@ public class Servo implements Sendable, AutoCloseable { * @param value Position from 0.0 to 1.0. */ public void set(double value) { - value = MathUtil.clamp(value, 0.0, 1.0); + value = Math.clamp(value, 0.0, 1.0); if (m_simPosition != null) { m_simPosition.set(value); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java index 9f5464257b..0b86fe3dab 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java @@ -8,7 +8,6 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -130,12 +129,12 @@ public class SharpIR implements Sendable, AutoCloseable { */ public double getRange() { if (m_simRange != null) { - return MathUtil.clamp(m_simRange.get(), m_min, m_max); + return Math.clamp(m_simRange.get(), m_min, m_max); } else { // Don't allow zero/negative values var v = Math.max(m_sensor.getVoltage(), 0.00001); - return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max); + return Math.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max); } } 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 b617a005d8..99fe0f9939 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 @@ -254,8 +254,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * @return Wheel speeds [-1.0..1.0]. */ public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) { - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); - zRotation = MathUtil.clamp(zRotation, -1.0, 1.0); + xSpeed = Math.clamp(xSpeed, -1.0, 1.0); + zRotation = Math.clamp(zRotation, -1.0, 1.0); // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. @@ -295,8 +295,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC */ public static WheelSpeeds curvatureDriveIK( double xSpeed, double zRotation, boolean allowTurnInPlace) { - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); - zRotation = MathUtil.clamp(zRotation, -1.0, 1.0); + xSpeed = Math.clamp(xSpeed, -1.0, 1.0); + zRotation = Math.clamp(zRotation, -1.0, 1.0); double leftSpeed; double rightSpeed; @@ -329,8 +329,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * @return Wheel speeds [-1.0..1.0]. */ public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) { - leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0); - rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0); + leftSpeed = Math.clamp(leftSpeed, -1.0, 1.0); + rightSpeed = Math.clamp(rightSpeed, -1.0, 1.0); // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index e0c19e56f2..0e0b3134f1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -269,8 +269,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea */ public static WheelSpeeds driveCartesianIK( double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) { - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); - ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); + xSpeed = Math.clamp(xSpeed, -1.0, 1.0); + ySpeed = Math.clamp(ySpeed, -1.0, 1.0); // Compensate for gyro angle. var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus()); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java index 02e610cb9f..511921c855 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.motorcontrol; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -99,7 +98,7 @@ public abstract class PWMMotorController extends MotorSafety */ protected final void setSpeed(double speed) { if (Double.isFinite(speed)) { - speed = MathUtil.clamp(speed, -1.0, 1.0); + speed = Math.clamp(speed, -1.0, 1.0); } else { speed = 0.0; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index f18ad7a49d..20f1b67643 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.simulation; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -135,7 +134,7 @@ public class ElevatorSim extends LinearSystemSim { * @param velocity New velocity in meters per second. */ public final void setState(double position, double velocity) { - setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity)); + setState(VecBuilder.fill(Math.clamp(position, m_minHeight, m_maxHeight), velocity)); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index af8a68cc49..18a8d0fce8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.simulation; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -117,8 +116,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param velocityRadPerSec The new angular velocity in radians per second. */ public final void setState(double angleRadians, double velocityRadPerSec) { - setState( - VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec)); + setState(VecBuilder.fill(Math.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec)); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java index 8ab4c3498a..356961aff9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java @@ -152,7 +152,7 @@ public class Color { } private static double roundAndClamp(double value) { - return MathUtil.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0); + return Math.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0); } // Helper methods diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java index f9983a5ae7..7f70def3e0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.util; -import edu.wpi.first.math.MathUtil; import java.util.Objects; /** Represents colors with 8 bits of precision. */ @@ -34,9 +33,9 @@ public class Color8Bit { * @param blue Blue value (0-255) */ public Color8Bit(int red, int green, int blue) { - this.red = MathUtil.clamp(red, 0, 255); - this.green = MathUtil.clamp(green, 0, 255); - this.blue = MathUtil.clamp(blue, 0, 255); + this.red = Math.clamp(red, 0, 255); + this.green = Math.clamp(green, 0, 255); + this.blue = Math.clamp(blue, 0, 255); } /** 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..43b4408fc5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -13,30 +13,6 @@ public final class MathUtil { throw new AssertionError("utility class"); } - /** - * Returns value clamped between low and high boundaries. - * - * @param value Value to clamp. - * @param low The lower boundary to which to clamp value. - * @param high The higher boundary to which to clamp value. - * @return The clamped value. - */ - public static int clamp(int value, int low, int high) { - return Math.max(low, Math.min(value, high)); - } - - /** - * Returns value clamped between low and high boundaries. - * - * @param value Value to clamp. - * @param low The lower boundary to which to clamp value. - * @param high The higher boundary to which to clamp value. - * @return The clamped value. - */ - public static double clamp(double value, double low, double high) { - return Math.max(low, Math.min(value, high)); - } - /** * 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. @@ -184,7 +160,7 @@ public final class MathUtil { * @return The interpolated value. */ public static double interpolate(double startValue, double endValue, double t) { - return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1); + return startValue + (endValue - startValue) * Math.clamp(t, 0, 1); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index eb49430892..0a9f9c5333 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -152,7 +152,7 @@ public final class StateSpaceUtil { Matrix u, Matrix umin, Matrix umax) { var result = new Matrix(new SimpleMatrix(u.getNumRows(), 1)); for (int i = 0; i < u.getNumRows(); i++) { - result.set(i, 0, MathUtil.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0))); + result.set(i, 0, Math.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0))); } return result; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java index f2c4e803bb..d1cb1c7be8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java @@ -455,7 +455,7 @@ public class PIDController implements Sendable, AutoCloseable { m_totalError = 0; } else if (m_ki != 0) { m_totalError = - MathUtil.clamp( + Math.clamp( m_totalError + m_error * m_period, m_minimumIntegral / m_ki, m_maximumIntegral / m_ki); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 2cfe982ce3..c68b8f1d9f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -5,7 +5,6 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -187,7 +186,7 @@ public class PoseEstimator { // the buffer will always use a timestamp between the first and last timestamps) double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); - timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); + timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); // Step 2: If there are no applicable vision updates, use the odometry-only information. if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 31c97549ff..b3844a2bde 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -5,7 +5,6 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -199,7 +198,7 @@ public class PoseEstimator3d { // the buffer will always use a timestamp between the first and last timestamps) double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); - timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); + timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); // Step 2: If there are no applicable vision updates, use the odometry-only information. if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java index d9eca58419..f963b43f85 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java @@ -5,7 +5,6 @@ package edu.wpi.first.math.filter; import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; /** * A class that limits the rate of change of an input value. Useful for implementing voltage, @@ -56,7 +55,7 @@ public class SlewRateLimiter { double currentTime = MathSharedStore.getTimestamp(); double elapsedTime = currentTime - m_prevTime; m_prevVal += - MathUtil.clamp( + Math.clamp( input - m_prevVal, m_negativeRateLimit * elapsedTime, m_positiveRateLimit * elapsedTime); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 4e252b7f57..14183681c6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -6,7 +6,6 @@ package edu.wpi.first.math.geometry; import static edu.wpi.first.units.Units.Meters; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.proto.Rectangle2dProto; import edu.wpi.first.math.geometry.struct.Rectangle2dStruct; import edu.wpi.first.units.measure.Distance; @@ -217,9 +216,9 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { // Find nearest point point = new Translation2d( - MathUtil.clamp( + Math.clamp( point.getX(), m_center.getX() - m_xWidth / 2.0, m_center.getX() + m_xWidth / 2.0), - MathUtil.clamp( + Math.clamp( point.getY(), m_center.getY() - m_yWidth / 2.0, m_center.getY() + m_yWidth / 2.0)); // Undo rotation diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index f8c2b965a8..fce505581f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -12,7 +12,6 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Rotation2dProto; @@ -364,7 +363,7 @@ public class Rotation2d @Override public Rotation2d interpolate(Rotation2d endValue, double t) { - return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); + return plus(endValue.minus(this).times(Math.clamp(t, 0, 1))); } /** Rotation2d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 5d3f479a97..b0a32e2465 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -12,7 +12,6 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -562,7 +561,7 @@ public class Rotation3d @Override public Rotation3d interpolate(Rotation3d endValue, double t) { - return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); + return plus(endValue.minus(this).times(Math.clamp(t, 0, 1))); } /** Rotation3d protobuf for serialization. */ 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..ed9f4ef8f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -18,21 +18,6 @@ class MathUtilTest extends UtilityClassTest { super(MathUtil.class); } - @Test - void testClamp() { - // int - assertEquals(5, MathUtil.clamp(10, 1, 5)); - - // double - assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5)); - - // negative int - assertEquals(-5, MathUtil.clamp(-10, -5, -1)); - - // negative double - assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5)); - } - @Test void testApplyDeadbandUnityScale() { // < 0 diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java index 70041ebac2..7c3672d406 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java @@ -9,7 +9,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -297,7 +296,7 @@ class MerweUKFTest { return MatBuilder.fill( Nat.N1(), Nat.N1(), - MathUtil.clamp( + Math.clamp( 8 * Math.sin(Math.PI * Math.sqrt(2.0) * t) + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t) + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t), diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java index be9740820e..df485a2cb9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java @@ -9,7 +9,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -297,7 +296,7 @@ class S3UKFTest { return MatBuilder.fill( Nat.N1(), Nat.N1(), - MathUtil.clamp( + Math.clamp( 8 * Math.sin(Math.PI * Math.sqrt(2.0) * t) + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t) + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t), diff --git a/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java b/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java index baf9275dbb..c66a8f388d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java @@ -6,7 +6,6 @@ package edu.wpi.first.math.optimization; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.MathUtil; import java.util.function.DoubleUnaryOperator; import org.junit.jupiter.api.Test; @@ -20,7 +19,7 @@ class SimulatedAnnealingTest { var simulatedAnnealing = new SimulatedAnnealing( 2.0, - x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3), + x -> Math.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3), function::applyAsDouble); double solution = simulatedAnnealing.solve(-1.0, 5000); @@ -37,7 +36,7 @@ class SimulatedAnnealingTest { var simulatedAnnealing = new SimulatedAnnealing( 2.0, - x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7), + x -> Math.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7), function::applyAsDouble); double solution = simulatedAnnealing.solve(-1.0, 5000);