/** * 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 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()); }
/** * 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()); }
/** * Sets this orientation 2D to the sum of the two given orientation 2Ds:<br> * {@code this = orientation1 + orientation2} * <p> * Note that resulting angle is computed such that it is contained in [-<i>pi</i>, <i>pi</pi>]. * </p> * * @param orientation1 the first orientation 2D. Not modified. * @param orientation2 the second orientation 2D. Not modified. */ default void add(Orientation2DReadOnly orientation1, Orientation2DReadOnly orientation2) { add(orientation1.getYaw(), orientation2.getYaw()); }
/** * Sets this orientation 2D to the difference of the two given orientation 2Ds:<br> * {@code this = orientation1 - orientation2} * <p> * Note that resulting angle is computed such that it is contained in [-<i>pi</i>, <i>pi</pi>]. * </p> * * @param orientation1 the first orientation 2D. Not modified. * @param orientation2 the second orientation 2D. Not modified. */ default void sub(Orientation2DReadOnly orientation1, Orientation2DReadOnly orientation2) { sub(orientation1.getYaw(), orientation2.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> * * @param other the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the two orientations are equal, {@code false} otherwise. */ default boolean epsilonEquals(Orientation2DReadOnly other, double epsilon) { return EuclidCoreTools.epsilonEquals(getYaw(), other.getYaw(), epsilon); }
/** * Performs a linear interpolation from {@code orientation1} to {@code orientation2} given the * percentage {@code alpha}. * <p> * this = (1.0 - alpha) * orientation1 + alpha * orientation2 * </p> * * @param orientation1 the first orientation 2D used in the interpolation. Not modified. * @param orientation2 the second orientation 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 orientation1}, while a value of 1 is equivalent to setting * {@code this} to {@code orientation2}. */ default void interpolate(Orientation2DReadOnly orientation1, Orientation2DReadOnly orientation2, double alpha) { double deltaYaw = EuclidCoreTools.angleDifferenceMinusPiToPi(orientation2.getYaw(), orientation1.getYaw()); add(orientation1.getYaw(), alpha * deltaYaw); }
/** * 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(); }
/** * Adds the given {@code orientation} to the orientation of this pose 2D. * <p> * If the position part of this pose 2D is to be rotated by the given {@code orientation}, use * {@link #prependRotation(Orientation2DReadOnly)}. * </p> * * @param orientation the orientation to append to this pose 2D. Not modified. */ default void appendRotation(Orientation2DReadOnly orientation) { appendRotation(orientation.getYaw()); }
/** * Subtracts the other orientation 2D from this orientation 2D:<br> * {@code this -= other} * <p> * Note that resulting angle is computed such that it is contained in [-<i>pi</i>, <i>pi</pi>]. * </p> * * @param other the other orientation 2D to subtract. Not modified. */ default void sub(Orientation2DReadOnly other) { sub(other.getYaw()); }
/** * Sets this orientation 2D to the {@code other} orientation 2D. * * @param other the other orientation 2D. Not modified. */ default void set(Orientation2DReadOnly other) { setYaw(other.getYaw()); }
/** * Rotates the position part of this pose 2D by {@code orientation} and adds {@code orientation} to * the orientation part. * <p> * If the rotation should not affect this pose's position, use * {@link #appendRotation(Orientation2DReadOnly)}. * </p> * * @param orientation the orientation to prepend to this pose 2D. Not modified. */ default void prependRotation(Orientation2DReadOnly orientation) { prependRotation(orientation.getYaw()); }
/** * Adds the other orientation 2D to this:<br> * {@code this += other} * <p> * Note that resulting angle is computed such that it is contained in [-<i>pi</i>, <i>pi</pi>]. * </p> * * @param other the other orientation 2D to add to this. Not modified. */ default void add(Orientation2DReadOnly other) { add(other.getYaw()); }
/** * Transforms 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. */ default void transform(Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed) { RotationMatrixTools.applyYawRotation(getYaw(), tupleOriginal, tupleTransformed); }
/** * 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. */ default void inverseTransform(Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed) { RotationMatrixTools.applyYawRotation(-getYaw(), tupleOriginal, tupleTransformed); }
/** * 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> * / cos(-yaw) -sin(-yaw) 0 \ * tupleTransformed = | sin(-yaw) cos(-yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. */ default void inverseTransform(Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) { RotationMatrixTools.applyYawRotation(-getYaw(), tupleOriginal, tupleTransformed); }
/** * Transforms the given {@code tupleOriginal} by the rotation about the z-axis described by this and * stores the result in {@code tupleTransformed}. * * <pre> * / cos(yaw) -sin(yaw) 0 \ * tupleTransformed = | sin(yaw) cos(yaw) 0 | * tupleOriginal * \ 0 0 1 / * </pre> * * @param tupleOriginal the tuple to be transformed. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. */ default void transform(Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) { RotationMatrixTools.applyYawRotation(getYaw(), tupleOriginal, tupleTransformed); }
/** * Gets a representative {@code String} of {@code orientation2D} given a specific format to use. * <p> * Using the default format {@link #DEFAULT_FORMAT}, this provides a {@code String} as follows: * * <pre> * (0.174) * </pre> * </p> * * @param format the format to use for each number. * @param orientation2D the object to get the {@code String} of. Not modified. * @return the representative {@code String}. */ public static String getOrientation2DString(String format, Orientation2DReadOnly orientation2D) { if (orientation2D == null) return "null"; else return getOrientation2DString(format, orientation2D.getYaw()); }
/** * Sets the orientation part of this pose 3D with the given orientation 2D. * * @param orientation the orientation 2D used to set this pose's orientation. Not modified. */ default void setOrientation(Orientation2DReadOnly orientation) { getOrientation().setToYawQuaternion(orientation.getYaw()); }