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