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