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;
}
}
