How to use

FrameVector3DReadOnly

in

us.ihmc.euclid.referenceFrame.interfaces

@Override
public ReferenceFrame getReferenceFrame()
{
 angularPart.checkReferenceFrameMatch(linearPart);
 return angularPart.getReferenceFrame();
}

/**
* 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();
}

public void update(FrameVector3DReadOnly inputAngularVelocity, FrameVector3DReadOnly inputLinearAcceleration, FrameVector3DReadOnly inputMagneticVector)
{
 inputAngularVelocity.checkReferenceFrameMatch(sensorFrame);
 inputLinearAcceleration.checkReferenceFrameMatch(sensorFrame);
 if (inputMagneticVector != null)
   inputMagneticVector.checkReferenceFrameMatch(sensorFrame);
 update((Vector3DReadOnly) inputAngularVelocity, (Vector3DReadOnly) inputLinearAcceleration, (Vector3DReadOnly) inputMagneticVector);
}

@Override
public ReferenceFrame getReferenceFrame()
{
 return referenceVector.getReferenceFrame();
}

/**
* Tests on a per component basis if this vector is equal to the given {@code other} to an
* {@code epsilon} and both vectors are expressed in the same reference frame.
*
* @param other the other 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.
*/
default boolean epsilonEquals(SpatialVectorReadOnly other, double epsilon)
{
 if (getReferenceFrame() != other.getReferenceFrame())
   return false;
 if (!getAngularPart().epsilonEquals(other.getAngularPart(), epsilon))
   return false;
 if (!getLinearPart().epsilonEquals(other.getLinearPart(), epsilon))
   return false;
 return true;
}

/** {@inheritDoc} */
@Override
default void setJointAngularVelocity(Vector3DReadOnly jointAngularVelocity)
{
 setQd(getJointAxis().dot(jointAngularVelocity));
}

if (!getAngularPart().geometricallyEquals(other.getAngularPart(), epsilon))
  return false;
if (!getLinearPart().geometricallyEquals(other.getLinearPart(), epsilon))
  return false;
return true;

  /**
  * 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;
  }
}

angularPart.checkReferenceFrameMatch(linearPart);

/**
* 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);
}

static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FrameQuaternionReadOnly expectedOrientation,
                           FrameVector3DReadOnly expectedAngularVelocity, FrameSO3TrajectoryPoint testedFrameSO3TrajectoryPoint,
                           double epsilon)
{
 assertTrue(expectedFrame == testedFrameSO3TrajectoryPoint.getReferenceFrame());
 assertEquals(expectedTime, testedFrameSO3TrajectoryPoint.getTime(), epsilon);
 assertTrue(expectedOrientation.geometricallyEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getOrientation(), epsilon));
 assertTrue(expectedAngularVelocity.epsilonEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getAngularVelocity(), epsilon));
 Quaternion actualOrientation = new Quaternion();
 Vector3D actualAngularVelocity = new Vector3D();
 testedFrameSO3TrajectoryPoint.getOrientation(actualOrientation);
 testedFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity);
 assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon));
 assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
 FrameQuaternion actualFrameOrientation = new FrameQuaternion();
 FrameVector3D actualFrameAngularVelocity = new FrameVector3D();
 testedFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation);
 testedFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity);
 assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon));
 assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
 actualFrameOrientation = new FrameQuaternion(expectedFrame);
 actualFrameAngularVelocity = new FrameVector3D(expectedFrame);
 testedFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation);
 testedFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity);
 assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon));
 assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
}

/** {@inheritDoc} */
@Override
default void setJointAngularAcceleration(Vector3DReadOnly jointAngularAcceleration)
{
 setQdd(getJointAxis().dot(jointAngularAcceleration));
}

if (!getAngularPart().geometricallyEquals(getAngularPart(), epsilon))
  return false;
if (!getLinearPart().geometricallyEquals(getLinearPart(), epsilon))
  return false;
