MotionPlannerGraph*
MotionPlanner::init_graph(int island_idx)
{
    if (this->graphs[island_idx + 1] == NULL) {
        Polygons pp;
        if (island_idx == -1) {
            pp = this->outer;
        } else {
            pp = this->inner[island_idx];
        }
        
        MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph();
        
        // add polygon boundaries as edges
        size_t node_idx = 0;
        Lines lines;
        for (Polygons::const_iterator polygon = pp.begin(); polygon != pp.end(); ++polygon) {
            graph->nodes.push_back(polygon->points.back());
            node_idx++;
            for (Points::const_iterator p = polygon->points.begin(); p != polygon->points.end(); ++p) {
                graph->nodes.push_back(*p);
                double dist = graph->nodes[node_idx-1].distance_to(*p);
                graph->add_edge(node_idx-1, node_idx, dist);
                graph->add_edge(node_idx, node_idx-1, dist);
                node_idx++;
            }
            polygon->lines(&lines);
        }
        
        // add Voronoi edges as internal edges
        {
            typedef voronoi_diagram<double> VD;
            typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
            VD vd;
            t_vd_vertices vd_vertices;
            
            boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
            for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) {
                if (edge->is_infinite()) continue;
                
                const VD::vertex_type* v0 = edge->vertex0();
                const VD::vertex_type* v1 = edge->vertex1();
                Point p0 = Point(v0->x(), v0->y());
                Point p1 = Point(v1->x(), v1->y());
                // contains() should probably be faster than contains(),
                // and should it fail on any boundary points it's not a big problem
                if (island_idx == -1) {
                    if (!this->outer.contains(p0) || !this->outer.contains(p1)) continue;
                } else {
                    if (!this->inner[island_idx].contains(p0) || !this->inner[island_idx].contains(p1)) continue;
                }
                
                t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
                size_t v0_idx;
                if (i_v0 == vd_vertices.end()) {
                    graph->nodes.push_back(p0);
                    v0_idx = node_idx;
                    vd_vertices[v0] = node_idx;
                    node_idx++;
                } else {
                    v0_idx = i_v0->second;
                }
                
                t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1);
                size_t v1_idx;
                if (i_v1 == vd_vertices.end()) {
                    graph->nodes.push_back(p1);
                    v1_idx = node_idx;
                    vd_vertices[v1] = node_idx;
                    node_idx++;
                } else {
                    v1_idx = i_v1->second;
                }
                
                double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
                graph->add_edge(v0_idx, v1_idx, dist);
            }
        }
        
        return graph;
    }
    return this->graphs[island_idx + 1];
}
Beispiel #2
0
MotionPlannerGraph*
MotionPlanner::init_graph(int island_idx)
{
    if (this->graphs[island_idx + 1] == NULL) {
        // if this graph doesn't exist, initialize it
        MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph();
        
        /*  We don't add polygon boundaries as graph edges, because we'd need to connect
            them to the Voronoi-generated edges by recognizing coinciding nodes. */
        
        typedef voronoi_diagram<double> VD;
        VD vd;
        
        // mapping between Voronoi vertices and graph nodes
        typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
        t_vd_vertices vd_vertices;
        
        // get boundaries as lines
        ExPolygonCollection env = this->get_env(island_idx);
        Lines lines = env.lines();
        boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
        
        // traverse the Voronoi diagram and generate graph nodes and edges
        for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) {
            if (edge->is_infinite()) continue;
            
            const VD::vertex_type* v0 = edge->vertex0();
            const VD::vertex_type* v1 = edge->vertex1();
            Point p0 = Point(v0->x(), v0->y());
            Point p1 = Point(v1->x(), v1->y());
            
            // skip edge if any of its endpoints is outside our configuration space
            if (!env.contains_b(p0) || !env.contains_b(p1)) continue;
            
            t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
            size_t v0_idx;
            if (i_v0 == vd_vertices.end()) {
                graph->nodes.push_back(p0);
                vd_vertices[v0] = v0_idx = graph->nodes.size()-1;
            } else {
                v0_idx = i_v0->second;
            }
            
            t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1);
            size_t v1_idx;
            if (i_v1 == vd_vertices.end()) {
                graph->nodes.push_back(p1);
                vd_vertices[v1] = v1_idx = graph->nodes.size()-1;
            } else {
                v1_idx = i_v1->second;
            }
            
            // Euclidean distance is used as weight for the graph edge
            double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
            graph->add_edge(v0_idx, v1_idx, dist);
        }
        
        return graph;
    }
    return this->graphs[island_idx + 1];
}