How to use

getReferenceFrame

method

in

us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly

@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);
}