void
MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline)
{
if (!this->initialized) this->initialize();
if (this->islands.empty()) {
polyline->points.push_back(from);
polyline->points.push_back(to);
return;
}
// 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->points.push_back(from);
polyline->points.push_back(to);
return;
}
island_idx = island - this->islands.begin();
break;
}
}
// Now check whether points are inside the environment.
Point inner_from = from;
Point inner_to = to;
bool from_is_inside, to_is_inside;
if (island_idx == -1) {
if (!(from_is_inside = this->outer.contains(from))) {
// Find the closest inner point to start from.
from.nearest_point(this->outer, &inner_from);
}
if (!(to_is_inside = this->outer.contains(to))) {
// Find the closest inner point to start from.
to.nearest_point(this->outer, &inner_to);
}
} else {
if (!(from_is_inside = this->inner[island_idx].contains(from))) {
// Find the closest inner point to start from.
from.nearest_point(this->inner[island_idx], &inner_from);
}
if (!(to_is_inside = this->inner[island_idx].contains(to))) {
// Find the closest inner point to start from.
to.nearest_point(this->inner[island_idx], &inner_to);
}
}
// perform actual path search
MotionPlannerGraph* graph = this->init_graph(island_idx);
graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to), polyline);
polyline->points.insert(polyline->points.begin(), from);
polyline->points.push_back(to);
}
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];
}
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;
}
Polyline
MotionPlanner::shortest_path(const Point &from, const Point &to)
{
// if we have an empty configuration space, return a straight move
if (this->islands.empty())
return Line(from, to);
// Are both points in the same island?
int island_idx = -1;
for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
if (island->island.contains(from) && island->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->island.contains(Line(from, to)))
return Line(from, to);
island_idx = island - this->islands.begin();
break;
}
}
// lazy generation of configuration space
this->initialize();
// get environment
MotionPlannerEnv env = this->get_env(island_idx);
if (env.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
return Line(from, to);
}
// Now check whether points are inside the environment.
Point inner_from = from;
Point inner_to = to;
if (island_idx == -1) {
// TODO: instead of using the nearest_env_point() logic, we should
// create a temporary graph where we connect 'from' and 'to' to the
// nodes which don't require more than one crossing, and let Dijkstra
// figure out the entire path - this should also replace the call to
// find_node() below
if (!env.island.contains(inner_from)) {
// Find the closest inner point to start from.
inner_from = env.nearest_env_point(from, to);
}
if (!env.island.contains(inner_to)) {
// Find the closest inner point to start from.
inner_to = env.nearest_env_point(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_ex((Polygons)env.env, +SCALED_EPSILON));
if (island_idx == -1) {
/* If 'from' or 'to' are not inside our env, they were connected using the
nearest_env_point() search which maybe produce ugly paths since it does not
include the endpoint in the Dijkstra search; the simplify_by_visibility()
call below will not work in many cases where the endpoint is not contained in
grown_env (whose contour was arbitrarily constructed with MP_OUTER_MARGIN,
which may not be enough for, say, including a skirt point). So we prune
the extra points manually. */
if (!grown_env.contains(from)) {
// delete second point while the line connecting first to third crosses the
// boundaries as many times as the current first to second
while (polyline.points.size() > 2 && intersection((Lines)Line(from, polyline.points[2]), grown_env).size() == 1) {
polyline.points.erase(polyline.points.begin() + 1);
}
}
if (!grown_env.contains(to)) {
while (polyline.points.size() > 2 && intersection((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) {
polyline.points.erase(polyline.points.end() - 2);
}
}
}
// remove unnecessary vertices
// Note: this is computationally intensive and does not look very necessary
// now that we prune the endpoints with the logic above,
// so we comment it for now until a good test case arises
//polyline.simplify_by_visibility(grown_env);
/*
SVG svg("shortest_path.svg");
svg.draw(grown_env.expolygons);
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;
}
