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; }