/** * Compares {@code this} to {@code other} to determine if the two orientations are geometrically * similar, i.e. the difference in yaw of {@code this} and {@code other} is less than or equal to * {@code epsilon}. * * @param other the orientation to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two orientations represent the same geometry, {@code false} * otherwise. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same frame as * {@code this}. */ default boolean geometricallyEquals(FrameOrientation2DReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return Orientation2DReadOnly.super.geometricallyEquals(other, epsilon); }
/** * Asserts that the two orientation 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 orientation 2D. Not modified. * @param actual the actual orientation 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 orientation 2Ds do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ public static void assertOrientation2DGeometricallyEquals(String messagePrefix, Orientation2DReadOnly expected, Orientation2DReadOnly 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); } }
/** * 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. */ default boolean geometricallyEquals(Pose2DReadOnly other, double epsilon) { return getPosition().geometricallyEquals(other.getPosition(), epsilon) && getOrientation().geometricallyEquals(other.getOrientation(), epsilon); } }