public ArrayList<Polygon> getCSObstacles() { ArrayList<Polygon> csObstacles = new ArrayList<Polygon>(); for (Polygon obstacle : obstacles) { csObstacles.add(Polygon.minkowskiSumSimple(obstacle, reflectedRobot)); } return csObstacles; }
// occupancy grid is indexed from (xMin, yMin) public boolean[][] getOccupancyGrid(int nCellsLinear) { System.err.println("GOT HERE YO!"); int i, j; double xLow, xHigh, yLow, yHigh; double maxDimension = Math.max(xMax - xMin, yMax - yMin); double resolutionLinear = maxDimension / nCellsLinear; Polygon resolutionCellSpace; resolutionCellSpace = new Polygon( Arrays.asList( Mat.encodePoint(-1 * resolutionLinear / 2, -1 * resolutionLinear / 2), Mat.encodePoint(-1 * resolutionLinear / 2, resolutionLinear / 2), Mat.encodePoint(resolutionLinear / 2, resolutionLinear / 2), Mat.encodePoint(resolutionLinear / 2, -1 * resolutionLinear / 2))); reflectedRobot = Polygon.minkowskiSumSimple(reflectedRobot, resolutionCellSpace); ArrayList<Polygon> csObstacles = getCSObstacles(); System.err.println("GOT HERE as well."); boolean[][] occupancyGrid = new boolean[nCellsLinear][nCellsLinear]; for (i = 0; i < nCellsLinear; i++) { xLow = xMin + resolutionLinear * i; xHigh = xLow + resolutionLinear; for (j = 0; j < nCellsLinear; j++) { yLow = yMin + resolutionLinear * j; yHigh = yLow + resolutionLinear; if (i == 9 && j == 6) { System.err.print("(" + i + ", " + j + ") "); System.err.println("(" + ((xLow + xHigh) / 2) + ", " + ((yLow + yHigh) / 2) + ")"); } occupancyGrid[i][j] = true; for (Polygon obstacle : csObstacles) { if (Polygon.pointInPolygon( obstacle, Mat.encodePoint((xLow + xHigh) / 2, (yLow + yHigh) / 2))) { occupancyGrid[i][j] = false; break; } } } } System.err.println("GOT HERE TOO!"); return occupancyGrid; }