// this should throw away points that are inside of the other polygon
public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(
FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2) {
ReferenceFrame referenceFrame = polygon1.getReferenceFrame();
polygon2.checkReferenceFrameMatch(referenceFrame);
ConvexPolygon2dAndConnectingEdges polygonAndEdges =
combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon);
if (polygonAndEdges == null) return null; // Return null if not disjoint
FrameConvexPolygon2d frameConvexPolygon2d =
new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d());
FrameLineSegment2d connectingEdge1 =
new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1());
FrameLineSegment2d connectingEdge2 =
new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2());
FrameConvexPolygon2dAndConnectingEdges ret =
new FrameConvexPolygon2dAndConnectingEdges(
polygon1, polygon2, frameConvexPolygon2d, connectingEdge1, connectingEdge2);
return ret;
}
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();
return true;
}
public static int cutPolygonWithLine(
FrameLine2d cuttingLine, FrameConvexPolygon2d polygonToCut, RobotSide sideOfLineToCut) {
FramePoint2d[] intersectionPoints = polygonToCut.intersectionWith(cuttingLine);
if (intersectionPoints == null || intersectionPoints.length == 1) {
return -1;
} else {
int numberOfVerticesRemoved = 0;
int index = 0;
while (index < polygonToCut.getNumberOfVertices()) {
FramePoint2d vertex = polygonToCut.getFrameVertexUnsafe(index);
if (cuttingLine.isPointOnSideOfLine(vertex, sideOfLineToCut)) {
polygonToCut.removeVertex(index);
numberOfVerticesRemoved++;
} else {
index++;
}
}
polygonToCut.addVertices(intersectionPoints);
polygonToCut.update();
return numberOfVerticesRemoved;
}
}
