/** * 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 Vertex2DSupplier asVertex2DSupplier(Point2DReadOnly... vertices) { return asVertex2DSupplier(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 Vertex2DSupplier asVertex2DSupplier(Point2DReadOnly[] vertices, int numberOfVertices) { return asVertex2DSupplier(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 Vertex2DSupplier asVertex2DSupplier(List<? extends Point2DReadOnly> vertices) { return asVertex2DSupplier(vertices, vertices.size()); }
private T createSomeValidPolygon() { double[][] polygonPoints = new double[][] {{-0.05107802536335158, 0.04155594197133163}, {-0.05052044462374434, 0.1431544119584275}, {0.12219695435431863, 0.14220652470109518}, {0.12219695435431865, -0.041946248489056696}, {0.12163937361471142, -0.1435447184761526}, {-0.05107802536335154, -0.14259683121882027}}; T polygon = createConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(polygonPoints)); return polygon; }
public static FrameConvexPolygon2D constructPolygon(ReferenceFrame referenceFrame, double[][] points) { FrameConvexPolygon2D ret = new FrameConvexPolygon2D(referenceFrame, Vertex2DSupplier.asVertex2DSupplier(points)); verifyPointsAreClockwise(ret); return ret; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testBadCase() { ArrayList<Point2D> points = new ArrayList<Point2D>(); points.add(new Point2D(47.213206532573494, 33.6934385384089)); points.add(new Point2D(47.184403978806806, 33.6901486028859)); points.add(new Point2D(47.18629896969355, 33.945259199418715)); points.add(new Point2D(47.1996820005182, 37.28788526977694)); points.add(new Point2D(47.20659834223872, 36.93933424308558)); new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points)); }
private static final void moveCentroidToOrigin(List<Point2D> points) { ConvexPolygon2D footPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points)); Point2DReadOnly centroid = footPolygon.getCentroid(); for (Point2D point : points) { point.sub(centroid); } }
public static SteppingStone generateRandomCicularStone(String name, Random random, double xCenter, double yCenter, double baseZ, double height, double radius, ConvexPolygon2D shrunkenPolygon) { ArrayList<Point2D> points = generateRandomCircularPoints(xCenter, yCenter, radius, 40); ConvexPolygon2D polygon2d = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points)); SteppingStone steppingStone = new SteppingStone(name, baseZ, height, polygon2d, shrunkenPolygon); return steppingStone; }
@Override public double computeYawOffset(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx) { supportPolygon.set(Vertex2DSupplier.asVertex2DSupplier(contactPoints)); Point2D contactPoint = contactPoints.get(contactIdx); int vertexIndex = supportPolygon.getClosestVertexIndex(contactPoint); double angleOfEdge = getAngleOfEdgeAfterPoint(vertexIndex); double interiorAngle = getInteriorAngle(vertexIndex); return angleOfEdge - 0.5 * interiorAngle; }
private void setupTwoIntersectingBoxesMadeFromPolygons(CombinedTerrainObject3D combinedTerrainObject) { Vector3D normalVector = new Vector3D(0.0, 0.0, 1.0); double[][] firstVertices = {{0.0,0.0},{1.0,0.0},{1.0,1.0},{0.0,1.0}}; ConvexPolygon2D firstConvexPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(firstVertices)); RotatableConvexPolygonTerrainObject firstBox = new RotatableConvexPolygonTerrainObject(normalVector, firstConvexPolygon, 1.0); combinedTerrainObject.addTerrainObject(firstBox); double[][] secondVertices = {{-0.5,-0.5},{0.5,-0.5},{-0.5,0.5},{0.5,0.5}}; ConvexPolygon2D secondConvexPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(secondVertices)); RotatableConvexPolygonTerrainObject secondBox = new RotatableConvexPolygonTerrainObject(normalVector, secondConvexPolygon, 0.5); combinedTerrainObject.addTerrainObject(secondBox); }
private static SideDependentList<ConvexPolygon2D> createFootPolygonsFromContactPoints(RobotContactPointParameters<RobotSide> contactPointParameters) { SideDependentList<ConvexPolygon2D> footPolygons = new SideDependentList<>(); for (RobotSide side : RobotSide.values) { ArrayList<Point2D> footPoints = contactPointParameters.getFootContactPoints().get(side); ConvexPolygon2D scaledFoot = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(footPoints)); footPolygons.set(side, scaledFoot); } return footPolygons; }
public SteppingStone(String name, double baseZ, double height, ArrayList<Point2D> points, ConvexPolygon2D footPolygon) { this.name = name; this.baseZ = baseZ; this.height = height; convexPolygon2d = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points)); polygonToShrink = new ConvexPolygon2D(footPolygon); new ConvexPolygonTools().shrinkInto(polygonToShrink, new Point2D(0.0, 0.0), convexPolygon2d, shrunkenPolygon); }
private static SideDependentList<ConvexPolygon2D> createFootPolygonsFromContactPoints(RobotContactPointParameters<RobotSide> contactPointParameters) { SideDependentList<ConvexPolygon2D> footPolygons = new SideDependentList<>(); for (RobotSide side : RobotSide.values) { ArrayList<Point2D> footPoints = contactPointParameters.getFootContactPoints().get(side); ConvexPolygon2D scaledFoot = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(footPoints)); footPolygons.set(side, scaledFoot); } return footPolygons; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimplePointConvexPolygonShrinking() { ArrayList<Point2D> vertices = new ArrayList<Point2D>(); vertices.add(new Point2D(-1.0, 3.0)); ConvexPolygon2D polygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(vertices)); ConvexPolygonScaler shrinker = new ConvexPolygonScaler(); ConvexPolygon2D shrunkenPolygon = new ConvexPolygon2D(); shrinker.scaleConvexPolygon(polygon, 0.1, shrunkenPolygon); EuclidCoreTestTools.assertTuple2DEquals(new Point2D(-1.0, 3.0), shrunkenPolygon.getVertexCCW(0), 1e-7); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRegression5() { double[][] pointList = new double[][] {{-0.8598263783206956, 8.812003858355197}, {2.9111689851050997, -2.8275592421317626}, {-7.99259023564723, -6.327559047941249}, {-4.898907389671077, 3.467436032944315}}; FrameConvexPolygon2D supportPolygon = new FrameConvexPolygon2D(worldFrame, Vertex2DSupplier.asVertex2DSupplier(pointList)); FramePoint2D capturePoint = new FramePoint2D(worldFrame, -1.9885683919677177, -5.49168161377456); FramePoint2D desiredCMP = new FramePoint2D(worldFrame, 9.549202437801455, -5.225841656143075); FramePoint2D expectedCMP = new FramePoint2D(worldFrame, 2.911168985099841, -2.827559242128753); doTest(supportPolygon, capturePoint, desiredCMP, expectedCMP, null, null); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRegression7() { double[][] pointList = new double[][] {{-7.803067400320895, 5.2833847867746755}, {1.8456007994090697, 7.299950918772748}, {5.749971206125327, -0.4548297661618612}, {-5.840882755811931, -1.9631480755498387}}; FrameConvexPolygon2D supportPolygon = new FrameConvexPolygon2D(worldFrame, Vertex2DSupplier.asVertex2DSupplier(pointList)); FramePoint2D capturePoint = new FramePoint2D(worldFrame, -5.295144331776324, 5.807540241461124); FramePoint2D desiredCMP = new FramePoint2D(worldFrame, -7.791683042751529, 8.691901050356392); FramePoint2D expectedCMP = new FramePoint2D(worldFrame, -7.109596433690787, 5.428319821936232); doTest(supportPolygon, capturePoint, desiredCMP, expectedCMP, null, null); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRegression9() { double[][] pointList = new double[][] {{-8.135927687065115, -2.235069333986268}, {4.626495043779892, -9.594908447084016}}; FrameConvexPolygon2D supportPolygon = new FrameConvexPolygon2D(worldFrame, Vertex2DSupplier.asVertex2DSupplier(pointList)); FramePoint2D capturePoint = new FramePoint2D(worldFrame, 4.779252571336825, -2.7041636789385564); FramePoint2D desiredCMP = new FramePoint2D(worldFrame, -5.046299707229876, -4.103110230736704); FramePoint2D expectedCMP = new FramePoint2D(worldFrame, -4.926263656271011, -4.086019688157568); doTest(supportPolygon, capturePoint, desiredCMP, expectedCMP, null, null); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRegression4() { double[][] pointList = new double[][] {{-6.6364442778312505, 5.081037775538617}, {-5.809348495016624, 5.2896472244805715}, {2.795403317979172, 5.62762983725845}, {-0.47916782691148363, 3.346181735352964}, {-6.593198911501661, 0.0730878970354496}}; FrameConvexPolygon2D supportPolygon = new FrameConvexPolygon2D(worldFrame, Vertex2DSupplier.asVertex2DSupplier(pointList)); FramePoint2D capturePoint = new FramePoint2D(worldFrame, 6.203278757526469, -4.079058120870805); FramePoint2D desiredCMP = new FramePoint2D(worldFrame, 2.573986759898151, 5.590979195575493); FramePoint2D expectedCMP = new FramePoint2D(worldFrame, 2.573986759898151, 5.590979195575493); doTest(supportPolygon, capturePoint, desiredCMP, expectedCMP, null, null); }
@Test public void testGetCentroid() { double[][] verticesArray = {{0.0, 0.0}, {0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}}; T doubles = createConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(verticesArray)); Point2D centroid = new Point2D(); centroid.set(doubles.getCentroid()); assertEquals("Centroids should be equal", centroid, doubles.getCentroid()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRegression8() { double[][] pointList = new double[][] {{-9.515882456803075, -0.7165753982559391}, {-0.36081135780595197, -1.8177034363630717}, {3.1279637332797563, -2.4061970865397093}, {1.8287460206545418, -4.725313113918093}, {-8.38403587244779, -4.561486274023161}}; FrameConvexPolygon2D supportPolygon = new FrameConvexPolygon2D(worldFrame, Vertex2DSupplier.asVertex2DSupplier(pointList)); FramePoint2D capturePoint = new FramePoint2D(worldFrame, -1.617696297772003, -4.670027921361948); FramePoint2D desiredCMP = new FramePoint2D(worldFrame, -9.521814853350515, -4.6289424304336); FramePoint2D expectedCMP = new FramePoint2D(worldFrame, -8.456495592368146, -4.315338811064966); doTest(supportPolygon, capturePoint, desiredCMP, expectedCMP, null, null); }