void
ExPolygonCollection::simplify(double tolerance)
{
ExPolygons expp;
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
it->simplify(tolerance, expp);
}
this->expolygons = expp;
}
MotionPlanner::MotionPlanner(const ExPolygons &islands)
: initialized(false)
{
ExPolygons expp;
for (ExPolygons::const_iterator island = islands.begin(); island != islands.end(); ++island)
island->simplify(SCALED_EPSILON, &expp);
for (ExPolygons::const_iterator island = expp.begin(); island != expp.end(); ++island)
this->islands.push_back(MotionPlannerEnv(*island));
}
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;
}