/** * Computes the distance between {@code this} and {@code other} as the absolute difference in * angle:<br> * {@code distance = Math.abs(this.yaw - other.yaw)} * * @param other the other orientation 2D. Not modified. * @return the distance between {@code this} and {@code other} contained in [0, <i>pi</pi>]. */ default double distance(Orientation2DReadOnly other) { return Math.abs(difference(other)); }
/** * 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); }
/** * Performs the inverse of the transform to the given {@code tupleToTransform} by the rotation about * the z-axis described by this. * * <pre> * tupleToTransform = / cos(-yaw) -sin(-yaw) \ * tupleToTransform * \ sin(-yaw) cos(-yaw) / * </pre> * * @param tupleToTransform the tuple to transform. Modified. */ default void inverseTransform(Tuple2DBasics tupleToTransform) { inverseTransform(tupleToTransform, tupleToTransform); }
/** * Transforms the given {@code tupleToTransform} by the rotation about the z-axis described by this. * * <pre> * tupleToTransform = / cos(yaw) -sin(yaw) \ * tupleToTransform * \ sin(yaw) cos(yaw) / * </pre> * * @param tupleToTransform the tuple to transform. Modified. */ default void transform(Tuple2DBasics tupleToTransform) { transform(tupleToTransform, tupleToTransform); }
/** * Tests if this orientation 2D is exactly equal to {@code other}. * <p> * Note that this method performs number comparison and not an angle comparison, such that: * -<i>pi</i> ≠ <i>pi</i>. * </p> * * @param other the other orientation 2D to compare against this. Not modified. * @return {@code true} if the two orientations are exactly equal, {@code false} otherwise. */ default boolean equals(Orientation2DReadOnly other) { if (other == null) return false; else return getYaw() == other.getYaw(); } }
/** * Tests if the yaw angle of this orientation is equal to an {@code epsilon} to the yaw of * {@code other}. * <p> * Note that this method performs number comparison and not an angle comparison, such that: * -<i>pi</i> ≠ <i>pi</i>. * </p> * <p> * If the two orientations have different frames, this method returns {@code false}. * </p> * * @param other the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the two orientations are equal and are expressed in the same reference * frame, {@code false} otherwise. */ default boolean epsilonEquals(FrameOrientation2DReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; return Orientation2DReadOnly.super.epsilonEquals(other, epsilon); }
/** * 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); }
/** * Tests if this orientation 2D is exactly equal to {@code other}. * <p> * Note that this method performs number comparison and not an angle comparison, such that: * -<i>pi</i> ≠ <i>pi</i>. * </p> * * @param other the other orientation 2D to compare against this. Not modified. * @return {@code true} if the two orientations are exactly equal component-wise and are expressed * in the same reference frame, {@code false} otherwise. */ default boolean equals(FrameOrientation2DReadOnly other) { if (other == null || getReferenceFrame() != other.getReferenceFrame()) return false; return Orientation2DReadOnly.super.equals(other); } }
/** {@inheritDoc} */ @Override default boolean containsNaN() { return Orientation2DReadOnly.super.containsNaN(); }
/** * Performs the inverse of the transform to the given {@code tupleToTransform} by the rotation about * the z-axis described by this. * * <pre> * / cos(-yaw) -sin(-yaw) 0 \ * tupleToTransform = | sin(-yaw) cos(-yaw) 0 | * tupleToTransform * \ 0 0 1 / * </pre> * * @param tupleToTransform the tuple to transform. Modified. */ default void inverseTransform(Tuple3DBasics tupleToTransform) { transform(tupleToTransform, tupleToTransform); }
/** * Tests if this orientation 2D contains {@link Double#NaN}. * * @return {@code true} if this orientation 2D contains a {@link Double#NaN}, {@code false} * otherwise. */ default boolean containsNaN() { return Double.isNaN(getYaw()); }
/** * Asserts on a per component basis that the two orientation 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 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 are not equal. If only one of the arguments * is equal to {@code null}. */ public static void assertOrientation2DEquals(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.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
/** * 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); } }
/** * 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()); }
/** * 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(); }
/** * Transforms the given {@code tupleToTransform} by the rotation about the z-axis described by this. * * <pre> * / cos(yaw) -sin(yaw) 0 \ * tupleToTransform = | sin(yaw) cos(yaw) 0 | * tupleToTransform * \ 0 0 1 / * </pre> * * @param tupleToTransform the tuple to transform. Modified. */ default void transform(Tuple3DBasics tupleToTransform) { transform(tupleToTransform, tupleToTransform); }
/** * Performs the inverse of the transform to the given {@code tupleOriginal} by the rotation about * the z-axis described by this and stores the result in {@code tupleTransformed}. * * <pre> * tupleTransformed = / cos(-yaw) -sin(-yaw) \ * tupleOriginal * \ sin(-yaw) cos(-yaw) / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. * @throws ReferenceFrameMismatchException if reference frame of {@code this} and * {@code tupleTransformed} do not match. */ default void inverseTransform(Tuple2DReadOnly tupleOriginal, FixedFrameTuple2DBasics tupleTransformed) { checkReferenceFrameMatch(tupleTransformed); Orientation2DReadOnly.super.inverseTransform(tupleOriginal, tupleTransformed); }
/** * Computes and returns the difference between {@code this} and {@code other}:<br> * {@code distance = this.yaw - other.yaw} * * @param other the other orientation 2D. Not modified. * @return the difference between {@code this} and {@code other} contained in [-<i>pi</i>, * <i>pi</pi>]. */ default double difference(Orientation2DReadOnly other) { return EuclidCoreTools.angleDifferenceMinusPiToPi(getYaw(), other.getYaw()); }
/** * 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. */ default boolean geometricallyEquals(Orientation2DReadOnly other, double epsilon) { return Math.abs(difference(other)) <= epsilon; }
/** * 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); }