/** * Transform this twist by the inverse of the given transform. * <p> * Effectively, the new twist (T<sub>body</sub><sup>des, base</sup> = [ω<sub>new</sub>; * ν<sub>new</sub>]) is calculated as follows: * * <pre> * ω<sub>new</sub> = R<sup>T</sup> ω * ν<sub>new</sub> = R<sup>T</sup> ( ν - P × ω ) * </pre> * * where: * <ul> * <li>ω and ν are the current angular and linear parts of this spatial vector, * <li>R and P are the rotation and translation parts of the given transform. * </ul> * </p> * * @throws UnsupportedOperationException if the given transform is not a * {@code RigidBodyTransform}. */ @Override default void applyInverseTransform(Transform transform) { if (transform instanceof RigidBodyTransform) applyInverseTransform((RigidBodyTransform) transform); else throw new UnsupportedOperationException("The feature applyInverseTransform is not supported for the transform of the type: " + transform.getClass().getSimpleName()); }
/** * Transform this twist using the given transform. Effectively, the new twist * (T<sub>body</sub><sup>des, base</sup> = [ω<sub>new</sub>; ν<sub>new</sub>]) is * calculated as follows: * * <pre> * ω<sub>new</sub> = R ω * ν<sub>new</sub> = R ν + P × ω<sub>new</sub> * </pre> * * where: * <ul> * <li>ω and ν are the current angular and linear parts of this spatial vector, * <li>R and P are the rotation and translation parts of the given transform. * </ul> * </p> * * @throws UnsupportedOperationException if the given transform is not a * {@code RigidBodyTransform}. */ @Override default void applyTransform(Transform transform) { if (transform instanceof RigidBodyTransform) applyTransform((RigidBodyTransform) transform); else throw new UnsupportedOperationException("The feature applyTransform is not supported for the transform of the type: " + transform.getClass().getSimpleName()); }
/** * Sets this motion vector from a given motion measured at a different position. * <p> * Effectively, this motion is updated as follow: * * <pre> * ω<sub>this</sub> = ω<sub>new</sub> * ν<sub>this</sub> = ν<sub>new</sub> + P × ω<sub>new</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param expressedInFrame the reference frame in which the given motion is expressed. * @param angularPart the angular part of the motion. Not modified. * @param linearPart the linear part of the motion measured at the observer position. Not * modified. * @param observerPosition the location at which the motion is measured. Not modified. * @throws ReferenceFrameMismatchException if {@code expressedInFrame} is not the same as * {@code this.getReferenceFrame()}. */ default void set(ReferenceFrame expressedInFrame, Vector3DReadOnly angularPart, Vector3DReadOnly linearPart, Point3DReadOnly observerPosition) { checkExpressedInFrameMatch(expressedInFrame); set(angularPart, linearPart, observerPosition); }
/** * Sets this motion vector given an angular part and linear part. * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param angularPart the vector holding the new values for the angular part. Not modified. * @param linearPart the vector holding the new values for the linear part. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames from the arguments do * not match the current frames of {@code this}. */ default void set(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, Vector3DReadOnly angularPart, Vector3DReadOnly linearPart) { checkReferenceFrameMatch(bodyFrame, baseFrame, expressedInFrame); set(angularPart, linearPart); }
/** * Transform this twist using the given transform. Effectively, the new twist * (T<sub>body</sub><sup>des, base</sup> = [ω<sub>new</sub>; ν<sub>new</sub>]) is * calculated as follows: * * <pre> * ω<sub>new</sub> = R ω * ν<sub>new</sub> = R ν + P × ω<sub>new</sub> * </pre> * * where: * <ul> * <li>ω and ν are the current angular and linear parts of this spatial vector, * <li>R and P are the rotation and translation parts of the given transform. * </ul> * </p> * * @param transform the transform to use on this. Not modified. */ default void applyTransform(RigidBodyTransform transform) { if (transform.hasRotation()) { getAngularPart().applyTransform(transform); getLinearPart().applyTransform(transform); } if (transform.hasTranslation()) addCrossToLinearPart(transform.getTranslationVector(), getAngularPart()); }
/** * Sets this motion vector to {@code other}. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(SpatialMotionReadOnly)}. * </p> * <p> * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set * to {@code other} once transformed to be expressed in {@code this.getReferenceFrame()}. * </p> * * @param other the other vector to copy. Not modified. * @throws ReferenceFrameMismatchException if either the body frame or base frame in * {@code other} does not match {@code this}. */ default void setMatchingFrame(SpatialMotionReadOnly other) { other.checkBodyFrameMatch(getBodyFrame()); other.checkBaseFrameMatch(getBaseFrame()); FixedFrameSpatialVectorBasics.super.setMatchingFrame(other); }
/** * Sets this motion vector from a given motion measured at a different position. * <p> * Effectively, this motion is updated as follow: * * <pre> * ω<sub>this</sub> = ω<sub>new</sub> * ν<sub>this</sub> = ν<sub>new</sub> + P × ω<sub>new</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param angularPart the angular part of the motion. Not modified. * @param linearPart the linear part of the motion measured at the observer position. Not * modified. * @param observerPosition the location at which the motion is measured. Not modified. */ default void set(Vector3DReadOnly angularPart, Vector3DReadOnly linearPart, Point3DReadOnly observerPosition) { getAngularPart().set(angularPart); double linearPartX = linearPart.getX(); double linearPartY = linearPart.getY(); double linearPartZ = linearPart.getZ(); getLinearPart().cross(observerPosition, angularPart); getLinearPart().add(linearPartX, linearPartY, linearPartZ); }
/** * Sets this motion vector given an angular part and linear part. * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param angularPart the vector holding the new values for the angular part. Not modified. * @param linearPart the vector holding the new values for the linear part. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames from the arguments do * not match the current frames of {@code this}. */ default void set(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart) { angularPart.checkReferenceFrameMatch(linearPart); set(bodyFrame, baseFrame, angularPart.getReferenceFrame(), angularPart, linearPart); }
addCrossToLinearPart(getAngularPart(), transform.getTranslationVector()); getAngularPart().applyInverseTransform(transform); getLinearPart().applyInverseTransform(transform);
/** * Sets this motion vector to {@code spatialVector}. * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param spatialVector the spatial vector to copy values from. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames from the arguments do * not match the current frames of {@code this}. */ default void set(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, SpatialVectorReadOnly spatialVector) { set(bodyFrame, baseFrame, spatialVector.getReferenceFrame(), spatialVector.getAngularPart(), spatialVector.getLinearPart()); }
/** * Sets this motion vector from a given motion measured at a different position. * <p> * Effectively, this motion is updated as follow: * * <pre> * ω<sub>this</sub> = ω<sub>new</sub> * ν<sub>this</sub> = ν<sub>new</sub> + P × ω<sub>new</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param angularPart the angular part of the motion. Not modified. * @param linearPart the linear part of the motion measured at the observer position. Not * modified. * @param observerPosition the location at which the motion is measured. Not modified. * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in * {@code this.getReferenceFrame()}. */ default void set(FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart, FramePoint3DReadOnly observerPosition) { angularPart.checkReferenceFrameMatch(linearPart); angularPart.checkReferenceFrameMatch(observerPosition); set(angularPart.getReferenceFrame(), angularPart, linearPart, observerPosition); }
/** * Sets this motion vector to {@code other}. * * @param other the other vector to copy. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames in {@code other} do not * match {@code this}. */ default void set(SpatialMotionReadOnly other) { set(other.getBodyFrame(), other.getBaseFrame(), other.getReferenceFrame(), other.getAngularPart(), other.getLinearPart()); }