private static int[][] findCrossingIndices( boolean decrementP, int bridgeIndexForPolygonP, int bridgeIndexForPolygonQ, ConvexPolygon2d polygonP, ConvexPolygon2d polygonQ) { int incrementP = 1, incrementQ = 1; if (decrementP) incrementP = -1; else incrementQ = -1; int indexPStart = bridgeIndexForPolygonP; int indexQStart = bridgeIndexForPolygonQ; int indexPEnd = (indexPStart + incrementP) % polygonP.getNumberOfVertices(); int indexQEnd = (indexQStart + incrementQ) % polygonQ.getNumberOfVertices(); if (indexPEnd < 0) indexPEnd = polygonP.getNumberOfVertices() - 1; if (indexQEnd < 0) indexQEnd = polygonQ.getNumberOfVertices() - 1; Point2d linePStart = polygonP.getVertex(indexPStart); Point2d linePEnd = polygonP.getVertex(indexPEnd); Point2d lineQStart = polygonQ.getVertex(indexQStart); Point2d lineQEnd = polygonQ.getVertex(indexQEnd); int initialPPolygonStartIndex = indexPStart; int initialQPolygonStartIndex = indexQStart; boolean finished; do { finished = true; while (decrementP ^ GeometryTools.isPointOnLeftSideOfLine(lineQEnd, linePStart, linePEnd)) { lineQStart = lineQEnd; indexQStart = indexQEnd; indexQEnd = (indexQEnd + incrementQ) % polygonQ.getNumberOfVertices(); if (indexQEnd < 0) indexQEnd = polygonQ.getNumberOfVertices() - 1; lineQEnd = polygonQ.getVertex(indexQEnd); finished = false; if (indexQStart == initialQPolygonStartIndex) return null; // No intersection. Prevent infinite loop. } while ((!decrementP) ^ GeometryTools.isPointOnLeftSideOfLine(linePEnd, lineQStart, lineQEnd)) { linePStart = linePEnd; indexPStart = indexPEnd; indexPEnd = (indexPEnd + incrementP) % polygonP.getNumberOfVertices(); if (indexPEnd < 0) indexPEnd = polygonP.getNumberOfVertices() - 1; linePEnd = polygonP.getVertex(indexPEnd); finished = false; if (indexPStart == initialPPolygonStartIndex) return null; // No intersection. Prevent infinite loop. } } while (!finished); return new int[][] {{indexPStart, indexPEnd}, {indexQStart, indexQEnd}}; }
private static boolean constructPolygonForIntersection( ArrayList<Boolean> decrementP, int[][][] crossingIndices, ConvexPolygon2d polygonP, ConvexPolygon2d polygonQ, ConvexPolygon2d intersectingPolygonToPack) { int startIndexP1 = crossingIndices[0][0][0]; int endIndexP1 = crossingIndices[0][0][1]; int startIndexQ1 = crossingIndices[0][1][0]; int endIndexQ1 = crossingIndices[0][1][1]; // Want to do things in clockwise order so that making the new polygon is fast. Should only be // one of two cases. // a) P1/Q2 start->end increments, P2/Q1 start->end decrement; OR // b) P2/Q1 start->end increments, P1/Q2 start->end decrement; OR boolean incrementPNext; if ((startIndexP1 + 1) % polygonP.getNumberOfVertices() == endIndexP1) { incrementPNext = true; } else if ((startIndexQ1 + 1) % polygonQ.getNumberOfVertices() == endIndexQ1) { incrementPNext = false; } else { throw new RuntimeException("Neither P1, nor P2 increment!!!"); } intersectingPolygonToPack.clear(); // Start at the first intersection. Add it. Then march to the next intersection. Then continue. for (int i = 0; i < crossingIndices.length; i++) { int startIndexP = crossingIndices[i][0][0]; int endIndexP = crossingIndices[i][0][1]; int startIndexQ = crossingIndices[i][1][0]; int endIndexQ = crossingIndices[i][1][1]; Point2d startP = polygonP.getVertex(startIndexP); Point2d endP = polygonP.getVertex(endIndexP); Point2d startQ = polygonQ.getVertex(startIndexQ); Point2d endQ = polygonQ.getVertex(endIndexQ); Point2d intersection = GeometryTools.getIntersectionBetweenTwoLines(startP, endP, startQ, endQ); intersectingPolygonToPack.addVertex(intersection); if (incrementPNext) { int indexP = crossingIndices[i][0][1]; // endIndexP; int indexPNext = crossingIndices[(i + 1) % crossingIndices.length][0][0]; // System.out.println("indexP = " + indexP + ", indexPNext = " + indexPNext); while (indexP != indexPNext) { intersectingPolygonToPack.addVertex(polygonP.getVertex(indexP)); indexP = polygonP.getNextVertexIndex(indexP); } } else { int indexQ = crossingIndices[i][1][1]; // endIndexQ; int indexQNext = crossingIndices[(i + 1) % crossingIndices.length][1][0]; // System.out.println("indexQ = " + indexQ + ", indexQNext = " + indexQNext); while (indexQ != indexQNext) { intersectingPolygonToPack.addVertex(polygonQ.getVertex(indexQ)); indexQ = polygonQ.getNextVertexIndex(indexQ); } } incrementPNext = !incrementPNext; } intersectingPolygonToPack.update(); if (intersectingPolygonToPack.isEmpty()) return false; return true; }
/** * 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; }