How to use

epsilonEquals

method

in

us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly

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

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

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

assertTrue(expectedPosition.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getPosition(), epsilon));
assertTrue(expectedOrientation.geometricallyEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getSO3Waypoint().getOrientation(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getLinearVelocity(), epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getSO3Waypoint().getAngularVelocity(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));

assertEquals(expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getNameSuffix());
assertTrue(expectedOrientation.geometricallyEquals(testedYoFrameSO3TrajectoryPoint.getOrientation(), epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(testedYoFrameSO3TrajectoryPoint.getAngularVelocity(), epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));

assertEquals(expectedNameSuffix, testedYoFrameEuclideanTrajectoryPoint.getNameSuffix());
assertTrue(expectedPosition.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getPosition(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));

assertEquals(expectedOrientation.getReferenceFrame(), testedYoFrameSE3TrajectoryPoint.getOrientation().getReferenceFrame());
EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedOrientation, trajectoryPointQuaternion, epsilon);
assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameSE3TrajectoryPoint.getLinearVelocity(), epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(testedYoFrameSE3TrajectoryPoint.getAngularVelocity(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));

if (!EuclidCoreTools.epsilonEquals(getMass(), other.getMass(), epsilon))
  return false;
if (!getCenterOfMassOffset().epsilonEquals(other.getCenterOfMassOffset(), epsilon))
  return false;
if (!getMomentOfInertia().epsilonEquals(other.getMomentOfInertia(), epsilon))