예제 #1
0
 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;
 }
예제 #2
0
  // 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;
  }