/** * 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(FramePose3DReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return Pose3DReadOnly.super.geometricallyEquals(other, epsilon); }
/** * Asserts that the two pose 3Ds 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 3D. Not modified. * @param actual the actual pose 3D. 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 3Ds do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ public static void assertPose3DGeometricallyEquals(String messagePrefix, Pose3DReadOnly expected, Pose3DReadOnly 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); } }