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); } }
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]; }
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]; }