Пример #1
0
void
MedialAxis::build(Polylines* polylines)
{
    /*
    // build bounding box (we use it for clipping infinite segments)
    // --> we have no infinite segments
    this->bb = BoundingBox(this->lines);
    */
    
    construct_voronoi(this->lines.begin(), this->lines.end(), &this->vd);
    
    /*
    // DEBUG: dump all Voronoi edges
    {
        for (VD::const_edge_iterator edge = this->vd.edges().begin(); edge != this->vd.edges().end(); ++edge) {
            if (edge->is_infinite()) continue;
            
            Polyline polyline;
            polyline.points.push_back(Point( edge->vertex0()->x(), edge->vertex0()->y() ));
            polyline.points.push_back(Point( edge->vertex1()->x(), edge->vertex1()->y() ));
            polylines->push_back(polyline);
        }
        return;
    }
    */
    
    // collect valid edges (i.e. prune those not belonging to MAT)
    // note: this keeps twins, so it contains twice the number of the valid edges
    this->edges.clear();
    for (VD::const_edge_iterator edge = this->vd.edges().begin(); edge != this->vd.edges().end(); ++edge) {
        // if we only process segments representing closed loops, none if the
        // infinite edges (if any) would be part of our MAT anyway
        if (edge->is_secondary() || edge->is_infinite()) continue;
        this->edges.insert(&*edge);
    }
    
    // count valid segments for each vertex
    std::map< const VD::vertex_type*,std::set<const VD::edge_type*> > vertex_edges;
    std::set<const VD::vertex_type*> entry_nodes;
    for (VD::const_vertex_iterator vertex = this->vd.vertices().begin(); vertex != this->vd.vertices().end(); ++vertex) {
        // get a reference to the list of valid edges originating from this vertex
        std::set<const VD::edge_type*>& edges = vertex_edges[&*vertex];
        
        // get one random edge originating from this vertex
        const VD::edge_type* edge = vertex->incident_edge();
        do {
            if (this->edges.count(edge) > 0)    // only count valid edges
                edges.insert(edge);
            edge = edge->rot_next();            // next edge originating from this vertex
        } while (edge != vertex->incident_edge());
        
        // if there's only one edge starting at this vertex then it's a leaf
        size_t edge_count = edges.size();
        if (edge_count == 1) {
            entry_nodes.insert(&*vertex);
        }
    }
    
    // prune recursively
    while (!entry_nodes.empty()) {
        // get a random entry node
        const VD::vertex_type* v = *entry_nodes.begin();
    
        // get edge starting from v
        assert(!vertex_edges[v].empty());
        const VD::edge_type* edge = *vertex_edges[v].begin();
        
        if (!this->is_valid_edge(*edge)) {
            // if edge is not valid, erase it from edge list
            (void)this->edges.erase(edge);
            (void)this->edges.erase(edge->twin());
            
            // decrement edge counters for the affected nodes
            const VD::vertex_type* v1 = edge->vertex1();
            (void)vertex_edges[v].erase(edge);
            (void)vertex_edges[v1].erase(edge->twin());
            
            // also, check whether the end vertex is a new leaf
            if (vertex_edges[v1].size() == 1) {
                entry_nodes.insert(v1);
            } else if (vertex_edges[v1].empty()) {
                entry_nodes.erase(v1);
            }
        }
        
        // remove node from the set to prevent it from being visited again
        entry_nodes.erase(v);
    }
    
    // iterate through the valid edges to build polylines
    while (!this->edges.empty()) {
        const VD::edge_type& edge = **this->edges.begin();
        
        // start a polyline
        Polyline polyline;
        polyline.points.push_back(Point( edge.vertex0()->x(), edge.vertex0()->y() ));
        polyline.points.push_back(Point( edge.vertex1()->x(), edge.vertex1()->y() ));
        
        // remove this edge and its twin from the available edges
        (void)this->edges.erase(&edge);
        (void)this->edges.erase(edge.twin());
        
        // get next points
        this->process_edge_neighbors(edge, &polyline.points);
        
        // get previous points
        Points pp;
        this->process_edge_neighbors(*edge.twin(), &pp);
        polyline.points.insert(polyline.points.begin(), pp.rbegin(), pp.rend());
        
        // append polyline to result if it's not too small
        if (polyline.length() > this->max_width)
            polylines->push_back(polyline);
    }
}
Пример #2
0
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];
}
Пример #3
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];
}