public void printLoop(int n, float x[], float y[]) {
// Need to swap the y component
Polygon P = new Polygon();
for (int i = 0; i < n; i++) P.addPoint(Math.round(x[i]), Math.round(height - y[i]));
poly_draw.add(P);
poly_draw_color.add(curColor);
}
public boolean contains(int x, int y) {
if (!super.contains(x, y)) return false;
if (xSpline != null) {
FloatPolygon poly = new FloatPolygon(xSpline, ySpline, splinePoints);
return poly.contains(x - this.x, y - this.y);
} else if (xpf != null) {
FloatPolygon poly = new FloatPolygon(xpf, ypf, nPoints);
return poly.contains(x - this.x, y - this.y);
} else {
Polygon poly = new Polygon(xp, yp, nPoints);
return poly.contains(x - this.x, y - this.y);
}
}
void finishPolygon() {
if (xpf != null) {
FloatPolygon poly = new FloatPolygon(xpf, ypf, nPoints);
Rectangle r = poly.getBounds();
x = r.x;
y = r.y;
width = r.width;
height = r.height;
bounds = poly.getFloatBounds();
float xbase = (float) bounds.getX();
float ybase = (float) bounds.getY();
for (int i = 0; i < nPoints; i++) {
xpf[i] -= xbase;
ypf[i] -= ybase;
}
} else {
Polygon poly = new Polygon(xp, yp, nPoints);
Rectangle r = poly.getBounds();
x = r.x;
y = r.y;
width = r.width;
height = r.height;
for (int i = 0; i < nPoints; i++) {
xp[i] = xp[i] - x;
yp[i] = yp[i] - y;
}
bounds = null;
}
if (nPoints < 2
|| (!(type == FREELINE || type == POLYLINE || type == ANGLE)
&& (nPoints < 3 || width == 0 || height == 0))) {
if (imp != null) imp.deleteRoi();
if (type != POINT) return;
}
state = NORMAL;
if (imp != null && !(type == TRACED_ROI)) imp.draw(x - 5, y - 5, width + 10, height + 10);
oldX = x;
oldY = y;
oldWidth = width;
oldHeight = height;
if (Recorder.record
&& userCreated
&& (type == POLYGON
|| type == POLYLINE
|| type == ANGLE
|| (type == POINT && Recorder.scriptMode() && nPoints == 3)))
Recorder.recordRoi(getPolygon(), type);
if (type != POINT) modifyRoi();
LineWidthAdjuster.update();
}
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));
}
}
public ArrayList<Polygon> getCSObstacles() {
ArrayList<Polygon> csObstacles = new ArrayList<Polygon>();
for (Polygon obstacle : obstacles) {
csObstacles.add(Polygon.minkowskiSumSimple(obstacle, reflectedRobot));
}
return csObstacles;
}
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;
}
// 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;
}
// 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;
}
// From: http://forum.java.sun.com/thread.jspa?threadID=378460&tstart=135
void drawArrow(
Graphics2D g2d,
int xCenter,
int yCenter,
int x,
int y,
float stroke,
BasicStroke drawStroke) {
double aDir = Math.atan2(xCenter - x, yCenter - y);
// Line can be dashed.
g2d.setStroke(drawStroke);
g2d.drawLine(x, y, xCenter, yCenter);
// make the arrow head solid even if dash pattern has been specified
g2d.setStroke(lineStroke);
Polygon tmpPoly = new Polygon();
int i1 = 12 + (int) (stroke * 2);
// make the arrow head the same size regardless of the length length
int i2 = 6 + (int) stroke;
tmpPoly.addPoint(x, y);
tmpPoly.addPoint(x + xCor(i1, aDir + .5), y + yCor(i1, aDir + .5));
tmpPoly.addPoint(x + xCor(i2, aDir), y + yCor(i2, aDir));
tmpPoly.addPoint(x + xCor(i1, aDir - .5), y + yCor(i1, aDir - .5));
tmpPoly.addPoint(x, y); // arrow tip
g2d.drawPolygon(tmpPoly);
// Remove this line to leave arrow head unpainted:
g2d.fillPolygon(tmpPoly);
}
public void printPoly(int n, float x[], float y[]) {
Polygon P = new Polygon();
for (int i = 0; i < n; i++) P.addPoint(Math.round(x[i]), Math.round(height - y[i]));
poly_fill.add(P);
poly_fill_color.add(curColor);
}
