/** * 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 FrameVertex3DSupplier asFrameVertex3DSupplier(FramePoint3DReadOnly[] vertices, int startIndex, int numberOfVertices) { if (numberOfVertices == 0) return emptyFrameVertex3DSupplier(); if (startIndex + numberOfVertices > vertices.length) throw new IllegalArgumentException("The array is too small. Array length = " + vertices.length + ", expected minimum length = " + (startIndex + numberOfVertices)); return asFrameVertex3DSupplier(Arrays.asList(vertices), startIndex, 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(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; }
FrameVertex3DSupplier supplier = FrameVertex3DSupplier.asFrameVertex3DSupplier(original); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); for (int j = 0; j < numberOfVertices; j++) assertTrue(original.get(j) == supplier.getVertex(j)); FrameVertex3DSupplier supplier = FrameVertex3DSupplier.asFrameVertex3DSupplier(original, numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j)); FrameVertex3DSupplier supplier = FrameVertex3DSupplier.asFrameVertex3DSupplier(original, startIndex, numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j)); FrameVertex3DSupplier supplier = FrameVertex3DSupplier.asFrameVertex3DSupplier(original.toArray(new FramePoint3DReadOnly[0])); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); for (int j = 0; j < numberOfVertices; j++) assertTrue(original.get(j) == supplier.getVertex(j)); FrameVertex3DSupplier supplier = FrameVertex3DSupplier.asFrameVertex3DSupplier(original.toArray(new FramePoint3DReadOnly[0]), numberOfVertices); assertEquals(numberOfVertices, supplier.getNumberOfVertices()); assertTrue(subList.get(j) == supplier.getVertex(j));
@Test public void testCreatingEmptySupplier() { FrameVertex3DSupplier expected = FrameVertex3DSupplier.emptyFrameVertex3DSupplier(); FrameVertex3DSupplier actual; actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(new FramePoint3DReadOnly[0], 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(new FramePoint3DReadOnly[0], 0, 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(Collections.emptyList()); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(Collections.emptyList(), 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); actual = FrameVertex3DSupplier.asFrameVertex3DSupplier(Collections.emptyList(), 0, 0); assertEquals(expected, actual); assertTrue(expected.epsilonEquals(actual, EPSILON)); }
/** * 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 FrameVertex3DSupplier asFrameVertex3DSupplier(List<? extends FramePoint3DReadOnly> vertices, int numberOfVertices) { return asFrameVertex3DSupplier(vertices, 0, numberOfVertices); }
@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<FramePoint3D> listA = IntStream.range(0, sizeA).mapToObj(v -> EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame)) .collect(Collectors.toList()); List<FramePoint3D> listAPrime = listA.stream().map(FramePoint3D::new).collect(Collectors.toList()); List<FramePoint3D> listSizeA = IntStream.range(0, sizeA).mapToObj(v -> EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame)) .collect(Collectors.toList()); int sizeB = random.nextInt(100) + 1; List<FramePoint3D> listB = IntStream.range(0, sizeB).mapToObj(v -> EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame)) .collect(Collectors.toList()); assertTrue(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).equals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA))); assertTrue(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).equals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listAPrime))); assertFalse(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).equals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listSizeA))); assertFalse(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).equals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listB))); } }
FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).epsilonEquals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listA), epsilon)); assertTrue("Iteration: " + i, FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).epsilonEquals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listAPrime), epsilon)); assertFalse("Iteration: " + i, FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).epsilonEquals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listSizeA), epsilon)); assertFalse("Iteration: " + i, FrameVertex3DSupplier.asFrameVertex3DSupplier(listA).epsilonEquals(FrameVertex3DSupplier.asFrameVertex3DSupplier(listB), epsilon));
return emptyFrameVertex3DSupplier(); if (startIndex + numberOfVertices > vertices.size()) throw new IllegalArgumentException("The list is too small. List size = " + vertices.size() + ", expected minimum size = "
if (frameVertex3DSupplier.isEmpty()) if (frameVertex3DSupplier.getReferenceFrame() != null) clearAndUpdate(frameVertex3DSupplier.getReferenceFrame()); else clearAndUpdate(); setReferenceFrame(frameVertex3DSupplier.getReferenceFrame()); set(frameVertex3DSupplier);
/** * 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 FrameVertex3DSupplier asFrameVertex3DSupplier(FramePoint3DReadOnly... vertices) { return asFrameVertex3DSupplier(Arrays.asList(vertices)); }
/** * 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(FrameVertex3DSupplier 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 list. * * @param vertices the list by which the supplier will be backed. * @return the supplier. */ public static FrameVertex3DSupplier asFrameVertex3DSupplier(List<? extends FramePoint3DReadOnly> vertices) { return asFrameVertex3DSupplier(vertices, vertices.size()); }
/** * 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> * * @param frameVertex3DSupplier the supplier of vertices. * @see FrameVertex3DSupplier * @see #addVertexMatchingFrame(FramePoint3DReadOnly) */ default void addVerticesMatchingFrame(FrameVertex3DSupplier frameVertex3DSupplier) { for (int index = 0; index < frameVertex3DSupplier.getNumberOfVertices(); index++) { addVertexMatchingFrame(frameVertex3DSupplier.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 FrameVertex3DSupplier asFrameVertex3DSupplier(FramePoint3DReadOnly[] vertices, int numberOfVertices) { return asFrameVertex3DSupplier(Arrays.asList(vertices), numberOfVertices); }
/** * 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 frameVertex3DSupplier the supplier of vertices. * @throws ReferenceFrameMismatchException if any of the given {@code vertices} is a * {@code FramePoint3DReadOnly} and is not expressed in the same reference frame as * {@code this}. * @see FrameVertex3DSupplier * @see #addVertex(FramePoint3DReadOnly) */ default void addVertices(FrameVertex3DSupplier frameVertex3DSupplier) { for (int index = 0; index < frameVertex3DSupplier.getNumberOfVertices(); index++) { addVertex(frameVertex3DSupplier.getVertex(index)); } }
FramePoint3D[] framePoints = new FramePoint3D[]{p0, p1, p2, p3}; yoFrameConvexPolygon2d.set(FrameVertex3DSupplier.asFrameVertex3DSupplier(framePoints));