/** * Sets the orientation from the yaw angle of the given quaternion. * * @param orientation the orientation with the new angle value for this. Not modified. */ default void setOrientation(Orientation3DReadOnly orientation) { getOrientation().set(orientation); }
/** * Sets the orientation from the given orientation 2D. * * @param orientation the orientation with the new angle value for this. Not modified. */ default void setOrientation(Orientation2DReadOnly orientation) { getOrientation().set(orientation); }
/** * Sets the orientation angle value. * * @param yaw the orientation angle value. */ default void setOrientation(double yaw) { getOrientation().setYaw(yaw); }
/** * Sets the orientation yaw angle value. * * @param yaw the orientation angle value. */ default void setYaw(double yaw) { getOrientation().setYaw(yaw); }
/** * Adds the given {@code yaw} angle to the orientation of this pose 2D. * <p> * If the position part of this pose 2D is to be rotated by the given angle, use * {@link #prependRotation(double)}. * </p> * * @param yaw the angle about the z-axis to append to this pose 2D. */ default void appendRotation(double yaw) { getOrientation().add(yaw); }
/** * Sets all the components of this pose 2D to {@link Double#NaN}. */ @Override default void setToNaN() { getPosition().setToNaN(); getOrientation().setToNaN(); }
/** * Sets all the components of this pose 2D to zero. */ @Override default void setToZero() { getPosition().setToZero(); getOrientation().setToZero(); }
/** * Transforms the position and orientation parts of this pose 2D by the given {@code transform}. * * @param transform the geometric transform to apply on this pose 2D. Not modified. * @throws NotAMatrix2DException if the rotation part of {@code transform} is not a transformation * in the XY plane. */ @Override default void applyTransform(Transform transform) { getPosition().applyTransform(transform); getOrientation().applyTransform(transform); }
/** * Transforms the position and orientation parts of this pose 2D by the inverse of the given * {@code transform}. * * @param transform the geometric transform to apply on this pose 2D. Not modified. * @throws NotAMatrix2DException if the rotation part of {@code transform} is not a transformation * in the XY plane. */ @Override default void applyInverseTransform(Transform transform) { getPosition().applyInverseTransform(transform); getOrientation().applyInverseTransform(transform); } }
/** * Rotates the position part of this pose 2D by {@code yaw} and adds {@code yaw} to the orientation * part. * <p> * If the rotation should not affect this pose's position, use {@link #appendRotation(double)}. * </p> * * @param yaw the angle about the z-axis to prepend to this pose 2D. */ default void prependRotation(double yaw) { RotationMatrixTools.applyYawRotation(yaw, getPosition(), getPosition()); getOrientation().add(yaw); }
/** * 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); }
/** * 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); }
/** * Rotates, then adds the translation (x, y) to this pose 2D. * <p> * Use this method if the translation (x, y) is expressed in the local coordinates described by this * pose 2D. Otherwise, use {@link #prependTranslation(double, double)}. * </p> * * @param x the translation distance along the x-axis. * @param y the translation distance along the y-axis. */ default void appendTranslation(double x, double y) { double thisX = getPosition().getX(); double thisY = getPosition().getY(); getPosition().set(x, y); getOrientation().transform(getPosition()); getPosition().add(thisX, thisY); }