Add isNotWithin() to Translation2dSubject and Translation3dSubject

limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java

@@ -141,7 +141,7 @@ public final void presentTest1() throws Exception {
     var blueEstimate = locationalData.getBotPoseEstimateBlue().get();
     assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0);
     var expectedPoseEstimate = new Pose2d(15.62, 4.52, rotation.toRotation2d());
-    assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate);
+    assertThat(blueEstimate.pose()).isWithin(0.01).of(expectedPoseEstimate);
 
     assertThat(locationalData.getBotPoseEstimateRed()).isPresent();
     var redEstimate = locationalData.getBotPoseEstimateRed().get();
@@ -173,7 +173,7 @@ public final void presentTest2() throws Exception {
     Rotation3d rotation =
         new Rotation3d(Math.toRadians(-5.18), Math.toRadians(-24.32), Math.toRadians(-164.64));
     Pose3d expectedPose = new Pose3d(7.469, 0.81, 1.01, rotation);
-    assertThat(actualPose).isWithin(0.005).of(expectedPose);
+    assertThat(actualPose).isWithin(0.01).of(expectedPose);
 
     assertThat(locationalData.getBotPoseEstimateBlue()).isPresent();
     var blueEstimate = locationalData.getBotPoseEstimateBlue().get();
@@ -201,7 +201,7 @@ public final void getBotposeBlue() throws Exception {
     Pose3d actualPose = botposeBlue.get();
     Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(-123.49));
     Pose3d expectedPose = new Pose3d(4.72, 5.20, 0, expectedRotation);
-    assertThat(actualPose).isWithin(0.005).of(expectedPose);
+    assertThat(actualPose).isWithin(0.01).of(expectedPose);
   }
 
   @Test
@@ -218,7 +218,7 @@ public final void getBotposeRed() throws Exception {
 
     Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(56.51));
     Pose3d expectedPose = new Pose3d(11.83, 3.01, 0, expectedRotation);
-    assertThat(actualPose).isWithin(0.005).of(expectedPose);
+    assertThat(actualPose).isWithin(0.01).of(expectedPose);
   }
 
   @Test

testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.FailureMetadata;
 import com.google.common.truth.Subject;
