public ArrayList<Polygon> getThetaObstacles(double theta, double thetaTolerance) {
Maybe<DoubleMap<ArrayList<Polygon>>.Pair> mp = cSpaceObstacles.get(theta, thetaTolerance);
ArrayList<Polygon> thetaObstacles;
Polygon strokedRobot;
if (mp.just) {
thetaObstacles = mp.value.v;
} else {
strokedRobot =
Polygon.strokeRot(theta - thetaTolerance, theta + thetaTolerance, reflectedRobot);
thetaObstacles = new ArrayList<Polygon>();
for (Polygon obstacle : obstacles) {
thetaObstacles.add(Polygon.minkowskiSum(obstacle, strokedRobot));
}
cSpaceObstacles.put(theta, thetaObstacles, thetaTolerance);
}
return thetaObstacles;
}
// occupancy grid is indexed from (xMin, yMin, 0)
public boolean[][][] getOccupancyGrid(int nCellsLinear, int nCellsAngular) {
int i, j, k;
double xLow, xHigh, yLow, yHigh, thetaLow, thetaHigh;
double maxDimension = Math.max(xMax - xMin, yMax - yMin);
double resolutionLinear = maxDimension / nCellsLinear;
double resolutionAngular = 2 * Math.PI / nCellsAngular;
Polygon strokedRobot;
ArrayList<Polygon> thetaObstacles;
Polygon resolutionCellSpace;
boolean[][][] occupancyGrid = new boolean[nCellsLinear][nCellsLinear][nCellsAngular];
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;
for (k = 0; k < nCellsAngular; k++) {
thetaLow = resolutionAngular * k;
thetaHigh = thetaLow + resolutionAngular;
resolutionCellSpace =
new Polygon(
Arrays.asList(
Mat.encodePoint(xLow, yLow),
Mat.encodePoint(xLow, yHigh),
Mat.encodePoint(xHigh, yHigh),
Mat.encodePoint(xHigh, yLow)));
strokedRobot = Polygon.strokeRot(thetaLow, thetaHigh, reflectedRobot);
occupancyGrid[i][j][k] = true;
for (Polygon obstacle : obstacles) {
if (Polygon.polygonsIntersect(
resolutionCellSpace, Polygon.minkowskiSum(obstacle, strokedRobot))) {
occupancyGrid[i][j][k] = false;
break;
}
}
}
}
}
return occupancyGrid;
}