/** * This function changes the polygon given, such that it has the desired number of vertices. It * is conservative in the sense, that the modified polygon will be contained in the original * polygon completely. * * @param polygon: modified to have the desired number of vertices * @param desiredVertices: number of vertices that the polygon should have */ public static void limitVerticesConservative(FixedFrameConvexPolygon2DBasics polygon, int desiredVertices) { limitVerticesConservative((ConvexPolygon2DBasics) polygon, desiredVertices); }
if (computeIntersectionOfPolygons(polygon1, polygon2, intersectingPolygonToPack)) findStartAndEndTangents(polygon2.getVertex(0), polygon1, epsilon, v1Tangents); findStartAndEndTangents(polygon1.getVertex(0), polygon2, epsilon, v2Tangents); int v2End = v2Tangents[1]; binaryElimination(polygon1, polygon2, v1Start, v1End, v2Start, v2End, epsilon, updatedIndices); v1Start = updatedIndices[0]; v1End = updatedIndices[1]; v2End = updatedIndices[3]; getClosestPointsFromRemainingEdgesAndVertices(polygon1, polygon2, v1Start, v1End, v2Start, v2End, point1ToPack, point2ToPack);
public SteppingStone(String name, double baseZ, double height, ConvexPolygon2D convexPolygon2d, ConvexPolygon2D footPolygon) { this.name = name; this.baseZ = baseZ; this.height = height; this.convexPolygon2d = new ConvexPolygon2D(convexPolygon2d); polygonToShrink = new ConvexPolygon2D(footPolygon); new ConvexPolygonTools().shrinkInto(polygonToShrink, new Point2D(0.0, 0.0), convexPolygon2d, shrunkenPolygon); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testIntersectionWhenFullyInsideWithRepeatedPoint() { ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); ArrayList<Point2D> listOfPoints = new ArrayList<Point2D>(); listOfPoints.add(new Point2D(0.19, 0.0)); listOfPoints.add(new Point2D(0.192, 0.6)); listOfPoints.add(new Point2D(0.25, 0.5)); listOfPoints.add(new Point2D(0.19, 0.0)); ConvexPolygon2D convexPolygon2dA = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(listOfPoints)); listOfPoints.clear(); listOfPoints.add(new Point2D(-1.0, -1.0)); listOfPoints.add(new Point2D(2.0, -1.0)); listOfPoints.add(new Point2D(-1.0, 2.0)); listOfPoints.add(new Point2D(2.0, 2.0)); ConvexPolygon2D convexPolygon2dB = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(listOfPoints)); ConvexPolygon2D intersection = new ConvexPolygon2D(); convexPolygonTools.computeIntersectionOfPolygons(convexPolygon2dA, convexPolygon2dB, intersection); boolean epsilonEquals = intersection.epsilonEquals(convexPolygon2dA, 1e-14); assertTrue(epsilonEquals); convexPolygonTools.computeIntersectionOfPolygons(convexPolygon2dB, convexPolygon2dA, intersection); epsilonEquals = intersection.epsilonEquals(convexPolygon2dA, 1e-14); assertTrue(epsilonEquals); }
Random random = new Random(756483920L); ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); assertTrue(polygonWithOnePoint.getVertex(0).equals(pointThatDefinesThePolygon)); assertFalse(convexPolygonTools.computeIntersectionOfPolygons(polygonWithOnePoint, sparePolygon, intersectionPolygon)); assertTrue(polygonWithOnePoint.intersectionWith(arbitraryLine) == null); assertFalse(polygonWithOnePoint.isPointInside(arbitraryPoint0)); LineSegment2D connectingEdge1 = new LineSegment2D(); LineSegment2D connectingEdge2 = new LineSegment2D(); convexPolygonTools.combineDisjointPolygons(polygonWithOnePoint, anotherPolygonWithOnePoint, combinedPolygon, connectingEdge1, connectingEdge2); assertEquals(2, combinedPolygon.getNumberOfVertices()); point0 = combinedPolygon.getVertex(0); assertEqualsInEitherOrder(pointThatDefinesThePolygon, pointThatDefinesAnotherPolygon, point0, point1); assertTrue(convexPolygonTools.computeIntersectionOfPolygons(polygonWithOnePoint, anotherPolygonWithOnePoint, new ConvexPolygon2D()) == false); ConvexPolygon2D intersection = new ConvexPolygon2D(); convexPolygonTools.computeIntersectionOfPolygons(polygonWithOnePoint, polygonWithOnePoint, intersection); assertEquals(1, intersection.getNumberOfVertices()); convexPolygonTools.computeIntersectionOfPolygons(polygonWithOnePoint, polygonWithOnePoint, intersection); assertTrue(intersection.getVertex(0).equals(pointThatDefinesThePolygon)); assertTrue(polygonWithOnePoint.intersectionWith(arbitraryLineSegment) == null);
public double computeIntersectionAreaOfPolygons(ConvexPolygon2DReadOnly polygonP, ConvexPolygon2DReadOnly polygonQ) { if (computeIntersectionOfPolygons(polygonP, polygonQ, intersectionToThrowAway)) return intersectionToThrowAway.getArea(); else return 0.0; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testDistanceBetweenPolygonsIntersectingPolygons() { ConvexPolygon2D polygon1 = getPolygon(new double[] {0, 0, 0, 1, 1, 0, 2, 1, 1, 2}); ConvexPolygon2D polygon2 = getPolygon(new double[] {1, 1, 0, 3, 2, 2, 3, 0}); try { new ConvexPolygonTools().computeMinimumDistancePoints(polygon1, polygon2, new Point2D(), new Point2D()); fail(); } catch (RuntimeException re) { assertEquals(re.getMessage(), "Cannot compute minimum distance between intersecting polygons."); } try { new ConvexPolygonTools().computeMinimumDistancePoints(polygon2, polygon1, new Point2D(), new Point2D()); fail(); } catch (RuntimeException re) { assertEquals(re.getMessage(), "Cannot compute minimum distance between intersecting polygons."); } }
polygon2.update(); ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); FrameConvexPolygon2D actualPolygon = new FrameConvexPolygon2D(); FrameLineSegment2D connectingEdge1 = new FrameLineSegment2D(); FrameLineSegment2D connectingEdge2 = new FrameLineSegment2D(); convexPolygonTools.combineDisjointPolygons(polygon1, polygon2, actualPolygon, connectingEdge1, connectingEdge2);
/** * Efficiently combines two Disjoint Polygons. Returns null if not disjoint. * Note: Generates garbage! * * @param polygon1 ConvexPolygon2d * @param polygon2 ConvexPolygon2d * @return ConvexPolygon2dAndConnectingEdges */ public static ConvexPolygon2dAndConnectingEdges combineDisjointPolygons(ConvexPolygon2d polygon1, ConvexPolygon2d polygon2) { ConvexPolygon2d combinedPolygon = new ConvexPolygon2d(); LineSegment2d connectingEdge1 = new LineSegment2d(); LineSegment2d connectingEdge2 = new LineSegment2d(); boolean success = combineDisjointPolygons(polygon1, polygon2, combinedPolygon, connectingEdge1, connectingEdge2); if (!success) return null; return new ConvexPolygon2dAndConnectingEdges(combinedPolygon, connectingEdge1, connectingEdge2); }
public int cutPolygonWithLine(FrameLine2DReadOnly cuttingLine, FixedFrameConvexPolygon2DBasics polygonToCut, RobotSide sideOfLineToCut) { cuttingLine.checkReferenceFrameMatch(polygonToCut); return cutPolygonWithLine(cuttingLine, (ConvexPolygon2DBasics) polygonToCut, sideOfLineToCut); }
public void computeMinimumDistancePoints(ConvexPolygon2DReadOnly polygon1, ConvexPolygon2DReadOnly polygon2, Point2DBasics point1ToPack, Point2DBasics point2ToPack) { computeMinimumDistancePoints(polygon1, polygon2, .01, point1ToPack, point2ToPack); }
boolean success = new ConvexPolygonTools().computeIntersectionOfPolygons(convexPolygon2ds.get(i), captureRegionPolygon, intersection); if (success)
public boolean intersectionWith(ConvexPolygon2d convexPolygon, ConvexPolygon2d intersectionToPack) { checkIfUpToDate(); return ConvexPolygonTools.computeIntersectionOfPolygons(this, convexPolygon, intersectionToPack); }
private void assertPolygons(double[] p1, double[] p2, double[] expectedSolution, double epsilon) { if (expectedSolution.length != 4) { throw new RuntimeException("Invalid input."); } ConvexPolygon2D polygon1 = getPolygon(p1); ConvexPolygon2D polygon2 = getPolygon(p2); Point2D[] closestPoints = new Point2D[] {new Point2D(), new Point2D()}; Point2D[] closestPointsReversed = new Point2D[] {new Point2D(), new Point2D()}; ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); convexPolygonTools.computeMinimumDistancePoints(polygon1, polygon2, closestPoints[0], closestPoints[1]); convexPolygonTools.computeMinimumDistancePoints(polygon2, polygon1, closestPointsReversed[0], closestPointsReversed[1]); assertEquals(closestPoints[0].distance(closestPoints[1]), closestPointsReversed[0].distance(closestPointsReversed[1]), epsilon); assertEquals(expectedSolution[0], closestPoints[0].getX(), epsilon); assertEquals(expectedSolution[1], closestPoints[0].getY(), epsilon); assertEquals(expectedSolution[2], closestPoints[1].getX(), epsilon); assertEquals(expectedSolution[3], closestPoints[1].getY(), epsilon); }
FrameConvexPolygon2D polygon2 = new FrameConvexPolygon2D(FrameVertex2DSupplier.asFrameVertex2DSupplier(points2)); ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); FrameConvexPolygon2D combinedPolygon = new FrameConvexPolygon2D(zUpFrame); FrameLineSegment2D connectingEdge1 = new FrameLineSegment2D(zUpFrame); for (int i = 0; i < numTests; i++) convexPolygonTools.combineDisjointPolygons(polygon1, polygon2, combinedPolygon, connectingEdge1, connectingEdge2);
public static boolean combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2, FrameConvexPolygon2d combinedPolygonToPack, FrameLineSegment2d connectingEdge1ToPack, FrameLineSegment2d connectingEdge2ToPack) { combinedPolygonToPack.clear(polygon1.getReferenceFrame()); combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge1ToPack); combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge2ToPack); boolean success = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon, combinedPolygonToPack.convexPolygon, connectingEdge1ToPack.lineSegment, connectingEdge2ToPack.lineSegment); if (!success) return false; // combinedPolygonToPack.updateFramePoints(); combinedPolygonToPack.update(); return true; }
public static int cutPolygonWithLine(FrameLine2d cuttingLine, FrameConvexPolygon2d polygonToCut, RobotSide sideOfLineToCut) { cuttingLine.checkReferenceFrameMatch(polygonToCut); return cutPolygonWithLine(cuttingLine.getLine2d(), polygonToCut.getConvexPolygon2d(), sideOfLineToCut); }
public static Point2d[] computeMinimumDistancePoints(ConvexPolygon2d polygon1, ConvexPolygon2d polygon2) { return computeMinimumDistancePoints(polygon1, polygon2, .01); }
/** * This function changes the polygon given, such that it has the desired number of vertices. It is conservative in * the sense, that the modified polygon will be contained in the original polygon completely. * * @param polygon: modified to have the desired number of vertices * @param desiredVertices: number of vertices that the polygon should have */ public static void limitVerticesConservative(FrameConvexPolygon2d polygon, int desiredVertices) { limitVerticesConservative(polygon.getConvexPolygon2d(), desiredVertices); }
/** * Returns a transform that will move the given polygon into a planar region. Problematic if the planar region consists of * multiple sub convex polygons. The polygon to wiggle must have the same transform to world as the planar region. * * @param polygonToWiggleInRegionFrame * @param regionToWiggleInto * @param wiggleParameters * @return */ public static RigidBodyTransform wigglePolygonIntoRegion(ConvexPolygon2D polygonToWiggleInRegionFrame, PlanarRegion regionToWiggleInto, WiggleParameters parameters) { // find the part of the region that has the biggest intersection with the polygon ConvexPolygon2D bestMatch = null; double overlap = 0.0; ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools(); for (int i = 0; i < regionToWiggleInto.getNumberOfConvexPolygons(); i++) { ConvexPolygon2D intersection = new ConvexPolygon2D(); convexPolygonTools.computeIntersectionOfPolygons(regionToWiggleInto.getConvexPolygon(i), polygonToWiggleInRegionFrame, intersection); if (intersection.getArea() > overlap) { overlap = intersection.getArea(); bestMatch = regionToWiggleInto.getConvexPolygon(i); } } if (bestMatch == null) return null; return findWiggleTransform(polygonToWiggleInRegionFrame, bestMatch, parameters); }