/** * Gets the y-component of this vector's linear part. * * @return the y-component of this vector's linear part. */ default double getLinearPartY() { return getLinearPart().getY(); }
/** * Gets the x-component of this vector's linear part. * * @return the x-component of this vector's linear part. */ default double getLinearPartX() { return getLinearPart().getX(); }
/** * Gets the z-component of this vector's linear part. * * @return the z-component of this vector's linear part. */ default double getLinearPartZ() { return getLinearPart().getZ(); }
/** * Subtracts the given spatial vector to {@code this} providing the expected {@code bodyFrame} * for additional safety. * <p> * {@code this -= spatialVector} * </p> * * @param bodyFrame the body frame associated with the given vector. * @param spatialVector the spatial vector to subtract to {@code this}. Not modified. * @throws ReferenceFrameMismatchException if {@code bodyFrame != this.getBodyFrame()} or if the * given vector is not expressed in the same frame as this wrench. */ default void sub(ReferenceFrame bodyFrame, SpatialVectorReadOnly spatialVector) { sub(bodyFrame, spatialVector.getAngularPart(), spatialVector.getLinearPart()); }
/** * Adds the given spatial vector to {@code this} providing the expected {@code bodyFrame} for * additional safety. * <p> * {@code this += spatialVector} * </p> * * @param bodyFrame the body frame associated with the given vector. * @param spatialVector the spatial vector to add to {@code this}. Not modified. * @throws ReferenceFrameMismatchException if {@code bodyFrame != this.getBodyFrame()} or if the * given vector is not expressed in the same frame as this wrench. */ default void add(ReferenceFrame bodyFrame, SpatialVectorReadOnly spatialVector) { add(bodyFrame, spatialVector.getAngularPart(), spatialVector.getLinearPart()); }
/** * Calculates and returns the value of the dot product of this vector with {@code other}. * <p> * For instance, the dot product of two vectors p and q is defined as: <br> * p ⋅ q = ∑<sub>i=1:6</sub>(p<sub>i</sub> × q<sub>i</sub>) * </p> * * @param other the other vector used for the dot product. Not modified. * @return the value of the dot product. */ default double dot(SpatialVectorReadOnly other) { checkReferenceFrameMatch(other); return getAngularPart().dot(other.getAngularPart()) + getLinearPart().dot(other.getLinearPart()); }
/** * 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 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 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()); }
/** * Tests if this vector contains a {@link Double#NaN}. * * @return {@code true} if this vector contains a {@link Double#NaN}, {@code false} otherwise. */ default boolean containsNaN() { return getAngularPart().containsNaN() || getLinearPart().containsNaN(); }
/** * Packs the components of this vector in an array starting from {@code startIndex}. * <p> * The components are packed in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param startIndex the index in the array where the first component is stored. * @param arrayToPack the array in which this vector is stored. Modified. */ default void get(int startIndex, float[] arrayToPack) { getAngularPart().get(startIndex, arrayToPack); getLinearPart().get(startIndex + 3, arrayToPack); }
/** * 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()); }
/** * Subtracts the given vector to this vector performing a per-component subtraction. * <p> * {@code this -= other} * </p> * * @param other the other vector to subtract to this vector. Not modified. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default void sub(SpatialVectorReadOnly other) { checkReferenceFrameMatch(other); sub((Vector3DReadOnly) other.getAngularPart(), (Vector3DReadOnly) other.getLinearPart()); }
/** * Adds the given vector to this vector performing a per-component addition. * <p> * {@code this += other} * </p> * * @param other the other vector to add to this vector. Not modified. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default void add(SpatialVectorReadOnly other) { checkReferenceFrameMatch(other); add((Vector3DReadOnly) other.getAngularPart(), (Vector3DReadOnly) other.getLinearPart()); }
/** * Packs the components of this vector in an array starting from {@code startIndex}. * <p> * The components are packed in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param startIndex the index in the array where the first component is stored. * @param arrayToPack the array in which this vector is stored. Modified. */ default void get(int startIndex, double[] arrayToPack) { getAngularPart().get(startIndex, arrayToPack); getLinearPart().get(startIndex + 3, arrayToPack); }
/** * Calculates and returns the square of the magnitude of this vector. * <p> * length<sup>2</sup> = angular<sub>x</sub><sup>2</sup> + angular<sub>y</sub><sup>2</sup> + * angular<sub>z</sub><sup>2</sup> + linear<sub>x</sub><sup>2</sup> + * linear<sub>y</sub><sup>2</sup> + linear<sub>z</sub><sup>2</sup> * </p> * <p> * This method is usually preferred over {@link #length()} when calculation speed matters and * knowledge of the actual magnitude does not, i.e. when comparing several vectors by theirs * magnitude. * </p> * * @return the square of the magnitude of this vector. */ default double lengthSquared() { return getAngularPart().lengthSquared() + getLinearPart().lengthSquared(); }
public void appendWaypoint(double timeAtWaypoint, SpatialVectorReadOnly waypoint) { checkNumberOfWaypoints(numberOfWaypoints.getIntegerValue() + 1); appendWaypointUnsafe(timeAtWaypoint, waypoint.getAngularPart(), waypoint.getLinearPart()); }
/** * Tests on a per component basis, if this vector is exactly equal to {@code other} and expressed * in the same reference frame. * * @param other the other vector to compare against this. Not modified. * @return {@code true} if the two vectors are exactly equal component-wise, {@code false} * otherwise. */ default boolean equals(SpatialVectorReadOnly other) { if (other == null) return false; if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getAngularPart().equals(other.getAngularPart())) return false; if (!getLinearPart().equals(other.getLinearPart())) return false; return true; } }
/** * 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}. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(SpatialVectorReadOnly)}. * </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. */ default void setMatchingFrame(SpatialVectorReadOnly other) { getAngularPart().set((Vector3DReadOnly) other.getAngularPart()); getLinearPart().set((Vector3DReadOnly) other.getLinearPart()); other.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this); }