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