@@ -56,8 +58,20 @@ public TolerantComparison<Pose2d> isWithin(double tolerance) {
     return new TolerantComparison<Pose2d>() {
       @Override
       public void of(Pose2d expected) {
-        translation().isWithin(tolerance).of(expected.getTranslation());
-        rotation().isWithin(tolerance).of(expected.getRotation());
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        Pose2d actual = nonNullActualPose();
+        if (!Translation2dSubject.equalWithinTolerance(
+                expected.getTranslation(), actual.getTranslation(), tolerance)
+            || !Rotation2dSubject.equalWithinTolerance(
+                expected.getRotation(), actual.getRotation(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
       }
     };
   }

testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.FailureMetadata;
 import com.google.common.truth.Subject;
@@ -56,8 +58,20 @@ public TolerantComparison<Pose3d> isWithin(double tolerance) {
     return new TolerantComparison<Pose3d>() {
       @Override
       public void of(Pose3d expected) {
-        translation().isWithin(tolerance).of(expected.getTranslation());
-        rotation().isWithin(tolerance).of(expected.getRotation());
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        Pose3d actual = nonNullActualPose();
+        if (!Translation3dSubject.equalWithinTolerance(
+                expected.getTranslation(), actual.getTranslation(), tolerance)
+            || !Rotation3dSubject.equalWithinTolerance(
+                expected.getRotation(), actual.getRotation(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
       }
     };
   }

testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.DoubleSubject;
 import com.google.common.truth.FailureMetadata;
@@ -53,11 +55,27 @@ public TolerantComparison<Rotation2d> isWithin(double tolerance) {
     return new TolerantComparison<Rotation2d>() {
       @Override
       public void of(Rotation2d expected) {
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
         getRadians().isWithin(tolerance).of(expected.getRadians());
       }
     };
   }
 
+  static boolean equalWithinTolerance(
+      Rotation2d rotation1, Rotation2d rotation2, double tolerance) {
+    double distance = rotation1.getRadians() - rotation2.getRadians();
+    return Math.abs(distance) < tolerance;
+  }
+
   public void isZero() {
     if (!Rotation2d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));

testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.DoubleSubject;
 import com.google.common.truth.FailureMetadata;
@@ -53,13 +55,37 @@ public TolerantComparison<Rotation3d> isWithin(double tolerance) {
     return new TolerantComparison<Rotation3d>() {
       @Override
       public void of(Rotation3d expected) {
-        x().isWithin(tolerance).of(expected.getX()); // roll, in radians
-        y().isWithin(tolerance).of(expected.getY()); // pitch, in radians
-        z().isWithin(tolerance).of(expected.getZ()); // yaw, in radians
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
+        x().isWithin(tolerance).of(expected.getX());
+        y().isWithin(tolerance).of(expected.getY());
+        z().isWithin(tolerance).of(expected.getZ());
       }
     };
   }
 
+  static boolean equalWithinTolerance(
+      Rotation3d rotation1, Rotation3d rotation2, double tolerance) {
+    double distance = rotation1.getX() - rotation2.getX(); // roll, in radians
+    if (Math.abs(distance) >= tolerance) {
+      return false;
+    }
+    distance = rotation1.getY() - rotation2.getY(); // pitch, in radians
+    if (Math.abs(distance) >= tolerance) {
+      return false;
+    }
+    distance = rotation1.getZ() - rotation2.getZ(); // yaw, in radians
+    return Math.abs(distance) < tolerance;
+  }
+
   public void isZero() {
     if (!Rotation3d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));

testing/src/main/java/com/team2813/lib2813/testing/truth/SubjectHelper.java

@@ -0,0 +1,41 @@
+/*
+Copyright 2026 Prospect Robotics SWENext Club
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+package com.team2813.lib2813.testing.truth;
+
+import static com.google.common.base.Preconditions.checkArgument;
+import static java.lang.Double.doubleToLongBits;
+
+class SubjectHelper {
+  private static final long NEG_ZERO_BITS = doubleToLongBits(-0.0);
+
+  /**
+   * Ensures that the given tolerance is a non-negative finite value, i.e. not {@code Double.NaN},
+   * {@code Double.POSITIVE_INFINITY}, or negative, including {@code -0.0}.
+   */
+  static void checkTolerance(double tolerance) {
+    checkArgument(!Double.isNaN(tolerance), "tolerance cannot be NaN");
+    checkArgument(tolerance >= 0.0, "tolerance (%s) cannot be negative", tolerance);
+    checkArgument(
+        doubleToLongBits(tolerance) != NEG_ZERO_BITS,
+        "tolerance (%s) cannot be negative",
+        tolerance);
+    checkArgument(tolerance != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY");
+  }
+
+  private SubjectHelper() {
+    throw new AssertionError("Not instantiable");
+  }
+}

testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.DoubleSubject;
 import com.google.common.truth.FailureMetadata;
@@ -54,12 +56,44 @@ public TolerantComparison<Translation2d> isWithin(double tolerance) {
     return new TolerantComparison<Translation2d>() {
       @Override
       public void of(Translation2d expected) {
-        x().isWithin(tolerance).of(expected.getX());
-        y().isWithin(tolerance).of(expected.getY());
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
       }
     };
   }
 
+  public TolerantComparison<Translation2d> isNotWithin(double tolerance) {
+    return new TolerantComparison<Translation2d>() {
+      @Override
+      public void of(Translation2d expected) {
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected not to be", expected),
+              fact("but was", actual),
+              fact("within tolerance", tolerance));
+        }
+      }
+    };
+  }
+
+  static boolean equalWithinTolerance(
+      Translation2d translation1, Translation2d translation2, double tolerance) {
+    double distance = translation1.getDistance(translation2);
+    return Math.abs(distance) < tolerance;
+  }
+
   public void isZero() {
     if (!Translation2d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));

testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java

@@ -15,8 +15,10 @@
 */
 package com.team2813.lib2813.testing.truth;
 
+import static com.google.common.truth.Fact.fact;
 import static com.google.common.truth.Fact.simpleFact;
 import static com.google.common.truth.Truth.assertAbout;
+import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance;
 
 import com.google.common.truth.DoubleSubject;
 import com.google.common.truth.FailureMetadata;
@@ -54,13 +56,44 @@ public TolerantComparison<Translation3d> isWithin(double tolerance) {
     return new TolerantComparison<Translation3d>() {
       @Override
       public void of(Translation3d expected) {
-        x().isWithin(tolerance).of(expected.getX());
-        y().isWithin(tolerance).of(expected.getY());
-        z().isWithin(tolerance).of(expected.getZ());
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected", expected),
+              fact("but was", actual),
+              fact("outside tolerance", tolerance));
+        }
       }
     };
   }
 
+  public TolerantComparison<Translation3d> isNotWithin(double tolerance) {
+    return new TolerantComparison<Translation3d>() {
+      @Override
+      public void of(Translation3d expected) {
+        if (expected == null) {
+          throw new NullPointerException("Expected value cannot be null.");
+        }
+        checkTolerance(tolerance);
+        if (equalWithinTolerance(expected, nonNullActual(), tolerance)) {
+          failWithoutActual(
+              fact("expected not to be", expected),
+              fact("but was", actual),
+              fact("within tolerance", tolerance));
+        }
+      }
+    };
+  }
+
+  static boolean equalWithinTolerance(
+      Translation3d translation1, Translation3d translation2, double tolerance) {
+    double distance = translation1.getDistance(translation2);
+    return Math.abs(distance) < tolerance;
+  }
+
   public void isZero() {
     if (!Translation3d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));