/** * Tests on a per-vertex basis if this supplier and {@code other} are equal to an * {@code epsilon}. * * @param other the other supplier to compare against this. * @param epsilon the tolerance to use. * @return {@code true} if the two suppliers are equal. */ default boolean epsilonEquals(FrameVertex3DSupplier other, double epsilon) { if (getNumberOfVertices() != other.getNumberOfVertices()) return false; for (int i = 0; i < getNumberOfVertices(); i++) { if (!getVertex(i).epsilonEquals(other.getVertex(i), 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)); }
assertEquals(expectedNamePrefix, testedYoFrameEuclideanTrajectoryPoint.getNamePrefix()); assertEquals(expectedNameSuffix, testedYoFrameEuclideanTrajectoryPoint.getNameSuffix()); assertTrue(expectedPosition.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getPosition(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(), epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, 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)); testedFrameSE3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); testedFrameSE3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); testedFrameSE3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
assertEquals(expectedNamePrefix, testedYoFrameSE3TrajectoryPoint.getNamePrefix()); assertEquals(expectedNameSuffix, testedYoFrameSE3TrajectoryPoint.getNameSuffix()); assertTrue(expectedPosition.epsilonEquals(testedYoFrameSE3TrajectoryPoint.getPosition(), epsilon)); Quaternion trajectoryPointQuaternion = new Quaternion(testedYoFrameSE3TrajectoryPoint.getOrientation()); assertEquals(expectedOrientation.getReferenceFrame(), testedYoFrameSE3TrajectoryPoint.getOrientation().getReferenceFrame()); testedYoFrameSE3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); testedYoFrameSE3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); testedYoFrameSE3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));