ExPolygonCollection
MotionPlanner::get_env(int island_idx) const
{
if (island_idx == -1) {
return ExPolygonCollection(this->outer);
} else {
return this->inner[island_idx];
}
}
void
MotionPlanner::initialize()
{
if (this->initialized) return;
if (this->islands.empty()) return; // prevent initialization of empty BoundingBox
ExPolygons expp;
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
island->simplify(SCALED_EPSILON, expp);
}
this->islands = expp;
// loop through islands in order to create inner expolygons and collect their contours
this->inner.reserve(this->islands.size());
Polygons outer_holes;
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
this->inner.push_back(ExPolygonCollection());
offset(*island, &this->inner.back().expolygons, -MP_INNER_MARGIN);
outer_holes.push_back(island->contour);
}
// grow island contours in order to prepare holes of the outer environment
// This is actually wrong because it might merge contours that are close,
// thus confusing the island check in shortest_path() below
//offset(outer_holes, &outer_holes, +MP_OUTER_MARGIN);
// generate outer contour as bounding box of everything
Points points;
for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
points.insert(points.end(), contour->points.begin(), contour->points.end());
BoundingBox bb(points);
// grow outer contour
Polygons contour;
offset(bb.polygon(), &contour, +MP_OUTER_MARGIN);
assert(contour.size() == 1);
// make expolygon for outer environment
ExPolygons outer;
diff(contour, outer_holes, &outer);
assert(outer.size() == 1);
this->outer = outer.front();
this->graphs.resize(this->islands.size() + 1, NULL);
this->initialized = true;
}
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;
}
