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