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