Example #1
0
    // Generates a new polygon by dragging a polygon through an angle, and taking all the points it
    // touched (and a few extras for overestimation)
    // Only valid for convex polygons, and small rotations (20 degrees or less)
    // stroke from theta1 to theta2
    public static Polygon strokeRot(double theta1, double theta2, Polygon poly) {
      Polygon poly1 = mul(Mat.rotation(theta1), poly);
      Polygon poly2 = mul(Mat.rotation(theta2), poly);
      Polygon stroked = combine(poly1, poly2);
      ArrayList<Mat> cornerVerts;
      Polygon corner;
      int thisVertex, prevVertex, nextVertex;
      Mat origin = Mat.encodePoint(0, 0);
      Mat cornerVert1a, cornerVert1b;
      Mat cornerVert2a, cornerVert2b;
      double actualDist, desiredDist;
      Mat cornerTransform;
      for (thisVertex = 0; thisVertex < poly.vertices.size(); thisVertex++) {
        if (thisVertex == 0) {
          prevVertex = poly.vertices.size() - 1;
        } else {
          prevVertex = thisVertex - 1;
        }

        if (thisVertex == poly.vertices.size() - 1) {
          nextVertex = 0;
        } else {
          nextVertex = thisVertex + 1;
        }

        cornerVerts = new ArrayList<Mat>();
        cornerVerts.add(poly2.vertices.get(nextVertex));
        cornerVerts.add(origin);
        cornerVerts.add(poly1.vertices.get(prevVertex));

        cornerVert1a = poly1.vertices.get(thisVertex);
        cornerVert2a = poly2.vertices.get(thisVertex);

        actualDist = ptSegDistance(cornerVert1a, cornerVert2a, origin);
        desiredDist = Mat.dist(cornerVert1a, Mat.encodePoint(0, 0));

        cornerTransform = Mat.mul(desiredDist / actualDist, Mat.eye(4));
        cornerVert1b = Mat.mul(cornerTransform, cornerVert1a);
        cornerVert2b = Mat.mul(cornerTransform, cornerVert2a);

        cornerVert1b =
            lineSegIntersection(
                cornerVert1b, cornerVert2b, poly1.vertices.get(prevVertex), cornerVert1a);
        cornerVert2b =
            lineSegIntersection(
                cornerVert1b, cornerVert2b, poly2.vertices.get(nextVertex), cornerVert2a);

        cornerVerts.add(cornerVert1b);
        cornerVerts.add(cornerVert2b);

        corner = new Polygon(cornerVerts);
        stroked = combine(stroked, corner);
      }

      return stroked;
    }
Example #2
0
    public static Mat lineSegIntersection(Mat e00, Mat e01, Mat e10, Mat e11) {
      double x0 = e00.data[0][0];
      double y0 = e00.data[1][0];
      double x1 = e10.data[0][0];
      double y1 = e10.data[1][0];

      double x, y;
      double m0, m1;
      if (x0 == e01.data[0][0]) {
        m1 = (y1 - e11.data[1][0]) / (x1 - e11.data[0][0]);

        x = x0;
        y = m1 * (x - x1) + y1;
      } else if (x1 == e11.data[0][0]) {
        m0 = (y0 - e01.data[1][0]) / (x0 - e01.data[0][0]);

        x = x1;
        y = m0 * (x - x0) + y0;
      } else {
        m0 = (y0 - e01.data[1][0]) / (x0 - e01.data[0][0]);
        m1 = (y1 - e11.data[1][0]) / (x1 - e11.data[0][0]);

        // System.err.println("m0 = " + m0);
        // System.err.println("m1 = " + m1);

        x = (m0 * x0 - m1 * x1 - y0 + y1) / (m0 - m1);
        y = m1 * (x - x1) + y1;

        // System.err.println("x = " + x);
        // System.err.println("y = " + y);
      }
      return Mat.encodePoint(x, y);
    }
Example #3
0
 public void addObstacle(PolygonObstacle obstacle) {
   ArrayList<Mat> vertices = new ArrayList<Mat>();
   for (Point2D.Double vert : obstacle.getVertices()) {
     vertices.add(Mat.encodePoint(vert.x, vert.y));
   }
   addObstacle(new Polygon(vertices));
 }
Example #4
0
  // 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;
  }
Example #5
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;
  }
