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