public static ConvexPolygon2d shrinkInto( ConvexPolygon2d polygonP, Point2d referencePointInP, ConvexPolygon2d polygonQ) { if (polygonQ.hasAtLeastOneVertex() && !polygonQ.hasAtLeastThreeVertices()) { return new ConvexPolygon2d(polygonQ); } ArrayList<Line2d> rays = new ArrayList<Line2d>(); Point2d referencePointInPCopy = new Point2d(referencePointInP); int leftMostIndexOnPolygonQ = polygonQ.getMinXIndex(); Point2d vertexQ = polygonQ.getVertex(leftMostIndexOnPolygonQ); int nextVertexQIndex = polygonQ.getNextVertexIndex(leftMostIndexOnPolygonQ); Point2d nextVertexQ = polygonQ.getVertex(nextVertexQIndex); int leftMostIndexOnPolygonP = polygonP.getMinXIndex(); Point2d vertexP = polygonP.getVertex(leftMostIndexOnPolygonP); int nextVertexPIndex = polygonP.getNextVertexIndex(leftMostIndexOnPolygonP); Point2d nextVertexP = polygonP.getVertex(nextVertexPIndex); forEachPolygonQ: for (int i = 0; i < polygonQ.getNumberOfVertices(); i++) { Vector2d edgeOnQ = new Vector2d(nextVertexQ.getX() - vertexQ.getX(), nextVertexQ.getY() - vertexQ.getY()); int j = 0; while (j < polygonP.getNumberOfVertices()) { Vector2d edgeOnP = new Vector2d(nextVertexP.getX() - vertexP.getX(), nextVertexP.getY() - vertexP.getY()); double crossProduct = edgeOnQ.getX() * edgeOnP.getY() - edgeOnP.getX() * edgeOnQ.getY(); if (crossProduct <= 0.0) { referencePointInPCopy.setX(referencePointInP.getX() + vertexQ.getX() - vertexP.getX()); referencePointInPCopy.setY(referencePointInP.getY() + vertexQ.getY() - vertexP.getY()); Line2d ray = new Line2d(referencePointInPCopy, edgeOnQ); rays.add(ray); vertexQ = nextVertexQ; nextVertexQIndex = polygonQ.getNextVertexIndex(nextVertexQIndex); nextVertexQ = polygonQ.getVertex(nextVertexQIndex); continue forEachPolygonQ; } else { j++; vertexP = nextVertexP; nextVertexPIndex = polygonP.getNextVertexIndex(nextVertexPIndex); nextVertexP = polygonP.getVertex(nextVertexPIndex); } } throw new RuntimeException("Should never get here!!"); } ConvexPolygonConstructorFromInteriorOfRays convexPolygonConstructorFromInteriorOfRays = new ConvexPolygonConstructorFromInteriorOfRays(); ConvexPolygon2d polygonToReturn = new ConvexPolygon2d(); boolean foundPolygon = convexPolygonConstructorFromInteriorOfRays.constructFromInteriorOfRays( rays, polygonToReturn); if (foundPolygon) return polygonToReturn; return null; }
/** * Computes the intersection of two convex polygons. For references see: * http://www.iro.umontreal.ca/~plante/compGeom/algorithm.html Returns null if the polygons do not * intersect Returns the inside polygon if the two polygons are inside one another. * * @param polygonP ConvexPolygon2d * @param polygonQ ConvexPolygon2d * @return ConvexPolygon2d Intersection of polygonP and polygonQ */ public static boolean computeIntersectionOfPolygons( ConvexPolygon2d polygonP, ConvexPolygon2d polygonQ, ConvexPolygon2d intersectingPolygonToPack) { // return false if either polygon null if (polygonP == null || polygonP.isEmpty()) return false; if (polygonQ == null || polygonQ.isEmpty()) return false; if (polygonP.hasExactlyTwoVertices() && polygonQ.hasAtLeastTwoVertices()) { return ConvexPolygonTools .computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices( polygonP, polygonQ, intersectingPolygonToPack); } else if (polygonQ.hasExactlyTwoVertices()) { return ConvexPolygonTools .computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices( polygonQ, polygonP, intersectingPolygonToPack); } if (polygonP.hasExactlyOneVertex() && polygonQ.hasAtLeastOneVertex()) { return ConvexPolygonTools.computeIntersectionOfPolygonsIfOnePolygonHasExactlyOneVertex( polygonP, polygonQ, intersectingPolygonToPack); } else if (polygonQ.hasExactlyOneVertex()) { return ConvexPolygonTools.computeIntersectionOfPolygonsIfOnePolygonHasExactlyOneVertex( polygonQ, polygonP, intersectingPolygonToPack); } // Find left most point on polygon1 int currentPPolygonPointIndex = polygonP.getMinXIndex(); int initialPolygonPIndex = polygonP.getMinXIndex(); Point2d currentPolygonPPoint = polygonP.getVertex(currentPPolygonPointIndex); // Find left most point on polygon2 int currentQPolygonPointIndex = polygonQ.getMinXIndex(); int initialPolygonQIndex = polygonQ.getMinXIndex(); Point2d currentPolygonQPoint = polygonQ.getVertex(currentQPolygonPointIndex); // At each of those two vertices, place a vertical line passing through it. Associate that line // to the polygon to which the vertex belongs. Vector2d caliperForPolygonP = new Vector2d(0.0, 1.0); Vector2d caliperForPolygonQ = new Vector2d(0.0, 1.0); // determine which side polygon2's caliper line is on relative to polygon1's caliper line Point2d lineStart = new Point2d(currentPolygonPPoint); Point2d lineEnd = new Point2d(currentPolygonPPoint); lineEnd.add(caliperForPolygonP); boolean isOnLeft = GeometryTools.isPointOnLeftSideOfLine(currentPolygonQPoint, lineStart, lineEnd); boolean wasOnLeft = isOnLeft; // System.out.println("wasOnLeft = " + wasOnLeft); boolean gotAroundPOnce = false; boolean gotAroundQOnce = false; boolean DONE = false; int bridgeCount = 0; ArrayList<Integer> bridgeIndicesP = new ArrayList<Integer>(); ArrayList<Integer> bridgeIndicesQ = new ArrayList<Integer>(); ArrayList<Boolean> bridgeWasOnLeft = new ArrayList<Boolean>(); do { if (gotAroundPOnce && gotAroundQOnce) { DONE = true; } // Rotate these two lines (called calipers) by the smallest angle between a caliper and the // segment following the vertex it passes // through (in clockwise order). The rotation is done about the vertex through which the line // passes on the associated polygon. // If the line passes through more than one vertex of the assciated polygon, the farthest (in // clockwise order) is taken. // find angle from current to next point for polygon1 Vector2d vectorToNextPointOnPolygonP = new Vector2d(polygonP.getNextVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.sub(polygonP.getVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.normalize(); // +++JEP: Don't actually compute the angle! Just look at the dot products! // double angleToNextPointOnPolygonP = // caliperForPolygonP.angle(vectorToNextPointOnPolygonP); // //Math.acos(vectorToNextPointOnPolygon1.getY()); double dotProductToNextPointOnPolygonP = caliperForPolygonP.dot( vectorToNextPointOnPolygonP); // Math.acos(vectorToNextPointOnPolygon1.getY()); // find angle from current to next point for polygon2 Vector2d vectorToNextPointOnPolygonQ = new Vector2d(polygonQ.getNextVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.sub(polygonQ.getVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.normalize(); // double angleToNextPointOnPolygonQ = // caliperForPolygonQ.angle(vectorToNextPointOnPolygonQ); // //Math.acos(vectorToNextPointOnPolygon2.getY()); double dotProductToNextPointOnPolygonQ = caliperForPolygonQ.dot( vectorToNextPointOnPolygonQ); // Math.acos(vectorToNextPointOnPolygon2.getY()); // determine the smallest angle and rotate both calipers by this amount boolean moveCaliperP = false; boolean moveCaliperQ = false; // if (angleToNextPointOnPolygonP == angleToNextPointOnPolygonQ) if (dotProductToNextPointOnPolygonP == dotProductToNextPointOnPolygonQ) { caliperForPolygonP.set(vectorToNextPointOnPolygonP); caliperForPolygonQ.set(caliperForPolygonP); moveCaliperP = true; moveCaliperQ = true; } // else if (angleToNextPointOnPolygonP < angleToNextPointOnPolygonQ) else if (dotProductToNextPointOnPolygonP > dotProductToNextPointOnPolygonQ) { caliperForPolygonP.set(vectorToNextPointOnPolygonP); caliperForPolygonQ.set(caliperForPolygonP); moveCaliperP = true; } else { caliperForPolygonQ.set(vectorToNextPointOnPolygonQ); caliperForPolygonP.set(caliperForPolygonQ); moveCaliperQ = true; } // Whenever the order of the two calipers change, a pocket has been found. To detect this, a // direction is associated with one of // the lines (for example the green one, associated to P). Then all points of the red line // (associated to Q) are either to the // left or to the right of the green line. When a rotation makes them change from one side to // the other of the green line, then // the order of the two lines has changed. // detemine which side polygon Q's caliper line is on relative to polygon P's caliper lline lineStart = new Point2d(currentPolygonPPoint); lineEnd = new Point2d(currentPolygonPPoint); lineEnd.add(caliperForPolygonP); isOnLeft = GeometryTools.isPointOnLeftSideOfLine(currentPolygonQPoint, lineStart, lineEnd); // System.out.println("new isOnLeft = " + isOnLeft); if (wasOnLeft != isOnLeft) { // find all of the bridges // System.out.println("Found bridge. wasOnLeft = " + wasOnLeft + ", isOnLeft = " + // isOnLeft); // Some weird fence post thing here. Sometime you want to consider the start ones at the // end, sometimes you don't. // So that's why DONE is computed at the top and checked on the bottom... boolean addThisOne = true; if ((DONE) && (!bridgeIndicesP.isEmpty())) { if ((bridgeIndicesP.get(0) == currentPPolygonPointIndex) && (bridgeIndicesQ.get(0) == currentQPolygonPointIndex)) { addThisOne = false; } } if (addThisOne) { bridgeIndicesP.add(currentPPolygonPointIndex); bridgeIndicesQ.add(currentQPolygonPointIndex); bridgeWasOnLeft.add(wasOnLeft); // bridgeIndices1[bridgeCount] = currentPPolygonPointIndex; // bridgeIndices2[bridgeCount] = currentQPolygonPointIndex; // bridgeWasOnLeft[bridgeCount] = wasOnLeft; bridgeCount++; // update current caliper relationship wasOnLeft = isOnLeft; } } // The algorithm terminates once it has gone around both polygons if (moveCaliperP) { currentPPolygonPointIndex = polygonP.getNextVertexIndex(currentPPolygonPointIndex); currentPolygonPPoint = polygonP.getVertex(currentPPolygonPointIndex); if (currentPPolygonPointIndex == (initialPolygonPIndex) % polygonP.getNumberOfVertices()) { gotAroundPOnce = true; } } if (moveCaliperQ) { currentQPolygonPointIndex = polygonQ.getNextVertexIndex(currentQPolygonPointIndex); currentPolygonQPoint = polygonQ.getVertex(currentQPolygonPointIndex); if (currentQPolygonPointIndex == (initialPolygonQIndex) % polygonQ.getNumberOfVertices()) { gotAroundQOnce = true; } } } while (!DONE); // If no bridges, then check if the polygons are contained in each other. if (bridgeCount == 0) { // check to see if a polygons is contained in another. if (polygonP.isPointInside(polygonQ.getVertex(0))) { intersectingPolygonToPack.setAndUpdate(polygonQ); } if (polygonQ.isPointInside(polygonP.getVertex(0))) { intersectingPolygonToPack.setAndUpdate(polygonP); } } else { boolean success = ConvexPolygonTools.buildCommonPolygonFromBridges( bridgeIndicesP, bridgeIndicesQ, bridgeWasOnLeft, polygonP, polygonQ, intersectingPolygonToPack); if (!success) { intersectingPolygonToPack.clearAndUpdate(); return false; } } // Merge the inner chains to determine intersection return true; }