Example #6
0
 public static void tests() {
   System.err.println("HEY!");
   Mat p0 = Mat.encodePoint(0, 0);
   Mat p1 = Mat.encodePoint(1, .1);
   Mat p2 = Mat.encodePoint(.1, 1);
   Mat p3 = Mat.encodePoint(-1, -.1);
   Mat p4 = Mat.encodePoint(-.1, -1);
   Mat p5 = Mat.encodePoint(0.0001, 0);
   Mat p6 = Mat.encodePoint(-0.0001, 0);
   if (!lineSegIntersect(p1, p3, p2, p4)) {
     try {
       throw new Exception();
     } catch (Exception e) {
       e.printStackTrace();
       System.err.println("hey there");
       System.exit(1);
     }
   }
   if (lineSegIntersect(p1, p4, p2, p3)) {
     try {
       throw new Exception();
     } catch (Exception e) {
       e.printStackTrace();
       System.err.println("hey there");
       System.exit(1);
     }
   }
   if (lineSegIntersect(p1, p5, p2, p4)) {
     try {
       throw new Exception();
     } catch (Exception e) {
       e.printStackTrace();
       System.err.println("hey there");
       System.exit(1);
     }
   }
   if (!lineSegIntersect(p1, p6, p2, p4)) {
     try {
       throw new Exception();
     } catch (Exception e) {
       e.printStackTrace();
       System.err.println("hey there");
       System.exit(1);
     }
   }
   Mat inter = lineSegIntersection(p1, p3, p2, p4);
   if (inter.data[0][0] != 0 || inter.data[1][0] != 0) {
     try {
       throw new Exception();
     } catch (Exception e) {
       e.printStackTrace();
       System.err.println("hey there");
       System.exit(1);
     }
   }
 }
Example #7
0
 // assumes that there will be no enclosed empty spaces in the result, and that the result will
 // be contiguous, and that everything is convex
 public static Polygon minkowskiSumSimple(Polygon poly1, Polygon poly2) {
   ArrayList<Mat> verts = new ArrayList<Mat>();
   for (Mat v1 : poly1.vertices) {
     for (Mat v2 : poly2.vertices) {
       verts.add(Mat.add(v1, v2));
     }
   }
   ArrayList<Point2D.Double> points = new ArrayList<Point2D.Double>();
   for (Mat v : verts) {
     points.add(new Point2D.Double(v.data[0][0], v.data[1][0]));
   }
   PolygonObstacle po = GeomUtils.convexHull(points);
   verts = new ArrayList<Mat>();
   for (Point2D.Double vert : po.getVertices()) {
     verts.add(Mat.encodePoint(vert.x, vert.y));
   }
   return new Polygon(verts);
 }
Example #8
0
  private void constructor(
      Polygon robot,
      double boundaryXMin,
      double boundaryYMin,
      double boundaryXMax,
      double boundaryYMax) {
    Polygon.tests();
    cSpaceObstacles.setWrap(-1 * Math.PI, Math.PI);

    xMin = boundaryXMin;
    yMin = boundaryYMin;
    xMax = boundaryXMax;
    yMax = boundaryYMax;

    reflectedRobot = Polygon.mul(Mat.mul(-1, Mat.eye(4)), robot);
    ArrayList<List<Mat>> boundaries = new ArrayList<List<Mat>>();

    boundaries.add(
        Arrays.asList(
            Mat.encodePoint(xMin - 0, yMin - 1),
            Mat.encodePoint(xMin - 0, yMax + 1),
            Mat.encodePoint(xMin - 1, yMax + 1),
            Mat.encodePoint(xMin - 1, yMin - 1)));

    boundaries.add(
        Arrays.asList(
            Mat.encodePoint(xMax + 0, yMin - 1),
            Mat.encodePoint(xMax + 0, yMax + 1),
            Mat.encodePoint(xMax + 1, yMax + 1),
            Mat.encodePoint(xMax + 1, yMin - 1)));

    boundaries.add(
        Arrays.asList(
            Mat.encodePoint(xMin - 1, yMin - 0),
            Mat.encodePoint(xMax + 1, yMin - 0),
            Mat.encodePoint(xMax + 1, yMin - 1),
            Mat.encodePoint(xMin - 1, yMin - 1)));

    boundaries.add(
        Arrays.asList(
            Mat.encodePoint(xMin - 1, yMax + 0),
            Mat.encodePoint(xMax + 1, yMax + 0),
            Mat.encodePoint(xMax + 1, yMax + 1),
            Mat.encodePoint(xMin - 1, yMax + 1)));

    for (List<Mat> boundary : boundaries) {
      addObstacle(new Polygon(boundary));
    }
  }