/** * 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 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 x-coordinate of the position part of this pose 2D. * * @return the x-coordinate of this pose 2D. */ default double getX() { return getPosition().getX(); }
/** * 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(); }
/** * 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); }
/** * 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()); }
/** * 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); } }
/** * 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()); }
/** * 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()); }
/** * 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(); }
/** * Sets this pose 2D to the {@code other} pose 2D. * * @param other the other pose 2D. Not modified. */ default void set(Pose2DReadOnly other) { setPosition(other.getPosition()); setOrientation(other.getOrientation()); }
/** * Performs a linear interpolation from {@code pose1} to {@code pose2} given the percentage * {@code alpha}. * <p> * this.position = (1.0 - alpha) * pose1.position + alpha * pose2.position<br> * this.orientation = (1.0 - alpha) * pose1.orientation + alpha * pose2.orientation * </p> * * @param pose1 the first pose 2D used in the interpolation. Not modified. * @param pose2 the second pose 2D used in the interpolation. Not modified. * @param alpha the percentage to use for the interpolation. A value of 0 will result in setting * {@code this} to {@code pose1}, while a value of 1 is equivalent to setting {@code this} * to {@code pose2}. */ default void interpolate(Pose2DReadOnly pose1, Pose2DReadOnly pose2, double alpha) { getPosition().interpolate(pose1.getPosition(), pose2.getPosition(), alpha); getOrientation().interpolate(pose1.getOrientation(), pose2.getOrientation(), alpha); }
/** * Performs a linear interpolation from {@code this} to {@code other} given the percentage * {@code alpha}. * <p> * this.position = (1.0 - alpha) * this.position + alpha * other.position<br> * this.orientation = (1.0 - alpha) * this.orientation + alpha * other.orientation * </p> * * @param other the other pose 2D used for the interpolation. Not modified. * @param alpha the percentage used for the interpolation. A value of 0 will result in not modifying * {@code this}, while a value of 1 is equivalent to setting {@code this} to * {@code other}. */ default void interpolate(Pose2DReadOnly other, double alpha) { getPosition().interpolate(other.getPosition(), alpha); getOrientation().interpolate(other.getOrientation(), alpha); }