/** * Returns a fixed-size supplier backed by a portion of the given list starting with the first * element and specified by its length {@code numberOfVertices}. * * @param vertices the list by which the supplier will be backed. * @param numberOfVertices the portion's length. * @return the supplier. */ public static Vertex3DSupplier asVertex3DSupplier(List<? extends Point3DReadOnly> vertices, int numberOfVertices) { return asVertex3DSupplier(vertices, 0, numberOfVertices); }
/** * 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(Vertex3DSupplier 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; }
/** * Returns a fixed-size supplier backed by a portion of the given array specified by the first * index {@code startIndex} and the portion length {@code numberOfVertices}. * * @param vertices the array by which the supplier will be backed. * @param startIndex the first vertex index. * @param numberOfVertices the portion's length. * @return the supplier. */ public static Vertex3DSupplier asVertex3DSupplier(Point3DReadOnly[] vertices, int startIndex, int numberOfVertices) { if (numberOfVertices == 0) return emptyVertex3DSupplier(); if (startIndex + numberOfVertices > vertices.length) throw new IllegalArgumentException("The array is too small. Array length = " + vertices.length + ", expected minimum length = " + (startIndex + numberOfVertices)); return asVertex3DSupplier(Arrays.asList(vertices), startIndex, numberOfVertices); }
Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); for (int j = 0; j < numberOfVertices; j++) assertTrue(original.get(j) == supplier.getVertex(j)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original, numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original, startIndex, numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original.toArray(new Point3DReadOnly[0])); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); for (int j = 0; j < numberOfVertices; j++) assertTrue(original.get(j) == supplier.getVertex(j)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original.toArray(new Point3DReadOnly[0]), numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j));
@Test public void testCreatingEmptySupplier() { Vertex3DSupplier expected = Vertex3DSupplier.emptyVertex3DSupplier(); Vertex3DSupplier actual; actual = Vertex3DSupplier.asVertex3DSupplier(); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = Vertex3DSupplier.asVertex3DSupplier(new Point3DReadOnly[0], 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = Vertex3DSupplier.asVertex3DSupplier(new Point3DReadOnly[0], 0, 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = Vertex3DSupplier.asVertex3DSupplier(Collections.emptyList()); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = Vertex3DSupplier.asVertex3DSupplier(Collections.emptyList(), 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = Vertex3DSupplier.asVertex3DSupplier(Collections.emptyList(), 0, 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); }
@Test public void testEquals() throws Exception { Random random = new Random(9017); for (int i = 0; i < ITERATIONS; i++) { int sizeA = random.nextInt(100) + 1; List<Point3D> listA = IntStream.range(0, sizeA).mapToObj(v -> EuclidCoreRandomTools.nextPoint3D(random)).collect(Collectors.toList()); List<Point3D> listAPrime = listA.stream().map(Point3D::new).collect(Collectors.toList()); List<Point3D> listSizeA = IntStream.range(0, sizeA).mapToObj(v -> EuclidCoreRandomTools.nextPoint3D(random)).collect(Collectors.toList()); int sizeB = random.nextInt(100) + 1; List<Point3D> listB = IntStream.range(0, sizeB).mapToObj(v -> EuclidCoreRandomTools.nextPoint3D(random)).collect(Collectors.toList()); assertTrue(Vertex3DSupplier.asVertex3DSupplier(listA).equals(Vertex3DSupplier.asVertex3DSupplier(listA))); assertTrue(Vertex3DSupplier.asVertex3DSupplier(listA).equals(Vertex3DSupplier.asVertex3DSupplier(listAPrime))); assertFalse(Vertex3DSupplier.asVertex3DSupplier(listA).equals(Vertex3DSupplier.asVertex3DSupplier(listSizeA))); assertFalse(Vertex3DSupplier.asVertex3DSupplier(listA).equals(Vertex3DSupplier.asVertex3DSupplier(listB))); } }
@Test public void testEpsilonquals() throws Exception { Random random = new Random(9017); for (int i = 0; i < ITERATIONS; i++) { double epsilon = random.nextDouble(); int sizeA = random.nextInt(100) + 1; List<Point3D> listA = IntStream.range(0, sizeA).mapToObj(v -> EuclidCoreRandomTools.nextPoint3D(random)).collect(Collectors.toList()); List<Point3D> listAPrime = listA.stream().map(Point3D::new).collect(Collectors.toList()); listAPrime.forEach(p -> p.add(nextDouble(random, epsilon), nextDouble(random, epsilon), nextDouble(random, epsilon))); List<Point3D> listSizeA = listA.stream().map(Point3D::new).collect(Collectors.toList()); listSizeA.forEach(p -> p.add((random.nextBoolean() ? -1.0 : 1.0) * nextDouble(random, epsilon, epsilon + 1.0), (random.nextBoolean() ? -1.0 : 1.0) * nextDouble(random, epsilon, epsilon + 1.0), (random.nextBoolean() ? -1.0 : 1.0) * nextDouble(random, epsilon, epsilon + 1.0))); int sizeB = random.nextInt(100) + 1; List<Point3D> listB = IntStream.range(0, sizeB).mapToObj(v -> EuclidCoreRandomTools.nextPoint3D(random)).collect(Collectors.toList()); assertTrue("Iteration: " + i, Vertex3DSupplier.asVertex3DSupplier(listA).epsilonEquals(Vertex3DSupplier.asVertex3DSupplier(listA), epsilon)); assertTrue("Iteration: " + i, Vertex3DSupplier.asVertex3DSupplier(listA).epsilonEquals(Vertex3DSupplier.asVertex3DSupplier(listAPrime), epsilon)); assertFalse("Iteration: " + i, Vertex3DSupplier.asVertex3DSupplier(listA).epsilonEquals(Vertex3DSupplier.asVertex3DSupplier(listSizeA), epsilon)); assertFalse("Iteration: " + i, Vertex3DSupplier.asVertex3DSupplier(listA).epsilonEquals(Vertex3DSupplier.asVertex3DSupplier(listB), epsilon)); } } }
@Override public int getNumberOfVertices() { return vertex2dSupplier.getNumberOfVertices(); }
@Override public FramePoint3DReadOnly getVertex(int index) { return new FramePoint3D(referenceFrame, vertex2dSupplier.getVertex(index)); } };
return emptyVertex3DSupplier(); if (startIndex + numberOfVertices > vertices.size()) throw new IllegalArgumentException("The list is too small. List size = " + vertices.size() + ", expected minimum size = "
/** * Tests whether this supplier is empty or not. * * @return {@code true} if this supplier has no vertices, {@code false} if it has at one vertex. */ default boolean isEmpty() { return getNumberOfVertices() == 0; }
/** * Tests on a per-vertex basis if this supplier and {@code other} are equal. * * @param other the other supplier to compare against this. * @return {@code true} if the two suppliers are equal. */ default boolean equals(Vertex3DSupplier other) { if (other == null) return false; if (getNumberOfVertices() != other.getNumberOfVertices()) return false; for (int i = 0; i < getNumberOfVertices(); i++) { if (!getVertex(i).equals(other.getVertex(i))) return false; } return true; }
/** * Returns a fixed-size supplier backed by the given array. * * @param vertices the array by which the supplier will be backed. * @return the supplier. */ public static Vertex3DSupplier asVertex3DSupplier(Point3DReadOnly... vertices) { return asVertex3DSupplier(Arrays.asList(vertices)); }
/** * Adds the vertices from the given vertex supplier. * <p> * Note that this polygon is marked as being out-of-date. The method {@link #update()} has to be * called afterward before being able to perform operations with this polygon. * </p> * <p> * Only the x and y coordinates of each vertex is used to add a vertex to this polygon. * </p> * * @param vertex3DSupplier the supplier of vertices. * @see Vertex3DSupplier */ default void addVertices(Vertex3DSupplier vertex3DSupplier) { for (int index = 0; index < vertex3DSupplier.getNumberOfVertices(); index++) { addVertex(vertex3DSupplier.getVertex(index)); } }
/** * Returns a fixed-size supplier backed by a portion of the given array starting with the first * element and specified by its length {@code numberOfVertices}. * * @param vertices the array by which the supplier will be backed. * @param numberOfVertices the portion's length. * @return the supplier. */ public static Vertex3DSupplier asVertex3DSupplier(Point3DReadOnly[] vertices, int numberOfVertices) { return asVertex3DSupplier(Arrays.asList(vertices), numberOfVertices); }
return "null"; if (vertex3DSupplier.getNumberOfVertices() == 0) return "Vertex 3D supplier: vertices = []"; for (int i = 0; i < vertex3DSupplier.getNumberOfVertices() - 1; i++) string += getTuple3DString(format, vertex3DSupplier.getVertex(i)) + ",\n"; string += getTuple3DString(format, vertex3DSupplier.getVertex(vertex3DSupplier.getNumberOfVertices() - 1)) + " ]"; return string;
/** * Returns a fixed-size supplier backed by the given list. * * @param vertices the list by which the supplier will be backed. * @return the supplier. */ public static Vertex3DSupplier asVertex3DSupplier(List<? extends Point3DReadOnly> vertices) { return asVertex3DSupplier(vertices, vertices.size()); }
/** * Generates a fixed-size supplier of random vertex 3D. * * @param random the random generator to use. * @param numberOfVertices the supplier's size. * @return the random supplier. */ public static Vertex3DSupplier nextVertex3DSupplier(Random random, int numberOfVertices) { List<Point3D> vertices = IntStream.range(0, numberOfVertices).mapToObj(i -> nextPoint3D(random)).collect(Collectors.toList()); return Vertex3DSupplier.asVertex3DSupplier(vertices); } }
public static FrameConvexPolygon2D unpackFootSupportPolygon(CapturabilityBasedStatus capturabilityBasedStatus, RobotSide robotSide) { if (robotSide == RobotSide.LEFT && capturabilityBasedStatus.getLeftFootSupportPolygon2d().size() > 0) return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), Vertex3DSupplier.asVertex3DSupplier(capturabilityBasedStatus.getLeftFootSupportPolygon2d())); else if (capturabilityBasedStatus.getRightFootSupportPolygon2d() != null) return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), Vertex3DSupplier.asVertex3DSupplier(capturabilityBasedStatus.getRightFootSupportPolygon2d())); else return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame()); }