/** * Compares {@code this} to {@code other} to determine if the two poses are geometrically similar. * <p> * Two poses are geometrically equal if both their position and orientation are geometrically equal. * </p> * * @param other the pose to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two poses represent the same geometry, {@code false} otherwise. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default boolean geometricallyEquals(FramePose2DReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return Pose2DReadOnly.super.geometricallyEquals(other, epsilon); }
/** * Asserts that the two pose 2Ds represent the same geometry to an {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the error message. * @param expected the expected pose 2D. Not modified. * @param actual the actual pose 2D. Not modified. * @param epsilon the tolerance to use. * @param format the format to use for printing each component when an {@code AssertionError} is * thrown. * @throws AssertionError if the two pose 2Ds do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ public static void assertPose2DGeometricallyEquals(String messagePrefix, Pose2DReadOnly expected, Pose2DReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.geometricallyEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }