예제 #1
0
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;
}
예제 #2
0
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));
}
예제 #3
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;
}