return true;

  /**
  * Tests on a per component basis, if this spatial inertia matrix is exactly equal to
  * {@code other} and have the same reference frames.
  *
  * @param other the other spatial inertia to compare against this. Not modified.
  * @return {@code true} if the two spatial inertia matrices are exactly equal component-wise,
  *         {@code false} otherwise.
  */
  default boolean equals(SpatialInertiaReadOnly other)
  {
   if (other == null)
     return false;
   if (getBodyFrame() != other.getBodyFrame())
     return false;
   if (getReferenceFrame() != other.getReferenceFrame())
     return false;
   if (!getMomentOfInertia().equals(other.getMomentOfInertia()))
     return false;
   if (getMass() != other.getMass())
     return false;
   if (!getCenterOfMassOffset().equals(other.getCenterOfMassOffset()))
     return false;
   return true;
  }
}

@Override
public ReferenceFrame getReferenceFrame()
{
 angularPart.checkReferenceFrameMatch(linearPart);
 return angularPart.getReferenceFrame();
}

public void setCubicUsingFinalAccelerationButNotFinalPosition(double t0, double tFinal, FramePoint3DReadOnly z0, FrameVector3DReadOnly zd0,
                               FrameVector3DReadOnly zdFinal, FrameVector3DReadOnly zddFinal)
{
 z0.checkReferenceFrameMatch(referenceFrame);
 zd0.checkReferenceFrameMatch(referenceFrame);
 zdFinal.checkReferenceFrameMatch(referenceFrame);
 zddFinal.checkReferenceFrameMatch(referenceFrame);
 super.setCubicUsingFinalAccelerationButNotFinalPosition(t0, tFinal, z0, zd0, zdFinal, zddFinal);
}

/**
* 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);
}

static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FramePoint3DReadOnly expectedPosition,
   FrameVector3DReadOnly expectedLinearVelocity, FrameEuclideanTrajectoryPoint testedFrameEuclideanTrajectoryPoint, double epsilon)
{
 assertTrue(expectedFrame == testedFrameEuclideanTrajectoryPoint.getReferenceFrame());
 assertEquals(expectedTime, testedFrameEuclideanTrajectoryPoint.getTime(), epsilon);
 assertTrue(expectedPosition.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getPosition(), epsilon));
 assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getLinearVelocity(), epsilon));
 Point3D actualPosition = new Point3D();
 Vector3D actualLinearVelocity = new Vector3D();
 testedFrameEuclideanTrajectoryPoint.getPosition(actualPosition);
 testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity);
 assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon));
 assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon));
 FramePoint3D actualFramePosition = new FramePoint3D();
 FrameVector3D actualFrameLinearVelocity = new FrameVector3D();
 testedFrameEuclideanTrajectoryPoint.getPositionIncludingFrame(actualFramePosition);
 testedFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity);
 assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon));
 assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
 actualFramePosition = new FramePoint3D(expectedFrame);
 actualFrameLinearVelocity = new FrameVector3D(expectedFrame);
 testedFrameEuclideanTrajectoryPoint.getPosition(actualFramePosition);
 testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity);
 assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon));
 assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
}

/** {@inheritDoc} */
@Override
default void setJointLinearAcceleration(Vector3DReadOnly jointLinearAcceleration)
{
 setQdd(getJointAxis().dot(jointLinearAcceleration));
}

/**
* Tests if {@code this} and {@code other} represent the same wrench to an {@code epsilon}.
* <p>
* It is likely that the implementation of this method will change in the future as the
* definition of "geometrically-equal" for wrenches might evolve. In the meantime, the current
* assumption is that two wrenches are geometrically equal if both their 3D torque and 3D force
* are independently geometrically equal, see
* {@link Vector3DReadOnly#geometricallyEquals(Vector3DReadOnly, double)}.
* </p>
* <p>
* Note that {@code this.geometricallyEquals(other, epsilon) == true} does not necessarily imply
* {@code this.epsilonEquals(other, epsilon)} and vice versa.
* </p>
*
* @param other the other wrench to compare against this. Not modified.
* @param epsilon the tolerance to use for the comparison.
* @return {@code true} if the two wrenches represent the same physical quantity, {@code false}
*         otherwise.
* @throws ReferenceFrameMismatchException if the reference frames of {@code other} do not
*            respectively match the reference frames of {@code this}.
*/
default boolean geometricallyEquals(WrenchReadOnly other, double epsilon)
{
 checkReferenceFrameMatch(other);
 if (!getAngularPart().geometricallyEquals(getAngularPart(), epsilon))
   return false;
 if (!getLinearPart().geometricallyEquals(getLinearPart(), epsilon))
   return false;
 return true;
}