From b5f38dd23f413b06e87ebf005a658bf85bb70af6 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Fri, 2 Jun 2017 13:33:19 +0200 Subject: [PATCH] Fixed the "avoid crossing perimeters" bug introduced in Slic3r 1.34.1.24 https://github.com/prusa3d/Slic3r/issues/311 https://github.com/prusa3d/Slic3r/issues/317 https://github.com/prusa3d/Slic3r/issues/323 --- xs/src/libslic3r/ClipperUtils.hpp | 10 +- xs/src/libslic3r/GCode.cpp | 38 +++-- xs/src/libslic3r/GCode.hpp | 2 +- xs/src/libslic3r/MotionPlanner.cpp | 239 ++++++++++++++++------------- xs/src/libslic3r/MotionPlanner.hpp | 56 ++++--- xs/src/libslic3r/Point.cpp | 32 ---- xs/src/libslic3r/Point.hpp | 2 - 7 files changed, 191 insertions(+), 188 deletions(-) diff --git a/xs/src/libslic3r/ClipperUtils.hpp b/xs/src/libslic3r/ClipperUtils.hpp index b14232d90..3c0bec1ec 100644 --- a/xs/src/libslic3r/ClipperUtils.hpp +++ b/xs/src/libslic3r/ClipperUtils.hpp @@ -170,12 +170,18 @@ intersection_pl(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, return _clipper_pl(ClipperLib::ctIntersection, subject, clip, safety_offset_); } -inline Slic3r::Lines -intersection_ln(const Slic3r::Lines &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false) +inline Slic3r::Lines intersection_ln(const Slic3r::Lines &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false) { return _clipper_ln(ClipperLib::ctIntersection, subject, clip, safety_offset_); } +inline Slic3r::Lines intersection_ln(const Slic3r::Line &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false) +{ + Slic3r::Lines lines; + lines.emplace_back(subject); + return _clipper_ln(ClipperLib::ctIntersection, lines, clip, safety_offset_); +} + // union inline Slic3r::Polygons union_(const Slic3r::Polygons &subject, bool safety_offset_ = false) diff --git a/xs/src/libslic3r/GCode.cpp b/xs/src/libslic3r/GCode.cpp index a443c3544..40cdb1393 100644 --- a/xs/src/libslic3r/GCode.cpp +++ b/xs/src/libslic3r/GCode.cpp @@ -26,13 +26,18 @@ namespace Slic3r { -Polyline AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point) +// Plan a travel move while minimizing the number of perimeter crossings. +// point is in unscaled coordinates, in the coordinate system of the current active object +// (set by gcodegen.set_origin()). +Polyline AvoidCrossingPerimeters::travel_to(const GCode &gcodegen, const Point &point) { + // If use_external, then perform the path planning in the world coordinate system (correcting for the gcodegen offset). + // Otherwise perform the path planning in the coordinate system of the active object. bool use_external = this->use_external_mp || this->use_external_mp_once; - Point scaled_origin = use_external ? Point(0, 0) : Point::new_scale(gcodegen.origin().x, gcodegen.origin().y); + Point scaled_origin = use_external ? Point::new_scale(gcodegen.origin().x, gcodegen.origin().y) : Point(0, 0); Polyline result = (use_external ? m_external_mp.get() : m_layer_mp.get())-> shortest_path(gcodegen.last_pos() + scaled_origin, point + scaled_origin); - if (! use_external) + if (use_external) result.translate(scaled_origin.negative()); return result; } @@ -489,25 +494,18 @@ bool GCode::do_export(FILE *file, Print &print) // Initialize a motion planner for object-to-object travel moves. if (print.config.avoid_crossing_perimeters.value) { - //coord_t distance_from_objects = coord_t(scale_(1.)); - // Compute the offsetted convex hull for each object and repeat it for each copy. - Polygons islands_p; - for (const PrintObject *object : print.objects) { - // Discard objects only containing thin walls (offset would fail on an empty polygon). - Polygons polygons; + // Collect outer contours of all objects over all layers. + // Discard objects only containing thin walls (offset would fail on an empty polygon). + Polygons islands; + for (const PrintObject *object : print.objects) for (const Layer *layer : object->layers) for (const ExPolygon &expoly : layer->slices.expolygons) - polygons.push_back(expoly.contour); - if (! polygons.empty()) { - // Translate convex hull for each object copy and append it to the islands array. - for (const Point © : object->_shifted_copies) - for (Polygon poly : polygons) { - poly.translate(copy); - islands_p.emplace_back(std::move(poly)); + for (const Point © : object->_shifted_copies) { + islands.emplace_back(expoly.contour); + islands.back().translate(copy); } - } - } - m_avoid_crossing_perimeters.init_external_mp(union_ex(islands_p)); + //FIXME Mege the islands in parallel. + m_avoid_crossing_perimeters.init_external_mp(union_ex(islands)); } // Calculate wiping points if needed @@ -1022,7 +1020,7 @@ void GCode::process_layer( // Extrude brim with the extruder of the 1st region. if (! m_brim_done) { - this->set_origin(0.f, 0.f); + this->set_origin(0., 0.); m_avoid_crossing_perimeters.use_external_mp = true; for (const ExtrusionEntity *ee : print.brim.entities) gcode += this->extrude_loop(*dynamic_cast(ee), "brim", m_config.support_material_speed.value); diff --git a/xs/src/libslic3r/GCode.hpp b/xs/src/libslic3r/GCode.hpp index 548b3bcda..baf37e9c6 100644 --- a/xs/src/libslic3r/GCode.hpp +++ b/xs/src/libslic3r/GCode.hpp @@ -42,7 +42,7 @@ public: void init_external_mp(const ExPolygons &islands) { m_external_mp = Slic3r::make_unique(islands); } void init_layer_mp(const ExPolygons &islands) { m_layer_mp = Slic3r::make_unique(islands); } - Polyline travel_to(GCode &gcodegen, Point point); + Polyline travel_to(const GCode &gcodegen, const Point &point); private: std::unique_ptr m_external_mp; diff --git a/xs/src/libslic3r/MotionPlanner.cpp b/xs/src/libslic3r/MotionPlanner.cpp index edd3bc8f2..e8605d68c 100644 --- a/xs/src/libslic3r/MotionPlanner.cpp +++ b/xs/src/libslic3r/MotionPlanner.cpp @@ -12,13 +12,13 @@ using boost::polygon::voronoi_diagram; namespace Slic3r { -MotionPlanner::MotionPlanner(const ExPolygons &islands) : initialized(false) +MotionPlanner::MotionPlanner(const ExPolygons &islands) : m_initialized(false) { ExPolygons expp; for (const ExPolygon &island : islands) { island.simplify(SCALED_EPSILON, &expp); for (ExPolygon &island : expp) - this->islands.push_back(MotionPlannerEnv(island)); + m_islands.emplace_back(MotionPlannerEnv(island)); expp.clear(); } } @@ -26,18 +26,18 @@ MotionPlanner::MotionPlanner(const ExPolygons &islands) : initialized(false) void MotionPlanner::initialize() { // prevent initialization of empty BoundingBox - if (this->initialized || this->islands.empty()) + if (m_initialized || m_islands.empty()) return; - - // loop through islands in order to create inner expolygons and collect their contours + + // loop through islands in order to create inner expolygons and collect their contours. Polygons outer_holes; - for (MotionPlannerEnv &island : this->islands) { - // generate the internal env boundaries by shrinking the island + 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.env = ExPolygonCollection(offset_ex(island.island, -MP_INNER_MARGIN)); - // island contours are holes of our external environment - outer_holes.push_back(island.island.contour); + // 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. @@ -46,30 +46,37 @@ void MotionPlanner::initialize() // make expolygon for outer environment ExPolygons outer = diff_ex(contour, outer_holes); assert(outer.size() == 1); - //FIXME What if some of the islands are nested? Then the front contour may not be the outmost contour! - this->outer.island = outer.front(); - this->outer.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN))); - this->graphs.resize(this->islands.size() + 1); - this->initialized = true; + // 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 (this->islands.empty()) + if (m_islands.empty()) return Line(from, to); // Are both points in the same island? - int island_idx = -1; - for (MotionPlannerEnv &island : islands) { - if (island.island_bbox.contains(from) && island.island_bbox.contains(to) && - island.island.contains(from) && island.island.contains(to)) { + 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.island.contains(Line(from, to))) + if (island.m_island.contains(Line(from, to))) return Line(from, to); // Both points are inside a single island, but the straight line crosses the island boundary. - island_idx = &island - this->islands.data(); + island_idx = idx; break; } } @@ -77,9 +84,10 @@ Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) // lazy generation of configuration space. this->initialize(); - // get environment + // 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.env.expolygons.empty()) { + 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 Line(from, to); @@ -90,32 +98,32 @@ Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) 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 (! env.island_bbox.contains(inner_from) || ! env.island.contains(inner_from)) { - // Find the closest inner point to start from. + 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 (! env.island_bbox.contains(inner_to) || ! env.island.contains(inner_to)) { - // Find the closest inner point to start from. + 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 actual path search + + // 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); - Polyline polyline = graph.shortest_path(graph.find_closest_node(inner_from), graph.find_closest_node(inner_to)); - + // 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.push_back(to); + 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.env.expolygons, +SCALED_EPSILON)); + 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 @@ -128,14 +136,17 @@ Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) 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((Lines)Line(from, polyline.points[2]), grown_env).size() == 1) + while (polyline.points.size() > 2 && intersection_ln(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_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) + if (! grown_env.contains(to)) + while (polyline.points.size() > 2 && intersection_ln(Line(*(polyline.points.end() - 3), to), 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 @@ -169,64 +180,73 @@ Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) const MotionPlannerGraph& MotionPlanner::init_graph(int island_idx) { - if (! this->graphs[island_idx + 1]) { - // if this graph doesn't exist, initialize it - this->graphs[island_idx + 1] = make_unique(); - MotionPlannerGraph* graph = this->graphs[island_idx + 1].get(); + // 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(); + 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 VD; VD vd; - - // mapping between Voronoi vertices and graph nodes - typedef std::map t_vd_vertices; - t_vd_vertices vd_vertices; - + // Mapping between Voronoi vertices and graph nodes. + std::map vd_vertices; // get boundaries as lines const MotionPlannerEnv &env = this->get_env(island_idx); - Lines lines = env.env.lines(); + 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 (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 + 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)) 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; + 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, p0.distance_to(p1)); } - - 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 *this->graphs[island_idx + 1].get(); + 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::infinity(); + for (const Point &p : middle_points) { + double d = start_point.distance_to(p) + p.distance_to(end_point); + 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 @@ -240,7 +260,7 @@ Point MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) co // get the points of the hole containing 'from', if any Points pp; - for (const ExPolygon &ex : this->env.expolygons) { + for (const ExPolygon &ex : m_env.expolygons) { for (const Polygon &hole : ex.holes) if (hole.contains(from)) pp = hole; @@ -248,19 +268,17 @@ Point MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) co break; } - /* If 'from' is not inside a hole, it's outside of all contours, so take all - contours' points */ + // 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 : this->env.expolygons) + 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() >= 2) { + // 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 = from.nearest_waypoint_index(pp, 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((Lines)Line(from, pp[result]), this->island).size() > 1) { + if (intersection_ln(Line(from, pp[result]), m_island).size() > 1) { // discard result pp.erase(pp.begin() + result); } else @@ -277,34 +295,35 @@ Point MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) co void MotionPlannerGraph::add_edge(size_t from, size_t to, double weight) { // Extend adjacency list until this start node. - if (this->adjacency_list.size() < from + 1) { + if (m_adjacency_list.size() < from + 1) { // Reserve in powers of two to avoid repeated reallocation. - this->adjacency_list.reserve(std::max(8, next_highest_power_of_2(from + 1))); + m_adjacency_list.reserve(std::max(8, next_highest_power_of_2(from + 1))); // Allocate new empty adjacency vectors. - this->adjacency_list.resize(from + 1); + m_adjacency_list.resize(from + 1); } - this->adjacency_list[from].emplace_back(Neighbor(node_t(to), weight)); + 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->adjacency_list.empty()) + if (this->empty()) return Polyline(); // Dijkstra algorithm, previous node of the current node 'u' in the shortest path towards node_start. - std::vector previous(this->adjacency_list.size(), -1); - std::vector distance(this->adjacency_list.size(), std::numeric_limits::infinity()); - std::vector map_node_to_queue_id(this->adjacency_list.size(), size_t(-1)); + std::vector previous(m_adjacency_list.size(), -1); + std::vector distance(m_adjacency_list.size(), std::numeric_limits::infinity()); + std::vector map_node_to_queue_id(m_adjacency_list.size(), size_t(-1)); distance[node_start] = 0.; auto queue = make_mutable_priority_queue( - [&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(this->adjacency_list.size()); - for (size_t i = 0; i < this->adjacency_list.size(); ++ i) + [&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()) { @@ -316,7 +335,7 @@ Polyline MotionPlannerGraph::shortest_path(size_t node_start, size_t node_end) c if (u == node_end) break; // Visit each edge starting at node u. - for (const Neighbor& neighbor : this->adjacency_list[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 @@ -329,11 +348,13 @@ Polyline MotionPlannerGraph::shortest_path(size_t node_start, size_t node_end) c } } + // 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(previous.size()); + polyline.points.reserve(m_adjacency_list.size()); for (node_t vertex = node_t(node_end); vertex != -1; vertex = previous[vertex]) - polyline.points.push_back(this->nodes[vertex]); - polyline.points.push_back(this->nodes[node_start]); + polyline.points.emplace_back(m_nodes[vertex]); + polyline.points.emplace_back(m_nodes[node_start]); polyline.reverse(); return polyline; } diff --git a/xs/src/libslic3r/MotionPlanner.hpp b/xs/src/libslic3r/MotionPlanner.hpp index add1f79cf..e912f2fb4 100644 --- a/xs/src/libslic3r/MotionPlanner.hpp +++ b/xs/src/libslic3r/MotionPlanner.hpp @@ -23,34 +23,45 @@ class MotionPlannerEnv friend class MotionPlanner; public: - ExPolygon island; - BoundingBox island_bbox; - ExPolygonCollection env; MotionPlannerEnv() {}; - MotionPlannerEnv(const ExPolygon &island) : island(island), island_bbox(get_extents(island)) {}; + MotionPlannerEnv(const ExPolygon &island) : m_island(island), m_island_bbox(get_extents(island)) {}; Point nearest_env_point(const Point &from, const Point &to) const; + bool island_contains(const Point &pt) const + { return m_island_bbox.contains(pt) && m_island.contains(pt); } + bool island_contains_b(const Point &pt) const + { return m_island_bbox.contains(pt) && m_island.contains_b(pt); } + +private: + ExPolygon m_island; + BoundingBox m_island_bbox; + // Region, where the travel is allowed. + ExPolygonCollection m_env; }; +// A 2D directed graph for searching a shortest path using the famous Dijkstra algorithm. class MotionPlannerGraph -{ - friend class MotionPlanner; - +{ +public: + // Add a directed edge into the graph. + size_t add_node(const Point &p) { m_nodes.emplace_back(p); return m_nodes.size() - 1; } + void add_edge(size_t from, size_t to, double weight); + size_t find_closest_node(const Point &point) const { return point.nearest_point_index(m_nodes); } + + bool empty() const { return m_adjacency_list.empty(); } + Polyline shortest_path(size_t from, size_t to) const; + Polyline shortest_path(const Point &from, const Point &to) const + { return this->shortest_path(this->find_closest_node(from), this->find_closest_node(to)); } + private: typedef int node_t; typedef double weight_t; struct Neighbor { + Neighbor(node_t target, weight_t weight) : target(target), weight(weight) {} node_t target; weight_t weight; - Neighbor(node_t arg_target, weight_t arg_weight) : target(arg_target), weight(arg_weight) {} }; - typedef std::vector> adjacency_list_t; - adjacency_list_t adjacency_list; - -public: - Points nodes; - void add_edge(size_t from, size_t to, double weight); - size_t find_closest_node(const Point &point) const { return point.nearest_point_index(this->nodes); } - Polyline shortest_path(size_t from, size_t to) const; + Points m_nodes; + std::vector> m_adjacency_list; }; class MotionPlanner @@ -60,18 +71,19 @@ public: ~MotionPlanner() {} Polyline shortest_path(const Point &from, const Point &to); - size_t islands_count() const { return this->islands.size(); } + size_t islands_count() const { return m_islands.size(); } private: - bool initialized; - std::vector islands; - MotionPlannerEnv outer; - std::vector> graphs; + bool m_initialized; + std::vector m_islands; + MotionPlannerEnv m_outer; + // 0th graph is the graph for m_outer. Other graphs are 1 indexed. + std::vector> m_graphs; void initialize(); const MotionPlannerGraph& init_graph(int island_idx); const MotionPlannerEnv& get_env(int island_idx) const - { return (island_idx == -1) ? this->outer : this->islands[island_idx]; } + { return (island_idx == -1) ? m_outer : m_islands[island_idx]; } }; } diff --git a/xs/src/libslic3r/Point.cpp b/xs/src/libslic3r/Point.cpp index a587e3406..ad25ba91a 100644 --- a/xs/src/libslic3r/Point.cpp +++ b/xs/src/libslic3r/Point.cpp @@ -119,29 +119,6 @@ int Point::nearest_point_index(const PointConstPtrs &points) const return idx; } -/* This method finds the point that is closest to both this point and the supplied one */ -size_t Point::nearest_waypoint_index(const Points &points, const Point &dest) const -{ - size_t idx = size_t(-1); - double d2min = std::numeric_limits::infinity(); // double because long is limited to 2147483647 on some platforms and it's not enough - - for (const Point &p : points) { - double d2 = - // distance from this to candidate - sqr(this->x - p.x) + sqr(this->y - p.y) + - // distance from candidate to dest - sqr(p.x - dest.x) + sqr(p.y - dest.y); - if (d2 < d2min) { - idx = &p - points.data(); - d2min = d2; - if (d2min < EPSILON) - break; - } - } - - return idx; -} - int Point::nearest_point_index(const PointPtrs &points) const { @@ -161,15 +138,6 @@ Point::nearest_point(const Points &points, Point* point) const return true; } -bool -Point::nearest_waypoint(const Points &points, const Point &dest, Point* point) const -{ - int idx = this->nearest_waypoint_index(points, dest); - if (idx == -1) return false; - *point = points.at(idx); - return true; -} - /* distance to the closest point of line */ double Point::distance_to(const Line &line) const diff --git a/xs/src/libslic3r/Point.hpp b/xs/src/libslic3r/Point.hpp index 3cdc99bd3..8f11bd50a 100644 --- a/xs/src/libslic3r/Point.hpp +++ b/xs/src/libslic3r/Point.hpp @@ -60,9 +60,7 @@ class Point int nearest_point_index(const Points &points) const; int nearest_point_index(const PointConstPtrs &points) const; int nearest_point_index(const PointPtrs &points) const; - size_t nearest_waypoint_index(const Points &points, const Point &point) const; bool nearest_point(const Points &points, Point* point) const; - bool nearest_waypoint(const Points &points, const Point &dest, Point* point) const; double distance_to(const Point &point) const { return sqrt(distance_to_sq(point)); } double distance_to_sq(const Point &point) const { double dx = double(point.x - this->x); double dy = double(point.y - this->y); return dx*dx + dy*dy; } double distance_to(const Line &line) const;