// this should throw away points that are inside of the other polygon
public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(
FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2) {
ReferenceFrame referenceFrame = polygon1.getReferenceFrame();
polygon2.checkReferenceFrameMatch(referenceFrame);
ConvexPolygon2dAndConnectingEdges polygonAndEdges =
combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon);
if (polygonAndEdges == null) return null; // Return null if not disjoint
FrameConvexPolygon2d frameConvexPolygon2d =
new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d());
FrameLineSegment2d connectingEdge1 =
new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1());
FrameLineSegment2d connectingEdge2 =
new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2());
FrameConvexPolygon2dAndConnectingEdges ret =
new FrameConvexPolygon2dAndConnectingEdges(
polygon1, polygon2, frameConvexPolygon2d, connectingEdge1, connectingEdge2);
return ret;
}
public static boolean combineDisjointPolygons(
FrameConvexPolygon2d polygon1,
FrameConvexPolygon2d polygon2,
FrameConvexPolygon2d combinedPolygonToPack,
FrameLineSegment2d connectingEdge1ToPack,
FrameLineSegment2d connectingEdge2ToPack) {
combinedPolygonToPack.clear(polygon1.getReferenceFrame());
combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge1ToPack);
combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge2ToPack);
boolean success =
combineDisjointPolygons(
polygon1.convexPolygon,
polygon2.convexPolygon,
combinedPolygonToPack.convexPolygon,
connectingEdge1ToPack.lineSegment,
connectingEdge2ToPack.lineSegment);
if (!success) return false;
combinedPolygonToPack.updateFramePoints();
return true;
}
