/** {@inheritDoc} */ @Override default boolean containsNaN() { return Pose2DReadOnly.super.containsNaN(); }
/** * Tests on a per-component basis if this pose is equal to {@code other} with separate tolerances * for the position {@code positionEpsilon} and the orientation {@code orientationEpsilon}. * * @param other the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the two poses are equal, {@code false} otherwise. */ default boolean epsilonEquals(Pose2DReadOnly other, double epsilon) { return getPosition().epsilonEquals(other.getPosition(), epsilon) && getOrientation().epsilonEquals(other.getOrientation(), epsilon); }
/** * Packs this pose 2D into the given {@code transformToPack}. * * @param transformToPack the rigid-body transform that is set to represent this pose 2D. Modified. */ default void get(RigidBodyTransform transformToPack) { transformToPack.setTranslation(getX(), getY(), 0.0); transformToPack.setRotationYaw(getYaw()); }
/** * Gets a representative {@code String} of {@code pose2D} given a specific format to use. * <p> * Using the default format {@link #DEFAULT_FORMAT}, this provides a {@code String} as follows: * * <pre> * Pose 2D: position = ( 0.174, -0.222 ), orientation = (-0.130 ) * </pre> * </p> * * @param format the format to use for each number. * @param pose2D the object to get the {@code String} of. Not modified. * @return the representative {@code String}. */ public static String getPose2DString(String format, Pose2DReadOnly pose2D) { if (pose2D == null) return "null"; else return getPose2DString(format, pose2D.getPosition(), pose2D.getYaw()); }
/** * 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); }
/** * Tests if this pose is equal to the given {@code other} to an {@code epsilon}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @param epsilon the tolerance to use when comparing.. * @return {@code true} if the two poses are equal and are expressed in the same reference frame, * {@code false} otherwise. */ default boolean epsilonEquals(FramePose2DReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; return Pose2DReadOnly.super.epsilonEquals(other, epsilon); }
/** * Computes the distances between the position part of the two poses. * * @param other the other pose used to measure the distance. Not modified. * @return the distance between the position part of the two poses. */ default double getPositionDistance(Pose2DReadOnly other) { return getPosition().distance(other.getPosition()); }
/** * Computes the absolute angle difference between this pose 2D and {@code other}. * * @param other the other pose 2D used to compute the orientation distance. Not modified. * @return the absolute angle difference between {@code this.orientation} and * {@code other.orientation}. */ default double getOrientationDistance(Pose2DReadOnly other) { return getOrientation().distance(other.getOrientation()); }
/** * Tests if this pose is exactly equal to {@code other}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @return {@code true} if the two poses are exactly equal and are expressed in the same reference * frame, {@code false} otherwise. */ default boolean equals(FramePose2DReadOnly other) { if (other == null || getReferenceFrame() != other.getReferenceFrame()) return false; return Pose2DReadOnly.super.equals(other); } }
/** * 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); } }
/** * Asserts on a per component basis that the two pose 2Ds are equal 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 are not equal. If only one of the arguments is * equal to {@code null}. */ public static void assertPose2DEquals(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.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
/** * Computes the distance between the position of this pose 2D and the given {@code point}. * * @param point the other point used to measure the distance. Not modified. * @return the distance between this pose and the given {@code point}. */ default double getPositionDistance(Point2DReadOnly point) { return getPosition().distance(point); }
/** * Gets the yaw angle of the orientation part of this pose 2D. * * @return the yaw angle of this pose 2D. */ default double getYaw() { return getOrientation().getYaw(); }
/** * Tests on a per component basis, if this pose 2D is exactly equal to {@code other}. * * @param other the other pose 2D to compare against this. Not modified. * @return {@code true} if the two poses are exactly equal component-wise, {@code false} otherwise. */ default boolean equals(Pose2DReadOnly other) { if (other == null) return false; else return getPosition().equals(other.getPosition()) && getOrientation().equals(other.getOrientation()); }
/** * Gets the x-coordinate of the position part of this pose 2D. * * @return the x-coordinate of this pose 2D. */ default double getX() { return getPosition().getX(); }
/** * Computes the absolute angle difference between the orientation part of this pose 2D and the give * {@code orientation}. * * @param other the orientation used to compute the orientation distance. Not modified. * @return the absolute angle difference between {@code this} and {@code orientation}. */ default double getOrientationDistance(Orientation2DReadOnly other) { return getOrientation().distance(other); }
/** * 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); } }
/** * Gets the y-coordinate of the position part of this pose 2D. * * @return the y-coordinate of this pose 2D. */ default double getY() { return getPosition().getY(); }
/** * Sets this pose 3D to the given {@code pose2DReadOnly}. * * @param pose2DReadOnly the pose 2D used to set this pose 3D. Not modified. */ default void set(Pose2DReadOnly pose2DReadOnly) { setPosition(pose2DReadOnly.getPosition(), 0.0); setOrientation(pose2DReadOnly.getOrientation()); }
/** * Tests if this pose 2D contains a {@link Double#NaN}. * * @return {@code true} if either the position or orientation part of this pose 2D contains * {@link Double#NaN}, {@code false} otherwise. */ default boolean containsNaN() { return getPosition().containsNaN() || getOrientation().containsNaN(); }