C++ Polyline::simplify_by_visibility方法代码示例

Polyline
MotionPlanner::shortest_path(const Point &from, const Point &to)
{
    // lazy generation of configuration space
    if (!this->initialized) this->initialize();
    
    // if we have an empty configuration space, return a straight move
    if (this->islands.empty()) {
        Polyline p;
        p.points.push_back(from);
        p.points.push_back(to);
        return p;
    }
    
    // Are both points in the same island?
    int island_idx = -1;
    for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
        if (island->contains(from) && island->contains(to)) {
            // since both points are in the same island, is a direct move possible?
            // if so, we avoid generating the visibility environment
            if (island->contains(Line(from, to))) {
                Polyline p;
                p.points.push_back(from);
                p.points.push_back(to);
                return p;
            }
            island_idx = island - this->islands.begin();
            break;
        }
    }
    
    // get environment
    ExPolygonCollection env = this->get_env(island_idx);
    if (env.expolygons.empty()) {
        // if this environment is empty (probably because it's too small), perform straight move
        // and avoid running the algorithms on empty dataset
        Polyline p;
        p.points.push_back(from);
        p.points.push_back(to);
        return p; // bye bye
    }
    
    // Now check whether points are inside the environment.
    Point inner_from    = from;
    Point inner_to      = to;

    if (!env.contains(from)) {
        // Find the closest inner point to start from.
        inner_from = this->nearest_env_point(env, from, to);
    }
    if (!env.contains(to)) {
        // Find the closest inner point to start from.
        inner_to = this->nearest_env_point(env, to, inner_from);
    }
    
    // perform actual path search
    MotionPlannerGraph* graph = this->init_graph(island_idx);
    Polyline polyline = graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to));
    
    polyline.points.insert(polyline.points.begin(), from);
    polyline.points.push_back(to);
    
    {
        // grow our environment slightly in order for simplify_by_visibility()
        // to work best by considering moves on boundaries valid as well
        ExPolygonCollection grown_env;
        offset(env, &grown_env.expolygons, +SCALED_EPSILON);
        
        // remove unnecessary vertices
        polyline.simplify_by_visibility(grown_env);
    }
    
    /*
        SVG svg("shortest_path.svg");
        svg.draw(this->outer);
        svg.arrows = false;
        for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) {
            Point a = graph->nodes[it - graph->adjacency_list.begin()];
            for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) {
                Point b = graph->nodes[n->target];
                svg.draw(Line(a, b));
            }
        }
        svg.arrows = true;
        svg.draw(from);
        svg.draw(inner_from, "red");
        svg.draw(to);
        svg.draw(inner_to, "red");
        svg.draw(*polyline, "red");
        svg.Close();
    */
    
    return polyline;
}