/** * Sets this motion vector to {@code spatialVector} and updates all its reference frames. * * @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. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, SpatialVectorReadOnly spatialVector) { setIncludingFrame(bodyFrame, baseFrame, spatialVector.getReferenceFrame(), spatialVector.getAngularPart(), spatialVector.getLinearPart()); }
/** * Sets this wrench to {@code spatialVector}. * * @param bodyFrame the body frame associated with the given spatial force. * @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, SpatialVectorReadOnly spatialVector) { set(bodyFrame, spatialVector.getReferenceFrame(), spatialVector.getAngularPart(), spatialVector.getLinearPart()); }
/** * 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()); }
/** * Sets this wrench to {@code other}. * * @param other the other wrench to copy. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames in {@code other} do not * match {@code this}. */ default void set(WrenchReadOnly other) { set(other.getBodyFrame(), other.getReferenceFrame(), other.getAngularPart(), other.getLinearPart()); }
/** * Sets this vector to {@code other} and updates the frame of this vector. * * @param other the other vector to copy. Not modified. */ default void setIncludingFrame(SpatialVectorReadOnly other) { setReferenceFrame(other.getReferenceFrame()); getAngularPart().set((Vector3DReadOnly) other.getAngularPart()); getLinearPart().set((Vector3DReadOnly) other.getLinearPart()); }
/** * Sets this vector to {@code other}. * * @param other the other vector to copy. Not modified. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default void set(SpatialVectorReadOnly other) { set(other.getReferenceFrame(), other.getAngularPart(), other.getLinearPart()); }
/** * Sets this motion vector to {@code other} including the reference frames. * * @param other the other motion vector used to update {@code this}. Not modified. */ default void setIncludingFrame(SpatialMotionReadOnly other) { setIncludingFrame(other.getBodyFrame(), other.getBaseFrame(), other.getReferenceFrame(), other.getAngularPart(), other.getLinearPart()); }
/** * 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()); }
/** * Adds the given wrench to {@code this} after performing the usual reference frame checks. * <p> * {@code this += other} * </p> * * @param other the other wrench to add to this. Not modified. * @throws ReferenceFrameMismatchException if the reference frame of {@code other} do not match * this wrench's reference frames. */ default void add(WrenchReadOnly other) { add(other.getBodyFrame(), other); }
/** * Sets this wrench to {@code other} including the reference frames. * * @param other the other wrench used to update {@code this}. Not modified. */ default void setIncludingFrame(WrenchReadOnly other) { setIncludingFrame(other.getBodyFrame(), other); }
/** * Tests on a per component basis if this vector is equal to the given {@code other} to an * {@code epsilon} and both vectors have the same frames. * * @param other the other wrench to compare against this. Not modified. * @param epsilon the tolerance to use when comparing each component. * @return {@code true} if the two vectors are equal, {@code false} otherwise. */ default boolean epsilonEquals(WrenchReadOnly other, double epsilon) { if (getBodyFrame() != other.getBodyFrame()) return false; return SpatialForceReadOnly.super.epsilonEquals(other, epsilon); }
/** * Sets all the components of this vector to zero and updates its reference frames. * * @param bodyFrame the new body frame. * @param baseFrame the new base frame. * @param expressedInFrame the new reference frame in which this motion is expressed. */ default void setToZero(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setToZero(expressedInFrame); }
@Override public ReferenceFrame getReferenceFrame() { return referenceAcceleration.getReferenceFrame(); }
/** * Tests on a per component basis if this vector is equal to the given {@code other} to an * {@code epsilon} and both vectors have the same frames. * * @param other the other motion vector to compare against this. Not modified. * @param epsilon the tolerance to use when comparing each component. * @return {@code true} if the two vectors are equal, {@code false} otherwise. */ @Override public boolean epsilonEquals(SpatialAcceleration other, double epsilon) { return SpatialAccelerationBasics.super.epsilonEquals(other, epsilon); }
@Override default boolean containsNaN() { return SpatialVectorBasics.super.containsNaN(); }
/** {@inheritDoc} */ @Override public void set(Wrench other) { WrenchBasics.super.set(other); }
/** {@inheritDoc} */ @Override default boolean containsNaN() { return SpatialInertiaReadOnly.super.containsNaN(); }
/** {@inheritDoc} */ @Override public void set(SpatialAcceleration other) { SpatialAccelerationBasics.super.set(other); }
/** {@inheritDoc} */ @Override public void set(SpatialForce other) { SpatialForceBasics.super.set(other); }
/** {@inheritDoc} */ @Override public void set(SpatialInertia other) { SpatialInertiaBasics.super.set(other); }