[wpiunits] Fix measure isNear function (wpilibsuite#6313)

Now the function allows comparison between negative numbers, positive numbers or both.
KangarooKoala · Jan 31, 2024 · 90bb6cf · 90bb6cf
1 parent cb094e4
commit 90bb6cf
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 3 deletions.
diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Measure.java b/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
@@ -260,10 +260,9 @@ default boolean isNear(Measure<?> other, double varianceThreshold) {
     }
 
     // abs so negative inputs are calculated correctly
-    var allowedVariance = Math.abs(varianceThreshold);
+    var tolerance = Math.abs(other.baseUnitMagnitude() * varianceThreshold);
 
-    return other.baseUnitMagnitude() * (1 - allowedVariance) <= this.baseUnitMagnitude()
-        && other.baseUnitMagnitude() * (1 + allowedVariance) >= this.baseUnitMagnitude();
+    return Math.abs(this.baseUnitMagnitude() - other.baseUnitMagnitude()) <= tolerance;
   }
 
   /**

diff --git a/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java b/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
@@ -319,5 +319,29 @@ void testIsNear() {
     assertFalse(measureA.isNear(measureB, 0.739370));
     assertTrue(measureA.isNear(measureB, 0.739375));
     assertTrue(measureA.isNear(measureB, 100)); // some stupidly large range +/- 10000%
+
+    var measureC = unit.of(-1.21);
+    var measureD = unit.ofBaseUnits(-64);
+
+    assertTrue(measureC.isNear(measureC, 0));
+    assertTrue(measureD.isNear(measureD, 0));
+
+    assertFalse(measureC.isNear(measureD, 0));
+    assertFalse(measureC.isNear(measureD, 0.50));
+    assertFalse(measureC.isNear(measureD, 0.739370));
+    assertTrue(measureC.isNear(measureD, 0.739375));
+    assertTrue(measureC.isNear(measureD, 100)); // some stupidly large range +/- 10000%
+
+    var measureE = Units.Meters.of(1);
+    var measureF = Units.Feet.of(-3.28084);
+
+    assertTrue(measureE.isNear(measureF, 2.01));
+    assertFalse(measureE.isNear(measureF, 1.99));
+
+    assertTrue(measureF.isNear(measureE, 2.01));
+    assertFalse(measureF.isNear(measureE, 1.99));
+
+    assertTrue(Units.Feet.zero().isNear(Units.Millimeters.zero(), 0.001));
+    assertFalse(Units.Feet.of(2).isNear(Units.Millimeters.of(0), 0.001));
   }
 }