void
MotionPlanner::initialize()
{
if (this->initialized) return;
if (this->islands.empty()) return; // prevent initialization of empty BoundingBox
// loop through islands in order to create inner expolygons and collect their contours
Polygons outer_holes;
for (std::vector<MotionPlannerEnv>::iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
// generate the internal env boundaries by shrinking the island
// we'll use these inner rings for motion planning (endpoints of the Voronoi-based
// graph, visibility check) in order to avoid moving too close to the boundaries
island->env = offset_ex(island->island, -MP_INNER_MARGIN);
// island contours are holes of our external environment
outer_holes.push_back(island->island.contour);
}
// generate outer contour as bounding box of everything
BoundingBox bb;
for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
bb.merge(contour->bounding_box());
// grow outer contour
Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2);
assert(contour.size() == 1);
// make expolygon for outer environment
ExPolygons outer = diff_ex(contour, outer_holes);
assert(outer.size() == 1);
this->outer.island = outer.front();
this->outer.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN)));
this->graphs.resize(this->islands.size() + 1, NULL);
this->initialized = true;
}
