@Override public ReferenceFrame getReferenceFrame() { return referenceVector.getReferenceFrame(); }
/** * Generates a random frame vector that is perpendicular to {@code vectorToBeOrthogonalTo}. * * @param random the random generator to use. * @param vectorToBeOrthogonalTo the frame vector to be orthogonal to. Not modified. * @param normalize whether to normalize the generated frame vector or not. * @return the random frame vector. */ public static FrameVector3D nextOrthogonalFrameVector3D(Random random, FrameVector3DReadOnly vectorToBeOrthogonalTo, boolean normalize) { return nextOrthogonalFrameVector3D(random, vectorToBeOrthogonalTo.getReferenceFrame(), vectorToBeOrthogonalTo, normalize); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { setIncludingFrame(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { setIncludingFrame(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
@Override public ReferenceFrame getReferenceFrame() { angularPart.checkReferenceFrameMatch(linearPart); return angularPart.getReferenceFrame(); }
/** * Redefines this line with a new point and a new direction vector. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code lineDirection} are not * expressed in the same reference frame. */ default void set(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { set(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line with a new point and a new direction vector. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code lineDirection} are not * expressed in the same reference frame. */ default void set(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { set(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
@Override public ReferenceFrame getReferenceFrame() { angularPart.checkReferenceFrameMatch(linearPart); return angularPart.getReferenceFrame(); }
@Override public ReferenceFrame getReferenceFrame() { angularPart.checkReferenceFrameMatch(linearPart); return angularPart.getReferenceFrame(); }
/** * Sets this vector given an angular part and linear part. * * @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 arguments is not expressed in the same * reference frame as {@code this}. */ default void set(FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart) { angularPart.checkReferenceFrameMatch(linearPart); set(angularPart.getReferenceFrame(), angularPart, linearPart); }
/** * Sets this wrench given an angular part and linear part. * * @param bodyFrame the body frame associated with the given spatial force. * @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, FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart) { angularPart.checkReferenceFrameMatch(linearPart); set(bodyFrame, angularPart.getReferenceFrame(), angularPart, linearPart); }
/** * 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); }
/** * Sets this vector given an angular part and linear part and updates the frame of this vector. * * @param angularPart the vector holding the new values for the angular part and expressed in the * new "expressed-in-frame" to use for this spatial vector. Not modified. * @param linearPart the vector holding the new values for the linear part and expressed in the * new "expressed-in-frame" to use for this spatial vector. Not modified. * @throws ReferenceFrameMismatchException if {@code angularPart} and {@code linearPart} are not * expressed in the same reference frame. */ default void setIncludingFrame(FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart) { angularPart.checkReferenceFrameMatch(linearPart); setIncludingFrame(angularPart.getReferenceFrame(), angularPart, linearPart); }
/** * Sets this frame orientation to the same orientation described by the given rotation vector * {@code rotationVector} and sets the frame to {@code rotationVector.getReferenceFrame()}. * <p> * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the * same axis-angle. * </p> * * @param rotationVector vector the rotation vector used to set this orientation. Not modified. */ default void setRotationVectorIncludingFrame(FrameVector3DReadOnly rotationVector) { setReferenceFrame(rotationVector.getReferenceFrame()); setRotationVector((Vector3DReadOnly) rotationVector); }
/** * Sets this motion vector given an angular part and linear part 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 angularPart the vector holding the new values for the angular part, it is expressed in * the new "expressed-in-frame" to use for this spatial motion vector. Not modified. * @param linearPart the vector holding the new values for the linear part, it is expressed in * the new "expressed-in-frame" to use for this spatial motion vector. Not modified. * @throws ReferenceFrameMismatchException if the given {@code angularPart} and * {@code linearPart} are not expressed in the same reference frame. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart) { angularPart.checkReferenceFrameMatch(linearPart); setIncludingFrame(bodyFrame, baseFrame, angularPart.getReferenceFrame(), angularPart, linearPart); }
/** * Subtracts the given angular and linear parts to {@code this} providing the expected * {@code bodyFrame} for additional safety. * <p> * {@code this.angularPart -= angular}<br> * {@code this.linearPart -= linear} * </p> * * @param bodyFrame the body frame associated with the given spatial force. * @param angular the vector to subtract to this vector's angular part. Not modified. * @param linear the vector to subtract to this vector's linear part. Not modified. * @throws ReferenceFrameMismatchException if {@code bodyFrame != this.getBodyFrame()} or if * either {@code angular} or {@code linear} are not expressed in the same frame as * this wrench. */ default void sub(ReferenceFrame bodyFrame, FrameVector3DReadOnly angular, FrameVector3DReadOnly linear) { checkReferenceFrameMatch(bodyFrame, angular.getReferenceFrame()); checkReferenceFrameMatch(linear); sub((Vector3DReadOnly) angular, (Vector3DReadOnly) linear); } }
private void computeFinalConstraintError(FrameQuaternionReadOnly finalOrientation, FrameVector3DReadOnly finalAngularVelocity, double finalTime) { computeFinalConstraintError(finalOrientation, finalTime); trajectoryFrame.checkReferenceFrameMatch(finalAngularVelocity.getReferenceFrame()); trajectory.getAngularVelocity(tempAngularVelocity); tempAngularVelocity.changeFrame(trajectoryFrame); finalConstraintAngularVelocityError.set(finalAngularVelocity); finalConstraintAngularVelocityError.sub(tempAngularVelocity); }
private void computeInitialConstraintError(FrameQuaternionReadOnly initialOrientation, FrameVector3DReadOnly initialAngularVelocity, double initialTime) { computeInitialConstraintError(initialOrientation, initialTime); trajectoryFrame.checkReferenceFrameMatch(initialAngularVelocity.getReferenceFrame()); trajectory.getAngularVelocity(tempAngularVelocity); tempAngularVelocity.changeFrame(trajectoryFrame); initialConstraintAngularVelocityError.set(initialAngularVelocity); initialConstraintAngularVelocityError.sub(tempAngularVelocity); }
private void computeInitialConstraintError(FramePoint3DReadOnly initialPosition, FrameVector3DReadOnly initialVelocity, double initialTime) { computeInitialConstraintError(initialPosition, initialTime); trajectoryFrame.checkReferenceFrameMatch(initialVelocity.getReferenceFrame()); trajectory.getVelocity(tempVelocity); tempVelocity.changeFrame(trajectoryFrame); initialConstraintVelocityError.set(initialVelocity); initialConstraintVelocityError.sub(tempVelocity); }
private void computeFinalConstraintError(FramePoint3DReadOnly finalPosition, FrameVector3DReadOnly finalVelocity, double finalTime) { computeFinalConstraintError(finalPosition, finalTime); trajectoryFrame.checkReferenceFrameMatch(finalVelocity.getReferenceFrame()); trajectory.getVelocity(tempVelocity); tempVelocity.changeFrame(trajectoryFrame); finalConstraintVelocityError.set(finalVelocity); finalConstraintVelocityError.sub(tempVelocity); }