diff --git a/xs/MANIFEST b/xs/MANIFEST index aa83d43cf..8983907a7 100644 --- a/xs/MANIFEST +++ b/xs/MANIFEST @@ -395,6 +395,7 @@ src/libslic3r/MotionPlanner.cpp src/libslic3r/MotionPlanner.hpp src/libslic3r/MultiPoint.cpp src/libslic3r/MultiPoint.hpp +src/libslic3r/MutablePriorityQueue.hpp src/libslic3r/PerimeterGenerator.cpp src/libslic3r/PerimeterGenerator.hpp src/libslic3r/PlaceholderParser.cpp diff --git a/xs/src/libslic3r/GCode.cpp b/xs/src/libslic3r/GCode.cpp index 2a06feb29..ef8ca2d6b 100644 --- a/xs/src/libslic3r/GCode.cpp +++ b/xs/src/libslic3r/GCode.cpp @@ -24,65 +24,16 @@ #include namespace Slic3r { - -AvoidCrossingPerimeters::AvoidCrossingPerimeters() - : use_external_mp(false), use_external_mp_once(false), disable_once(true), - _external_mp(NULL), _layer_mp(NULL) -{ -} - -AvoidCrossingPerimeters::~AvoidCrossingPerimeters() -{ - if (this->_external_mp != NULL) - delete this->_external_mp; - if (this->_layer_mp != NULL) - delete this->_layer_mp; -} - -void -AvoidCrossingPerimeters::init_external_mp(const ExPolygons &islands) +Polyline AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point) { - if (this->_external_mp != NULL) - delete this->_external_mp; - - this->_external_mp = new MotionPlanner(islands); -} - -void -AvoidCrossingPerimeters::init_layer_mp(const ExPolygons &islands) -{ - if (this->_layer_mp != NULL) - delete this->_layer_mp; - - this->_layer_mp = new MotionPlanner(islands); -} - -Polyline -AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point) -{ - if (this->use_external_mp || this->use_external_mp_once) { - // get current origin set in gcodegen - // (the one that will be used to translate the G-code coordinates by) - Point scaled_origin = Point::new_scale(gcodegen.origin().x, gcodegen.origin().y); - - // represent last_pos in absolute G-code coordinates - Point last_pos = gcodegen.last_pos(); - last_pos.translate(scaled_origin); - - // represent point in absolute G-code coordinates - point.translate(scaled_origin); - - // calculate path - Polyline travel = this->_external_mp->shortest_path(last_pos, point); - //exit(0); - // translate the path back into the shifted coordinate system that gcodegen - // is currently using for writing coordinates - travel.translate(scaled_origin.negative()); - return travel; - } else { - return this->_layer_mp->shortest_path(gcodegen.last_pos(), point); - } + 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); + 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) + result.translate(scaled_origin.negative()); + return result; } std::string OozePrevention::pre_toolchange(GCode &gcodegen) @@ -201,12 +152,6 @@ inline void writeln(FILE *file, const std::string &what) fprintf(file, "\n"); } -// Older compilers do not provide a std::make_unique template. Provide a simple one. -template -std::unique_ptr make_unique(Args&&... args) { - return std::unique_ptr(new T(std::forward(args)...)); -} - bool GCode::do_export(FILE *file, Print &print) { // How many times will be change_layer() called? @@ -544,6 +489,7 @@ void GCode::process_layer(FILE *file, const Print &print, const Layer &layer, co const SupportLayer *support_layer = dynamic_cast(&layer); // Check whether it is possible to apply the spiral vase logic for this layer. + // Just a reminder: A spiral vase mode is allowed for a single object, single material print only. if (m_spiral_vase) { bool enable = (layer.id() > 0 || print.config.brim_width.value == 0.) && (layer.id() >= print.config.skirt_height.value && ! print.has_infinite_skirt()); if (enable) { @@ -888,11 +834,9 @@ std::string GCode::change_layer(const Layer &layer) } // avoid computing islands and overhangs if they're not needed - if (m_config.avoid_crossing_perimeters) { - ExPolygons islands = union_ex(layer.slices, true); - m_avoid_crossing_perimeters.init_layer_mp(islands); - } - + if (m_config.avoid_crossing_perimeters) + m_avoid_crossing_perimeters.init_layer_mp(union_ex(layer.slices, true)); + if (m_layer_count > 0) gcode += m_writer.update_progress(m_layer_index, m_layer_count); diff --git a/xs/src/libslic3r/GCode.hpp b/xs/src/libslic3r/GCode.hpp index 7899d97c9..c8e941874 100644 --- a/xs/src/libslic3r/GCode.hpp +++ b/xs/src/libslic3r/GCode.hpp @@ -34,15 +34,17 @@ public: // we enable it by default for the first travel move in print bool disable_once; - AvoidCrossingPerimeters(); - ~AvoidCrossingPerimeters(); - void init_external_mp(const ExPolygons &islands); - void init_layer_mp(const ExPolygons &islands); + AvoidCrossingPerimeters() : use_external_mp(false), use_external_mp_once(false), disable_once(true) {} + ~AvoidCrossingPerimeters() {} + + 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); - + private: - MotionPlanner* _external_mp; - MotionPlanner* _layer_mp; + std::unique_ptr m_external_mp; + std::unique_ptr m_layer_mp; }; class OozePrevention { diff --git a/xs/src/libslic3r/MotionPlanner.cpp b/xs/src/libslic3r/MotionPlanner.cpp index 9fc1695c0..edd3bc8f2 100644 --- a/xs/src/libslic3r/MotionPlanner.cpp +++ b/xs/src/libslic3r/MotionPlanner.cpp @@ -1,5 +1,8 @@ #include "BoundingBox.hpp" #include "MotionPlanner.hpp" +#include "MutablePriorityQueue.hpp" +#include "Utils.hpp" + #include // for numeric_limits #include @@ -9,103 +12,73 @@ using boost::polygon::voronoi_diagram; namespace Slic3r { -MotionPlanner::MotionPlanner(const ExPolygons &islands) - : initialized(false) +MotionPlanner::MotionPlanner(const ExPolygons &islands) : initialized(false) { ExPolygons expp; - for (ExPolygons::const_iterator island = islands.begin(); island != islands.end(); ++island) - island->simplify(SCALED_EPSILON, &expp); - - for (ExPolygons::const_iterator island = expp.begin(); island != expp.end(); ++island) - this->islands.push_back(MotionPlannerEnv(*island)); + for (const ExPolygon &island : islands) { + island.simplify(SCALED_EPSILON, &expp); + for (ExPolygon &island : expp) + this->islands.push_back(MotionPlannerEnv(island)); + expp.clear(); + } } -MotionPlanner::~MotionPlanner() +void MotionPlanner::initialize() { - for (std::vector::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph) - delete *graph; -} - -size_t -MotionPlanner::islands_count() const -{ - return this->islands.size(); -} - -void -MotionPlanner::initialize() -{ - if (this->initialized) return; - if (this->islands.empty()) return; // prevent initialization of empty BoundingBox + // prevent initialization of empty BoundingBox + if (this->initialized || this->islands.empty()) + return; // loop through islands in order to create inner expolygons and collect their contours Polygons outer_holes; - for (std::vector::iterator island = this->islands.begin(); island != this->islands.end(); ++island) { + for (MotionPlannerEnv &island : this->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 = offset_ex(island->island, -MP_INNER_MARGIN); - + 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); + outer_holes.push_back(island.island.contour); } - // generate outer contour as bounding box of everything - BoundingBox bb; - for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour) - bb.merge(contour->bounding_box()); - - // grow outer contour - Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2); + // 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); + //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, NULL); + this->graphs.resize(this->islands.size() + 1); this->initialized = true; } -const MotionPlannerEnv& -MotionPlanner::get_env(int island_idx) const +Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) { - if (island_idx == -1) { - return this->outer; - } else { - return this->islands[island_idx]; - } -} - -Polyline -MotionPlanner::shortest_path(const Point &from, const Point &to) -{ - // if we have an empty configuration space, return a straight move + // 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::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))) + for (MotionPlannerEnv &island : islands) { + if (island.island_bbox.contains(from) && island.island_bbox.contains(to) && + 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(); + // Both points are inside a single island, but the straight line crosses the island boundary. + island_idx = &island - this->islands.data(); break; } } - // lazy generation of configuration space + // lazy generation of configuration space. this->initialize(); - + // get environment - MotionPlannerEnv env = this->get_env(island_idx); + const 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 @@ -122,19 +95,19 @@ MotionPlanner::shortest_path(const Point &from, const Point &to) // 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)) { + if (! env.island_bbox.contains(inner_from) || ! 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)) { + if (! env.island_bbox.contains(inner_to) || ! 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)); + 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)); polyline.points.insert(polyline.points.begin(), from); polyline.points.push_back(to); @@ -152,17 +125,15 @@ MotionPlanner::shortest_path(const Point &from, const Point &to) 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)) { + 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((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_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) { + if (! grown_env.contains(to)) { + while (polyline.points.size() > 2 && intersection_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) polyline.points.erase(polyline.points.end() - 2); - } } } @@ -178,7 +149,7 @@ MotionPlanner::shortest_path(const Point &from, const Point &to) 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::const_iterator n = it->begin(); n != it->end(); ++n) { + for (std::vector::const_iterator n = it->begin(); n != it->end(); ++n) { Point b = graph->nodes[n->target]; svg.draw(Line(a, b)); } @@ -196,12 +167,12 @@ MotionPlanner::shortest_path(const Point &from, const Point &to) return polyline; } -MotionPlannerGraph* -MotionPlanner::init_graph(int island_idx) +const MotionPlannerGraph& MotionPlanner::init_graph(int island_idx) { - if (this->graphs[island_idx + 1] == NULL) { + if (! this->graphs[island_idx + 1]) { // if this graph doesn't exist, initialize it - MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph(); + this->graphs[island_idx + 1] = make_unique(); + MotionPlannerGraph* graph = this->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. */ @@ -214,7 +185,7 @@ MotionPlanner::init_graph(int island_idx) t_vd_vertices vd_vertices; // get boundaries as lines - MotionPlannerEnv env = this->get_env(island_idx); + const MotionPlannerEnv &env = this->get_env(island_idx); Lines lines = env.env.lines(); boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd); @@ -228,6 +199,7 @@ MotionPlanner::init_graph(int island_idx) Point p1 = Point(v1->x(), v1->y()); // skip edge if any of its endpoints is outside our configuration space + //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); @@ -252,14 +224,12 @@ MotionPlanner::init_graph(int island_idx) 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]; + + return *this->graphs[island_idx + 1].get(); } -Point -MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const +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 @@ -270,23 +240,19 @@ MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const // get the points of the hole containing 'from', if any Points pp; - for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) { - for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) { - if (h->contains(from)) { - pp = *h; - } - } - if (!pp.empty()) break; + for (const ExPolygon &ex : this->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 (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) { - Points contour_pp = ex->contour; - pp.insert(pp.end(), contour_pp.begin(), contour_pp.end()); - } - } + if (pp.empty()) + for (const ExPolygon &ex : this->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) { @@ -297,115 +263,77 @@ MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const if (intersection_ln((Lines)Line(from, pp[result]), this->island).size() > 1) { // discard result pp.erase(pp.begin() + result); - } else { + } else return pp[result]; - } } // if we're here, return last point if any (better than nothing) - if (!pp.empty()) { - return pp.front(); - } - // 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 from; + return pp.empty() ? from : pp.front(); } -void -MotionPlannerGraph::add_edge(size_t from, size_t to, double weight) +// 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 (this->adjacency_list.size() < from+1) - this->adjacency_list.resize(from+1); - - this->adjacency_list[from].push_back(neighbor(to, weight)); -} - -size_t -MotionPlannerGraph::find_node(const Point &point) const -{ - /* - for (Points::const_iterator p = this->nodes.begin(); p != this->nodes.end(); ++p) { - if (p->coincides_with(point)) return p - this->nodes.begin(); + // Extend adjacency list until this start node. + if (this->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))); + // Allocate new empty adjacency vectors. + this->adjacency_list.resize(from + 1); } - */ - return point.nearest_point_index(this->nodes); + this->adjacency_list[from].emplace_back(Neighbor(node_t(to), weight)); } -Polyline -MotionPlannerGraph::shortest_path(size_t from, size_t to) +// Dijkstra's shortest path in a weighted graph from node_start to node_end. +// The returned path contains the end points. +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()) return Polyline(); - - const weight_t max_weight = std::numeric_limits::infinity(); - - std::vector dist; - std::vector previous; - { - // number of nodes - size_t n = this->adjacency_list.size(); - - // initialize dist and previous - dist.clear(); - dist.resize(n, max_weight); - dist[from] = 0; // distance from 'from' to itself - previous.clear(); - previous.resize(n, -1); - - // initialize the Q with all nodes - std::set Q; - for (node_t i = 0; i < n; ++i) Q.insert(i); - - while (!Q.empty()) - { - // get node in Q having the minimum dist ('from' in the first loop) - node_t u; - { - double min_dist = -1; - for (std::set::const_iterator n = Q.begin(); n != Q.end(); ++n) { - if (dist[*n] < min_dist || min_dist == -1) { - u = *n; - min_dist = dist[*n]; - } + // This prevents a crash in case for some reason we got here with an empty adjacency list. + if (this->adjacency_list.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)); + 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) + 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 (u == node_end) + break; + // Visit each edge starting at node u. + for (const Neighbor& neighbor : this->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]); } } - Q.erase(u); - - // stop searching if we reached our destination - if (u == to) break; - - // Visit each edge starting from node u - const std::vector &neighbors = this->adjacency_list[u]; - for (std::vector::const_iterator neighbor_iter = neighbors.begin(); - neighbor_iter != neighbors.end(); - ++neighbor_iter) - { - // neighbor node is v - node_t v = neighbor_iter->target; - - // skip if we already visited this - if (Q.find(v) == Q.end()) continue; - - // calculate total distance - weight_t alt = dist[u] + neighbor_iter->weight; - - // if total distance through u is shorter than the previous - // distance (if any) between 'from' and 'v', replace it - if (alt < dist[v]) { - dist[v] = alt; - previous[v] = u; - } - - } - } } - + Polyline polyline; - for (node_t vertex = to; vertex != -1; vertex = previous[vertex]) + polyline.points.reserve(previous.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[from]); + polyline.points.push_back(this->nodes[node_start]); polyline.reverse(); return polyline; } diff --git a/xs/src/libslic3r/MotionPlanner.hpp b/xs/src/libslic3r/MotionPlanner.hpp index b6251ff21..add1f79cf 100644 --- a/xs/src/libslic3r/MotionPlanner.hpp +++ b/xs/src/libslic3r/MotionPlanner.hpp @@ -2,11 +2,13 @@ #define slic3r_MotionPlanner_hpp_ #include "libslic3r.h" +#include "BoundingBox.hpp" #include "ClipperUtils.hpp" #include "ExPolygonCollection.hpp" #include "Polyline.hpp" #include #include +#include #include #define MP_INNER_MARGIN scale_(1.0) @@ -20,11 +22,12 @@ class MotionPlannerEnv { friend class MotionPlanner; - public: - ExPolygon island; +public: + ExPolygon island; + BoundingBox island_bbox; ExPolygonCollection env; MotionPlannerEnv() {}; - MotionPlannerEnv(const ExPolygon &island) : island(island) {}; + MotionPlannerEnv(const ExPolygon &island) : island(island), island_bbox(get_extents(island)) {}; Point nearest_env_point(const Point &from, const Point &to) const; }; @@ -32,43 +35,43 @@ class MotionPlannerGraph { friend class MotionPlanner; - private: - typedef int node_t; - typedef double weight_t; - struct neighbor { - node_t target; +private: + typedef int node_t; + typedef double weight_t; + struct Neighbor { + node_t target; weight_t weight; - neighbor(node_t arg_target, weight_t arg_weight) - : target(arg_target), weight(arg_weight) { } + Neighbor(node_t arg_target, weight_t arg_weight) : target(arg_target), weight(arg_weight) {} }; - typedef std::vector< std::vector > adjacency_list_t; + typedef std::vector> adjacency_list_t; adjacency_list_t adjacency_list; - public: - Points nodes; - //std::map, double> edges; - void add_edge(size_t from, size_t to, double weight); - size_t find_node(const Point &point) const; - Polyline shortest_path(size_t from, size_t to); +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; }; class MotionPlanner { - public: +public: MotionPlanner(const ExPolygons &islands); - ~MotionPlanner(); - Polyline shortest_path(const Point &from, const Point &to); - size_t islands_count() const; + ~MotionPlanner() {} + + Polyline shortest_path(const Point &from, const Point &to); + size_t islands_count() const { return this->islands.size(); } + +private: + bool initialized; + std::vector islands; + MotionPlannerEnv outer; + std::vector> graphs; - private: - bool initialized; - std::vector islands; - MotionPlannerEnv outer; - std::vector graphs; - - void initialize(); - MotionPlannerGraph* init_graph(int island_idx); - const MotionPlannerEnv& get_env(int island_idx) const; + 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]; } }; } diff --git a/xs/src/libslic3r/MutablePriorityQueue.hpp b/xs/src/libslic3r/MutablePriorityQueue.hpp new file mode 100644 index 000000000..82e992fd6 --- /dev/null +++ b/xs/src/libslic3r/MutablePriorityQueue.hpp @@ -0,0 +1,147 @@ +#ifndef slic3r_MutablePriorityQueue_hpp_ +#define slic3r_MutablePriorityQueue_hpp_ + +#include + +template +class MutablePriorityQueue +{ +public: + MutablePriorityQueue(IndexSetter &&index_setter, LessPredicate &&less_predicate) : + m_index_setter(std::forward(index_setter)), + m_less_predicate(std::forward(less_predicate)) + {} + ~MutablePriorityQueue() { clear(); } + + inline void clear() { m_heap.clear(); } + inline void reserve(size_t cnt) { m_heap.reserve(cnt); } + inline void push(const T &item); + inline void push(T &&item); + inline void pop(); + inline T& top() { return m_heap.front(); } + inline void remove(size_t idx); + inline void update(size_t idx) { T item = m_heap[idx]; remove(idx); push(item); } + + inline size_t size() const { return m_heap.size(); } + inline bool empty() const { return m_heap.empty(); } + +protected: + inline void update_heap_up(size_t top, size_t bottom); + inline void update_heap_down(size_t top, size_t bottom); + +private: + std::vector m_heap; + IndexSetter m_index_setter; + LessPredicate m_less_predicate; +}; + +template +MutablePriorityQueue make_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate) +{ + return MutablePriorityQueue( + std::forward(index_setter), std::forward(less_predicate)); +} + +template +inline void MutablePriorityQueue::push(const T &item) +{ + size_t idx = m_heap.size(); + m_heap.emplace_back(item); + m_index_setter(m_heap.back(), idx); + update_heap_up(0, idx); +} + +template +inline void MutablePriorityQueue::push(T &&item) +{ + size_t idx = m_heap.size(); + m_heap.emplace_back(std::move(item)); + m_index_setter(m_heap.back(), idx); + update_heap_up(0, idx); +} + +template +inline void MutablePriorityQueue::pop() +{ + assert(! m_heap.empty()); + if (m_heap.size() > 1) { + m_heap.front() = m_heap.back(); + m_heap.pop_back(); + m_index_setter(m_heap.front(), 0); + update_heap_down(0, m_heap.size() - 1); + } else + m_heap.clear(); +} + +template +inline void MutablePriorityQueue::remove(size_t idx) +{ + assert(idx < m_heap.size()); + if (idx + 1 == m_heap.size()) { + m_heap.pop_back(); + return; + } + m_heap[idx] = m_heap.back(); + m_index_setter(m_heap[idx], idx); + m_heap.pop_back(); + update_heap_down(idx, m_heap.size() - 1); + update_heap_up(0, idx); +} + +template +inline void MutablePriorityQueue::update_heap_up(size_t top, size_t bottom) +{ + size_t childIdx = bottom; + T *child = &m_heap[childIdx]; + for (;;) { + size_t parentIdx = (childIdx - 1) >> 1; + if (childIdx == 0 || parentIdx < top) + break; + T *parent = &m_heap[parentIdx]; + // switch nodes + if (! m_less_predicate(*parent, *child)) { + T tmp = *parent; + m_index_setter(*parent, childIdx); + m_index_setter(*child, parentIdx); + m_heap[parentIdx] = *child; + m_heap[childIdx] = tmp; + } + // shift up + childIdx = parentIdx; + child = parent; + } +} + +template +inline void MutablePriorityQueue::update_heap_down(size_t top, size_t bottom) +{ + size_t parentIdx = top; + T *parent = &m_heap[parentIdx]; + for (;;) { + size_t childIdx = (parentIdx << 1) + 1; + if (childIdx > bottom) + break; + T *child = &m_heap[childIdx]; + size_t child2Idx = childIdx + 1; + if (child2Idx <= bottom) { + T *child2 = &m_heap[child2Idx]; + if (! m_less_predicate(*child, *child2)) { + child = child2; + childIdx = child2Idx; + } + } + if (m_less_predicate(*parent, *child)) + return; + // switch nodes + m_index_setter(*parent, childIdx); + m_index_setter(*child, parentIdx); + T tmp = *parent; + m_heap[parentIdx] = *child; + m_heap[childIdx] = tmp; + // shift down + parentIdx = childIdx; + parent = child; + } +} + +#endif /* slic3r_MutablePriorityQueue_hpp_ */ diff --git a/xs/src/libslic3r/Point.cpp b/xs/src/libslic3r/Point.cpp index db90765d4..a587e3406 100644 --- a/xs/src/libslic3r/Point.cpp +++ b/xs/src/libslic3r/Point.cpp @@ -94,8 +94,7 @@ inline T sqr(const T x) return x * x; } -int -Point::nearest_point_index(const PointConstPtrs &points) const +int Point::nearest_point_index(const PointConstPtrs &points) const { int idx = -1; double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough @@ -121,28 +120,25 @@ Point::nearest_point_index(const PointConstPtrs &points) const } /* 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 Point::nearest_waypoint_index(const Points &points, const Point &dest) const { - size_t idx = -1; - double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough - - for (Points::const_iterator p = points.begin(); p != points.end(); ++p) { - // distance from this to candidate - double d = sqr(this->x - p->x) + sqr(this->y - p->y); - - // distance from candidate to dest - d += sqr(p->x - dest.x) + sqr(p->y - dest.y); - - // if the total distance is greater than current min distance, ignore it - if (distance != -1 && d > distance) continue; - - idx = p - points.begin(); - distance = d; - - if (distance < EPSILON) break; + 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; } diff --git a/xs/src/libslic3r/libslic3r.h b/xs/src/libslic3r/libslic3r.h index 4699cc9dc..614b4c263 100644 --- a/xs/src/libslic3r/libslic3r.h +++ b/xs/src/libslic3r/libslic3r.h @@ -127,6 +127,12 @@ void remove_nulls(std::vector &vec) vec.end()); } +// Older compilers do not provide a std::make_unique template. Provide a simple one. +template +inline std::unique_ptr make_unique(Args&&... args) { + return std::unique_ptr(new T(std::forward(args)...)); +} + } // namespace Slic3r #endif