Ejemplo n.º 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;
    }
Ejemplo n.º 2
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));
    }
  }