@Override public boolean geometricallyEquals(EuclideanWaypoint other, double epsilon) { if (!position.geometricallyEquals(other.position, epsilon)) return false; if (!linearVelocity.geometricallyEquals(other.linearVelocity, epsilon)) return false; return true; }
@Override public boolean geometricallyEquals(SO3Waypoint other, double epsilon) { if (!orientation.geometricallyEquals(other.orientation, epsilon)) return false; if (!angularVelocity.geometricallyEquals(other.angularVelocity, epsilon)) return false; return true; }
@Override @Test public void testGeometricallyEquals() throws Exception { super.testGeometricallyEquals(); Vector3D vectorA; Vector3D vectorB; Random random = new Random(621541L); for (int i = 0; i < 100; ++i) { vectorA = EuclidCoreRandomTools.nextVector3D(random); vectorB = EuclidCoreRandomTools.nextVector3D(random); if (((Vector3DReadOnly) vectorA).geometricallyEquals(vectorB, getEpsilon())) { assertTrue(vectorA.geometricallyEquals(vectorB, getEpsilon())); } else { assertFalse(vectorA.geometricallyEquals(vectorB, getEpsilon())); } } }
Box3D box2 = new Box3D(pose, length2, width2, height2); assertTrue("Iteration: " + i, box1.geometricallyEquals(box2, EPSILON) == size1.geometricallyEquals(size2, EPSILON)); double epsilon = random.nextDouble(); assertTrue("Iteration: " + i, box1.geometricallyEquals(box2, epsilon) == size1.geometricallyEquals(size2, epsilon)); size2.add(size1, EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 0.99 * epsilon)); Box3D box2 = new Box3D(pose, size2.getX(), size2.getY(), size2.getZ()); assertTrue("Iteration: " + i, box1.geometricallyEquals(box2, epsilon) == size1.geometricallyEquals(size2, epsilon)); assertTrue("Iteration: " + i, box1.geometricallyEquals(box2, epsilon) == size1.geometricallyEquals(size2, epsilon));
Ellipsoid3D ellipsoid2 = new Ellipsoid3D(pose, radiusX2, radiusY2, radiusZ2); assertTrue("Iteration: " + i, ellipsoid1.geometricallyEquals(ellipsoid2, EPSILON) == radii1.geometricallyEquals(radii2, EPSILON)); assertTrue("Iteration: " + i, ellipsoid1.geometricallyEquals(ellipsoid2, epsilon) == radii1.geometricallyEquals(radii2, epsilon)); radii2.add(radii1, EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 0.99 * epsilon)); Ellipsoid3D ellipsoid2 = new Ellipsoid3D(pose, radii2.getX(), radii2.getY(), radii2.getZ()); assertTrue("Iteration: " + i, ellipsoid1.geometricallyEquals(ellipsoid2, epsilon) == radii1.geometricallyEquals(radii2, epsilon)); assertTrue("Iteration: " + i, ellipsoid1.geometricallyEquals(ellipsoid2, epsilon) == radii1.geometricallyEquals(radii2, epsilon));