/** * 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()); }
/** * 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(); }
/** * 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); }
/** * 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()); }
/** * 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); }