polygon.checkNonEmpty(); polygon.clear(); polygon.update(); return; int vertices = polygon.getNumberOfVertices(); while (vertices > desiredVertices) Point2DReadOnly lastVertex = polygon.getVertex(0); for (int i = 1; i < vertices + 1; i++) nextVertex = polygon.getVertex(0); nextVertex = polygon.getVertex(i); Point2DReadOnly vertexA = polygon.getVertex(idx1); Point2DReadOnly vertexB = polygon.getVertex(idx2); double xNew = (vertexA.getX() + vertexB.getX()) / 2.0; double yNew = (vertexA.getY() + vertexB.getY()) / 2.0; polygon.removeVertex(idx1); polygon.removeVertex(idx2); polygon.addVertex(xNew, yNew); polygon.update(); vertices = polygon.getNumberOfVertices();
/** * Scale this convex polygon about its centroid. * <p> * The polygon centroid remains unchanged. * </p> * * @param scaleFactor the scale factor to apply to this polygon. A value of {@code 1.0} does * nothing. * @throws OutdatedPolygonException if {@link #update()} has not been called since last time this * polygon's vertices were edited. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default void scale(double scaleFactor) { scale(getCentroid(), scaleFactor); }
@Test public void testSetAndUpdates() { T doubleInt = createEmptyConvexPolygon2D(); int numberOfVertices = 4; double[][] verticesArray = {{0.0, 0.0}, {0.0, 1.0}, {1.0, 0.0}, {1.0, 1.0}}; doubleInt.set(Vertex2DSupplier.asVertex2DSupplier(verticesArray, numberOfVertices)); assertEquals("Number of vertices should be 4", 4.0, doubleInt.getNumberOfVertices(), EPSILON); assertTrue(doubleInt.isUpToDate()); }
/** * Sets this polygon such that it represents the smallest convex hull that contains all the vertices * supplied by the two suppliers. * <p> * Note that the resulting polygon is ready to be used for any operations, no need to call * {@link #update()}. * </p> * * @param firstVertex2DSupplier the first supplier of vertices. * @param secondVertex2DSupplier the second supplier of vertices. */ default void set(Vertex2DSupplier firstVertex2DSupplier, Vertex2DSupplier secondVertex2DSupplier) { clear(); addVertices(firstVertex2DSupplier); addVertices(secondVertex2DSupplier); update(); }
Point2DBasics maxPoint = polygon.getBoundingBox().getMaxPoint(); Point2DBasics minPoint = polygon.getBoundingBox().getMinPoint(); polygon.addVertex(new Point2D()); polygon.update(); EuclidCoreTestTools.assertTuple2DIsSetToZero(maxPoint); EuclidCoreTestTools.assertTuple2DIsSetToZero(minPoint); polygon.addVertex(new Point2D(1.0, 1.0)); polygon.update(); EuclidCoreTestTools.assertTuple2DIsSetToZero(minPoint); EuclidCoreTestTools.assertTuple2DEquals(new Point2D(1.0, 1.0), maxPoint, EPSILON); polygon.translate(1.0, 1.0); EuclidCoreTestTools.assertTuple2DEquals(new Point2D(1.0, 1.0), minPoint, EPSILON); EuclidCoreTestTools.assertTuple2DEquals(new Point2D(2.0, 2.0), maxPoint, EPSILON); polygon.set(Vertex2DSupplier.asVertex2DSupplier(points)); for (Point2D point : points) assertTrue(polygon.getBoundingBox().isInsideInclusive(point)); polygon.translate(10.0, 10.0); for (Point2D point : points) assertFalse(polygon.getBoundingBox().isInsideInclusive(point)); polygon.clearAndUpdate();
public int cutPolygonWithLine(Line2DReadOnly cuttingLine, ConvexPolygon2DBasics polygonToCut, RobotSide sideOfLineToCut) int intersectionPoints = polygonToCut.intersectionWith(cuttingLine, intersectionPoint1, intersectionPoint2); while (index < polygonToCut.getNumberOfVertices()) Point2DReadOnly vertex = polygonToCut.getVertex(index); if (cuttingLine.isPointOnSideOfLine(vertex, sideOfLineToCut == RobotSide.LEFT)) polygonToCut.removeVertex(index); polygonToCut.update(); numberOfVerticesRemoved++; polygonToCut.addVertex(intersectionPoint1); polygonToCut.addVertex(intersectionPoint2); polygonToCut.update(); return numberOfVerticesRemoved;
@Test public void testIntersectionWithLine3() { // point polygon T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(new Point2D(1.0, 0.0)); polygon.update(); Line2D line1 = new Line2D(new Point2D(3.0, 1.0), new Vector2D(-2.0, -1.0)); Point2D[] expected1 = new Point2D[] {new Point2D(1.0, 0.0)}; assertPointsEqual(expected1, polygon.intersectionWith(line1), false); Line2D line2 = new Line2D(new Point2D(2.0, 1.0), new Vector2D(-1.3, -0.8)); Point2D[] expected2 = null; assertPointsEqual(expected2, polygon.intersectionWith(line2), false); }
@Test public void testClear() { ArrayList<Point2D> verticesList = new ArrayList<>(); verticesList.add(new Point2D(0.0, 0.0)); verticesList.add(new Point2D(0.0, 1.0)); verticesList.add(new Point2D(1.0, 0.0)); verticesList.add(new Point2D(1.0, 1.0)); T list = createConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(verticesList)); assertEquals("Number of vertices should be 4", 4.0, list.getNumberOfVertices(), EPSILON); assertTrue(list.isUpToDate()); list.clearAndUpdate(); assertEquals("Number of vertices should be 0", 0.0, list.getNumberOfVertices(), EPSILON); assertTrue(list.isUpToDate()); list.clear(); assertFalse(list.isUpToDate()); list.clearAndUpdate(); assertTrue(list.isUpToDate()); EuclidCoreTestTools.assertTuple2DContainsOnlyNaN(list.getBoundingBox().getMinPoint()); EuclidCoreTestTools.assertTuple2DContainsOnlyNaN(list.getBoundingBox().getMaxPoint()); EuclidCoreTestTools.assertTuple2DContainsOnlyNaN(list.getCentroid()); assertTrue(Double.isNaN(list.getArea())); }
/** * Transforms this convex polygon using the given homogeneous transformation matrix. * * @param transform the transform to apply on the vertices of this convex polygon. Not modified. * @param checkIfTransformInXYPlane whether this method should assert that the rotation part of the * given transform represents a transformation in the XY plane. * @throws OutdatedPolygonException if {@link #update()} has not been called since last time this * polygon's vertices were edited. * @throws EmptyPolygonException if this polygon is empty when calling this method. * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the rotation part * of {@code transform} is not a transformation in the XY plane. */ default void applyTransform(Transform transform, boolean checkIfTransformInXYPlane) { checkIfUpToDate(); notifyVerticesChanged(); for (int i = 0; i < getNumberOfVertices(); i++) { getVertexUnsafe(i).applyTransform(transform, checkIfTransformInXYPlane); } update(); }
polygon.addVertex(new Point2D(0.0, 0.0)); polygon.addVertex(new Point2D(1.0, 0.0)); polygon.addVertex(new Point2D(0.0, 1.0)); polygon.addVertex(new Point2D(1.0, 1.0)); polygon.update(); for (int i = 0; i < polygon.getNumberOfVertices(); i++) assertFalse(polygon.canObserverSeeEdge(i, observer1)); for (int i = 0; i < polygon.getNumberOfVertices(); i++) if (polygon.getVertex(i).epsilonEquals(new Point2D(0.0, 0.0), EPSILON)) assertTrue(polygon.canObserverSeeEdge(i, observer2)); else assertFalse(polygon.canObserverSeeEdge(i, observer2)); for (int i = 0; i < polygon.getNumberOfVertices(); i++) if (polygon.getVertex(i).epsilonEquals(new Point2D(0.0, 1.0), EPSILON)) assertTrue(polygon.canObserverSeeEdge(i, observer3)); else if (polygon.getVertex(i).epsilonEquals(new Point2D(1.0, 1.0), EPSILON)) assertTrue(polygon.canObserverSeeEdge(i, observer3)); else assertFalse(polygon.canObserverSeeEdge(i, observer3));
@Test public void testIsPointInside5() { // foot polygon T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(new Point2D(-0.06, -0.08)); polygon.addVertex(new Point2D(0.14, -0.08)); polygon.addVertex(new Point2D(0.14, -0.19)); polygon.addVertex(new Point2D(-0.06, -0.19)); polygon.update(); Point2D point1 = new Point2D(0.03, 0.0); assertFalse(polygon.isPointInside(point1, 0.02)); Point2D point2 = new Point2D(0.03, -0.09); assertTrue(polygon.isPointInside(point2)); }
private boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices(ConvexPolygon2DReadOnly polygonWithExactlyTwoVertices, ConvexPolygon2DReadOnly polygonWithAtLeastTwoVertices, ConvexPolygon2DBasics intersectingPolygon) { polygonWithTwoVerticesAsLineSegment.set(polygonWithExactlyTwoVertices.getVertex(0), polygonWithExactlyTwoVertices.getVertex(1)); int intersections = polygonWithAtLeastTwoVertices.intersectionWith(polygonWithTwoVerticesAsLineSegment, intersection1, intersection2); if (intersections == 0) { intersectingPolygon.clearAndUpdate(); return false; } else if (intersections == 1) { intersectingPolygon.clear(); intersectingPolygon.addVertex(intersection1); intersectingPolygon.update(); return true; } else { intersectingPolygon.clear(); intersectingPolygon.addVertex(intersection1); intersectingPolygon.addVertex(intersection2); intersectingPolygon.update(); return true; } }
polygonToPack.set(polygonQ); return true; polygonToPack.clear(); polygonAsLineSegment.direction(true, normalizedVector); normalizedVector.scale(-distance); polygonToPack.addVertex(vertex0.getX() - normalizedVector.getX(), vertex0.getY() - normalizedVector.getY()); polygonToPack.addVertex(vertex1.getX() + normalizedVector.getX(), vertex1.getY() + normalizedVector.getY()); polygonToPack.addVertex(vertex0.getX() + normalizedVector.getX(), vertex0.getY() + normalizedVector.getY()); polygonToPack.addVertex(vertex0.getX() - normalizedVector.getX(), vertex0.getY() - normalizedVector.getY()); polygonToPack.addVertex(vertex1.getX() + normalizedVector.getX(), vertex1.getY() + normalizedVector.getY()); polygonToPack.addVertex(vertex1.getX() - normalizedVector.getX(), vertex1.getY() - normalizedVector.getY()); polygonToPack.update(); return true; polygonToPack.clear(); polygonToPack.addVertex(midPoint); polygonToPack.update(); return false; polygonToPack.clear(); polygonToPack.addVertex(newVertex0); polygonToPack.addVertex(newVertex1); polygonToPack.update();; polygonToPack.addVertex(vertex0.getX() + distance, vertex0.getY() + distance); polygonToPack.addVertex(vertex0.getX() + distance, vertex0.getY() - distance); polygonToPack.addVertex(vertex0.getX() - distance, vertex0.getY() + distance);
@Test public void testGetSignedDistance1() { // single point polygon T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(new Point2D(0.0, 0.0)); polygon.update(); Point2D point = new Point2D(2.5, 1.0); double distance = polygon.signedDistance(point); assertDistanceCorrect(Math.sqrt(2.5 * 2.5 + 1.0 * 1.0), distance); }
@Test public void testGetClosestEdge3() { T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(new Point2D()); polygon.update(); assertTrue(polygon.getClosestEdgeCopy(new Point2D()) == null); }
@Test public void testOrthogonalProjection3() { // single point polygon Point2D vertex = new Point2D(1.0, 2.0); T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(vertex); polygon.update(); EuclidCoreTestTools.assertTuple2DEquals(vertex, polygon.orthogonalProjectionCopy(new Point2D(0.0, 0.0)), EPSILON); EuclidCoreTestTools.assertTuple2DEquals(vertex, polygon.orthogonalProjectionCopy(new Point2D(1.0, -0.2)), EPSILON); EuclidCoreTestTools.assertTuple2DEquals(vertex, polygon.orthogonalProjectionCopy(new Point2D(1.0, 2.0)), EPSILON); }
@Test public void testGetClosestPointToRay3() { Point2D vertex = new Point2D(1.0, -1.0); T polygon = createEmptyConvexPolygon2D(); polygon.addVertex(vertex); polygon.update(); Line2D ray1 = new Line2D(new Point2D(5.0, -3.0), new Vector2D(0.0, 1.0)); EuclidCoreTestTools.assertTuple2DEquals(vertex, polygon.getClosestPointWithRay(ray1), EPSILON); Line2D ray2 = new Line2D(new Point2D(0.0, 0.0), new Vector2D(1.0, 0.0)); EuclidCoreTestTools.assertTuple2DEquals(vertex, polygon.getClosestPointWithRay(ray2), EPSILON); }
intersectingPolygonToPack.clear(); intersectingPolygonToPack.clear(); intersectingPolygonToPack.update(); return false; intersectingPolygonToPack.addVertex(intersection); intersectingPolygonToPack.addVertex(polygonP.getVertex(indexP)); indexP = polygonP.getNextVertexIndex(indexP); intersectingPolygonToPack.addVertex(polygonQ.getVertex(indexQ)); indexQ = polygonQ.getNextVertexIndex(indexQ); intersectingPolygonToPack.update(); if (intersectingPolygonToPack.isEmpty()) return false;