/** * 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); }
/** * 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)); }
/** * 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); }
/** * 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); } }
/** * 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); }
@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)); } } }
@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)); }
original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original); original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original, numberOfVertices); original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original, startIndex, numberOfVertices); original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original.toArray(new Point3DReadOnly[0])); original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original.toArray(new Point3DReadOnly[0]), numberOfVertices); original.add(EuclidCoreRandomTools.nextPoint3D(random)); Vertex3DSupplier supplier = Vertex3DSupplier.asVertex3DSupplier(original.toArray(new Point3DReadOnly[0]), startIndex, numberOfVertices);
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()); }