Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}