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