diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Measure.java b/wpiunits/src/main/java/edu/wpi/first/units/Measure.java index 3d77659704..41036b0e8e 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/Measure.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/Measure.java @@ -265,6 +265,25 @@ public interface Measure> extends Comparable> { return Math.abs(this.baseUnitMagnitude() - other.baseUnitMagnitude()) <= tolerance; } + /** + * Checks if this measure is near another measure of the same unit, with a specified tolerance of + * the same unit. + * + *
+   *     Meters.of(1).isNear(Meters.of(1.2), Millimeters.of(300)) // true
+   *     Degrees.of(90).isNear(Rotations.of(0.5), Degrees.of(45)) // false
+   * 
+ * + * @param other the other measure to compare against. + * @param tolerance the tolerance allowed in which the two measures are defined as near each + * other. + * @return true if this unit is near the other measure, otherwise false. + */ + default boolean isNear(Measure other, Measure tolerance) { + return Math.abs(this.baseUnitMagnitude() - other.baseUnitMagnitude()) + <= Math.abs(tolerance.baseUnitMagnitude()); + } + /** * Checks if this measure is equivalent to another measure of the same unit. * diff --git a/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java b/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java index 31be4ca581..dd943c449e 100644 --- a/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java +++ b/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java @@ -303,7 +303,7 @@ class MeasureTest { } @Test - void testIsNear() { + void testIsNearVarianceThreshold() { var unit = new ExampleUnit(92); var measureA = unit.of(1.21); var measureB = unit.ofBaseUnits(64); @@ -344,4 +344,42 @@ class MeasureTest { assertTrue(Units.Feet.zero().isNear(Units.Millimeters.zero(), 0.001)); assertFalse(Units.Feet.of(2).isNear(Units.Millimeters.of(0), 0.001)); } + + @Test + void testIsNearMeasureTolerance() { + var measureCompared = Units.Meters.of(1); + var measureComparing = Units.Meters.of(1.2); + + // Positive value with positive tolerance + assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(300))); + assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(10))); + + measureCompared = measureCompared.negate(); + measureComparing = measureComparing.negate(); + + // Negative value with positive tolerance + assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(300))); + assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(10))); + + measureCompared = measureCompared.negate(); + measureComparing = measureComparing.negate(); + + // Positive value with negative tolerance + assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(-300))); + assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(-10))); + + measureCompared = measureCompared.negate(); + measureComparing = measureComparing.negate(); + + // Negative value with negative tolerance. + assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(-300))); + assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(-10))); + + measureCompared = measureCompared.negate(); + measureComparing = measureComparing.negate(); + + // Tolerance exact difference between measures. + assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(200))); + assertTrue(measureCompared.isNear(measureComparing, Units.Centimeters.of(-20))); + } }