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