Welcome to mirror list, hosted at ThFree Co, Russian Federation.

github.com/prusa3d/PrusaSlicer.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/src/libslic3r/MotionPlanner.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/libslic3r/MotionPlanner.cpp')
-rw-r--r--src/libslic3r/MotionPlanner.cpp362
1 files changed, 0 insertions, 362 deletions
diff --git a/src/libslic3r/MotionPlanner.cpp b/src/libslic3r/MotionPlanner.cpp
deleted file mode 100644
index 45a80671c..000000000
--- a/src/libslic3r/MotionPlanner.cpp
+++ /dev/null
@@ -1,362 +0,0 @@
-#include "BoundingBox.hpp"
-#include "MotionPlanner.hpp"
-#include "MutablePriorityQueue.hpp"
-#include "Utils.hpp"
-
-#include <limits> // for numeric_limits
-#include <assert.h>
-
-#include "boost/polygon/voronoi.hpp"
-using boost::polygon::voronoi_builder;
-using boost::polygon::voronoi_diagram;
-
-namespace Slic3r {
-
-MotionPlanner::MotionPlanner(const ExPolygons &islands) : m_initialized(false)
-{
- ExPolygons expp;
- for (const ExPolygon &island : islands) {
- island.simplify(SCALED_EPSILON, &expp);
- for (ExPolygon &island : expp)
- m_islands.emplace_back(MotionPlannerEnv(island));
- expp.clear();
- }
-}
-
-void MotionPlanner::initialize()
-{
- // prevent initialization of empty BoundingBox
- if (m_initialized || m_islands.empty())
- return;
-
- // loop through islands in order to create inner expolygons and collect their contours.
- Polygons outer_holes;
- for (MotionPlannerEnv &island : m_islands) {
- // Generate the internal env boundaries by shrinking the island
- // we'll use these inner rings for motion planning (endpoints of the Voronoi-based
- // graph, visibility check) in order to avoid moving too close to the boundaries.
- island.m_env = ExPolygonCollection(offset_ex(island.m_island, -MP_INNER_MARGIN));
- // Island contours are holes of our external environment.
- outer_holes.push_back(island.m_island.contour);
- }
-
- // Generate a box contour around everyting.
- Polygons contour = offset(get_extents(outer_holes).polygon(), +MP_OUTER_MARGIN*2);
- assert(contour.size() == 1);
- // make expolygon for outer environment
- ExPolygons outer = diff_ex(contour, outer_holes);
- assert(outer.size() == 1);
- // If some of the islands are nested, then the 0th contour is the outer contour due to the order of conversion
- // from Clipper data structure into the Slic3r expolygons inside diff_ex().
- m_outer = MotionPlannerEnv(outer.front());
- m_outer.m_env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN)));
- m_graphs.resize(m_islands.size() + 1);
- m_initialized = true;
-}
-
-Polyline MotionPlanner::shortest_path(const Point &from, const Point &to)
-{
- // If we have an empty configuration space, return a straight move.
- if (m_islands.empty())
- return Polyline(from, to);
-
- // Are both points in the same island?
- int island_idx_from = -1;
- int island_idx_to = -1;
- int island_idx = -1;
- for (MotionPlannerEnv &island : m_islands) {
- int idx = &island - m_islands.data();
- if (island.island_contains(from))
- island_idx_from = idx;
- if (island.island_contains(to))
- island_idx_to = idx;
- if (island_idx_from == idx && island_idx_to == idx) {
- // Since both points are in the same island, is a direct move possible?
- // If so, we avoid generating the visibility environment.
- if (island.m_island.contains(Line(from, to)))
- return Polyline(from, to);
- // Both points are inside a single island, but the straight line crosses the island boundary.
- island_idx = idx;
- break;
- }
- }
-
- // lazy generation of configuration space.
- this->initialize();
-
- // Get environment. If the from / to points do not share an island, then they cross an open space,
- // therefore island_idx == -1 and env will be set to the environment of the empty space.
- const MotionPlannerEnv &env = this->get_env(island_idx);
- if (env.m_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 Polyline(from, to);
- }
-
- // Now check whether points are inside the environment.
- Point inner_from = from;
- Point inner_to = to;
-
- if (island_idx == -1) {
- // The end points do not share the same island. In that case some of the travel
- // will be likely performed inside the empty space.
- // 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 (island_idx_from != -1)
- // The start point is inside some island. Find the closest point at the empty space to start from.
- inner_from = env.nearest_env_point(from, to);
- if (island_idx_to != -1)
- // The start point is inside some island. Find the closest point at the empty space to start from.
- inner_to = env.nearest_env_point(to, inner_from);
- }
-
- // Perform a path search either in the open space, or in a common island of from/to.
- const MotionPlannerGraph &graph = this->init_graph(island_idx);
- // If no path exists without crossing perimeters, returns a straight segment.
- Polyline polyline = graph.shortest_path(inner_from, inner_to);
- polyline.points.insert(polyline.points.begin(), from);
- polyline.points.emplace_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(env.m_env.expolygons, float(+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_ln(Line(from, polyline.points[2]), (Polygons)grown_env).size() == 1)
- polyline.points.erase(polyline.points.begin() + 1);
- }
- if (! grown_env.contains(to))
- while (polyline.points.size() > 2 && intersection_ln(Line(*(polyline.points.end() - 3), to), (Polygons)grown_env).size() == 1)
- polyline.points.erase(polyline.points.end() - 2);
- }
-
- // Perform some quick simplification (simplify_by_visibility() would make this
- // unnecessary, but this is much faster)
- polyline.simplify(MP_INNER_MARGIN/10);
-
- // 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;
-}
-
-const MotionPlannerGraph& MotionPlanner::init_graph(int island_idx)
-{
- // 0th graph is the graph for m_outer. Other graphs are 1 indexed.
- MotionPlannerGraph *graph = m_graphs[island_idx + 1].get();
- if (graph == nullptr) {
- // If this graph doesn't exist, initialize it.
- m_graphs[island_idx + 1] = make_unique<MotionPlannerGraph>();
- graph = m_graphs[island_idx + 1].get();
-
- /* 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.
- std::map<const VD::vertex_type*, size_t> vd_vertices;
- // get boundaries as lines
- const MotionPlannerEnv &env = this->get_env(island_idx);
- Lines lines = env.m_env.lines();
- boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
- // traverse the Voronoi diagram and generate graph nodes and edges
- for (const VD::edge_type &edge : vd.edges()) {
- if (edge.is_infinite())
- continue;
- const VD::vertex_type* v0 = edge.vertex0();
- const VD::vertex_type* v1 = edge.vertex1();
- Point p0(v0->x(), v0->y());
- Point p1(v1->x(), v1->y());
- // Insert only Voronoi edges fully contained in the island.
- //FIXME This test has a terrible O(n^2) time complexity.
- if (env.island_contains_b(p0) && env.island_contains_b(p1)) {
- // Find v0 in the graph, allocate a new node if v0 does not exist in the graph yet.
- auto i_v0 = vd_vertices.find(v0);
- size_t v0_idx;
- if (i_v0 == vd_vertices.end())
- vd_vertices[v0] = v0_idx = graph->add_node(p0);
- else
- v0_idx = i_v0->second;
- // Find v1 in the graph, allocate a new node if v0 does not exist in the graph yet.
- auto i_v1 = vd_vertices.find(v1);
- size_t v1_idx;
- if (i_v1 == vd_vertices.end())
- vd_vertices[v1] = v1_idx = graph->add_node(p1);
- else
- v1_idx = i_v1->second;
- // Euclidean distance is used as weight for the graph edge
- graph->add_edge(v0_idx, v1_idx, (p1 - p0).cast<double>().norm());
- }
- }
- }
-
- return *graph;
-}
-
-// Find a middle point on the path from start_point to end_point with the shortest path.
-static inline size_t nearest_waypoint_index(const Point &start_point, const Points &middle_points, const Point &end_point)
-{
- size_t idx = size_t(-1);
- double dmin = std::numeric_limits<double>::infinity();
- for (const Point &p : middle_points) {
- double d = (p - start_point).cast<double>().norm() + (end_point - p).cast<double>().norm();
- if (d < dmin) {
- idx = &p - middle_points.data();
- dmin = d;
- if (dmin < EPSILON)
- break;
- }
- }
- return idx;
-}
-
-Point MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
-{
- /* In order to ensure that the move between 'from' and the initial env point does
- not violate any of the configuration space boundaries, we limit our search to
- the points that satisfy this condition. */
-
- /* Assume that this method is never called when 'env' contains 'from';
- so 'from' is either inside a hole or outside all contours */
-
- // get the points of the hole containing 'from', if any
- Points pp;
- for (const ExPolygon &ex : m_env.expolygons) {
- for (const Polygon &hole : ex.holes)
- if (hole.contains(from))
- pp = hole;
- if (! pp.empty())
- break;
- }
-
- // If 'from' is not inside a hole, it's outside of all contours, so take all contours' points.
- if (pp.empty())
- for (const ExPolygon &ex : m_env.expolygons)
- append(pp, ex.contour.points);
-
- // Find the candidate result and check that it doesn't cross too many boundaries.
- while (pp.size() > 1) {
- // find the point in pp that is closest to both 'from' and 'to'
- size_t result = nearest_waypoint_index(from, pp, to);
- // as we assume 'from' is outside env, any node will require at least one crossing
- if (intersection_ln(Line(from, pp[result]), m_island).size() > 1) {
- // discard result
- pp.erase(pp.begin() + result);
- } else
- return pp[result];
- }
-
- // if we're here, return last point if any (better than nothing)
- // if we have no points at all, then we have an empty environment and we
- // make this method behave as a no-op (we shouldn't get here by the way)
- return pp.empty() ? from : pp.front();
-}
-
-// Add a new directed edge to the adjacency graph.
-void MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
-{
- // Extend adjacency list until this start node.
- if (m_adjacency_list.size() < from + 1) {
- // Reserve in powers of two to avoid repeated reallocation.
- m_adjacency_list.reserve(std::max<uint32_t>(8, next_highest_power_of_2((uint32_t)(from + 1))));
- // Allocate new empty adjacency vectors.
- m_adjacency_list.resize(from + 1);
- }
- m_adjacency_list[from].emplace_back(Neighbor(node_t(to), weight));
-}
-
-// Dijkstra's shortest path in a weighted graph from node_start to node_end.
-// The returned path contains the end points.
-// If no path exists from node_start to node_end, a straight segment is returned.
-Polyline MotionPlannerGraph::shortest_path(size_t node_start, size_t node_end) const
-{
- // This prevents a crash in case for some reason we got here with an empty adjacency list.
- if (this->empty())
- return Polyline();
-
- // Dijkstra algorithm, previous node of the current node 'u' in the shortest path towards node_start.
- std::vector<node_t> previous(m_adjacency_list.size(), -1);
- std::vector<weight_t> distance(m_adjacency_list.size(), std::numeric_limits<weight_t>::infinity());
- std::vector<size_t> map_node_to_queue_id(m_adjacency_list.size(), size_t(-1));
- distance[node_start] = 0.;
-
- auto queue = make_mutable_priority_queue<node_t, false>(
- [&map_node_to_queue_id](const node_t node, size_t idx) { map_node_to_queue_id[node] = idx; },
- [&distance](const node_t node1, const node_t node2) { return distance[node1] < distance[node2]; });
- queue.reserve(m_adjacency_list.size());
- for (size_t i = 0; i < m_adjacency_list.size(); ++ i)
- queue.push(node_t(i));
-
- while (! queue.empty()) {
- // Get the next node with the lowest distance to node_start.
- node_t u = node_t(queue.top());
- queue.pop();
- map_node_to_queue_id[u] = size_t(-1);
- // Stop searching if we reached our destination.
- if (size_t(u) == node_end)
- break;
- // Visit each edge starting at node u.
- for (const Neighbor& neighbor : m_adjacency_list[u])
- if (map_node_to_queue_id[neighbor.target] != size_t(-1)) {
- weight_t alt = distance[u] + neighbor.weight;
- // If total distance through u is shorter than the previous
- // distance (if any) between node_start and neighbor.target, replace it.
- if (alt < distance[neighbor.target]) {
- distance[neighbor.target] = alt;
- previous[neighbor.target] = u;
- queue.update(map_node_to_queue_id[neighbor.target]);
- }
- }
- }
-
- // In case the end point was not reached, previous[node_end] contains -1
- // and a straight line from node_start to node_end is returned.
- Polyline polyline;
- polyline.points.reserve(m_adjacency_list.size());
- for (node_t vertex = node_t(node_end); vertex != -1; vertex = previous[vertex])
- polyline.points.emplace_back(m_nodes[vertex]);
- polyline.points.emplace_back(m_nodes[node_start]);
- polyline.reverse();
- return polyline;
-}
-
-}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-28 01:27:37 +0300