public static ConvexPolygon2d shrinkInto(
ConvexPolygon2d polygonP, Point2d referencePointInP, ConvexPolygon2d polygonQ) {
if (polygonQ.hasAtLeastOneVertex() && !polygonQ.hasAtLeastThreeVertices()) {
return new ConvexPolygon2d(polygonQ);
}
ArrayList<Line2d> rays = new ArrayList<Line2d>();
Point2d referencePointInPCopy = new Point2d(referencePointInP);
int leftMostIndexOnPolygonQ = polygonQ.getMinXIndex();
Point2d vertexQ = polygonQ.getVertex(leftMostIndexOnPolygonQ);
int nextVertexQIndex = polygonQ.getNextVertexIndex(leftMostIndexOnPolygonQ);
Point2d nextVertexQ = polygonQ.getVertex(nextVertexQIndex);
int leftMostIndexOnPolygonP = polygonP.getMinXIndex();
Point2d vertexP = polygonP.getVertex(leftMostIndexOnPolygonP);
int nextVertexPIndex = polygonP.getNextVertexIndex(leftMostIndexOnPolygonP);
Point2d nextVertexP = polygonP.getVertex(nextVertexPIndex);
forEachPolygonQ:
for (int i = 0; i < polygonQ.getNumberOfVertices(); i++) {
Vector2d edgeOnQ =
new Vector2d(nextVertexQ.getX() - vertexQ.getX(), nextVertexQ.getY() - vertexQ.getY());
int j = 0;
while (j < polygonP.getNumberOfVertices()) {
Vector2d edgeOnP =
new Vector2d(nextVertexP.getX() - vertexP.getX(), nextVertexP.getY() - vertexP.getY());
double crossProduct = edgeOnQ.getX() * edgeOnP.getY() - edgeOnP.getX() * edgeOnQ.getY();
if (crossProduct <= 0.0) {
referencePointInPCopy.setX(referencePointInP.getX() + vertexQ.getX() - vertexP.getX());
referencePointInPCopy.setY(referencePointInP.getY() + vertexQ.getY() - vertexP.getY());
Line2d ray = new Line2d(referencePointInPCopy, edgeOnQ);
rays.add(ray);
vertexQ = nextVertexQ;
nextVertexQIndex = polygonQ.getNextVertexIndex(nextVertexQIndex);
nextVertexQ = polygonQ.getVertex(nextVertexQIndex);
continue forEachPolygonQ;
} else {
j++;
vertexP = nextVertexP;
nextVertexPIndex = polygonP.getNextVertexIndex(nextVertexPIndex);
nextVertexP = polygonP.getVertex(nextVertexPIndex);
}
}
throw new RuntimeException("Should never get here!!");
}
ConvexPolygonConstructorFromInteriorOfRays convexPolygonConstructorFromInteriorOfRays =
new ConvexPolygonConstructorFromInteriorOfRays();
ConvexPolygon2d polygonToReturn = new ConvexPolygon2d();
boolean foundPolygon =
convexPolygonConstructorFromInteriorOfRays.constructFromInteriorOfRays(
rays, polygonToReturn);
if (foundPolygon) return polygonToReturn;
return null;
}
