From 41495a932a94a64b949a0950d0e8be445e9083e9 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Thu, 26 Sep 2019 09:44:38 +0200 Subject: [PATCH] Introduction of a greedy Traveling Salesman Problem algorithm, producing better shortest path estimate than the "closest next neighbor" heuristics. The new greedy algorithm utilizes KD tree for closest end point search, and builds a graph to detect loops. PerimeterGenerator newly uses the optimized TSP algorithm. ExtrusionEntity has been refactored / simplified. --- src/libslic3r/CMakeLists.txt | 4 +- src/libslic3r/ExtrusionEntityCollection.cpp | 17 +- src/libslic3r/ExtrusionEntityCollection.hpp | 11 +- src/libslic3r/KDTreeIndirect.hpp | 228 ++++++++++ src/libslic3r/MutablePriorityQueue.hpp | 31 +- src/libslic3r/PerimeterGenerator.cpp | 71 +-- src/libslic3r/ShortestPath.cpp | 479 ++++++++++++++++++++ src/libslic3r/ShortestPath.hpp | 17 + 8 files changed, 797 insertions(+), 61 deletions(-) create mode 100644 src/libslic3r/KDTreeIndirect.hpp create mode 100644 src/libslic3r/ShortestPath.cpp create mode 100644 src/libslic3r/ShortestPath.hpp diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 97e0fc09b..6fe68e56f 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -100,7 +100,7 @@ add_library(libslic3r STATIC Geometry.cpp Geometry.hpp Int128.hpp -# KdTree.hpp + KdTreeIndirect.hpp Layer.cpp Layer.hpp LayerRegion.cpp @@ -142,6 +142,8 @@ add_library(libslic3r STATIC PrintObject.cpp PrintRegion.cpp Semver.cpp + ShortestPath.cpp + ShortestPath.hpp SLAPrint.cpp SLAPrint.hpp SLA/SLAAutoSupports.hpp diff --git a/src/libslic3r/ExtrusionEntityCollection.cpp b/src/libslic3r/ExtrusionEntityCollection.cpp index 70c2348af..f7fab1ba1 100644 --- a/src/libslic3r/ExtrusionEntityCollection.cpp +++ b/src/libslic3r/ExtrusionEntityCollection.cpp @@ -16,7 +16,6 @@ ExtrusionEntityCollection& ExtrusionEntityCollection::operator=(const ExtrusionE this->entities = other.entities; for (size_t i = 0; i < this->entities.size(); ++i) this->entities[i] = this->entities[i]->clone(); - this->orig_indices = other.orig_indices; this->no_sort = other.no_sort; return *this; } @@ -24,7 +23,6 @@ ExtrusionEntityCollection& ExtrusionEntityCollection::operator=(const ExtrusionE void ExtrusionEntityCollection::swap(ExtrusionEntityCollection &c) { std::swap(this->entities, c.entities); - std::swap(this->orig_indices, c.orig_indices); std::swap(this->no_sort, c.no_sort); } @@ -82,10 +80,10 @@ ExtrusionEntityCollection ExtrusionEntityCollection::chained_path(bool no_revers return coll; } -void ExtrusionEntityCollection::chained_path(ExtrusionEntityCollection* retval, bool no_reverse, ExtrusionRole role, std::vector* orig_indices) const +void ExtrusionEntityCollection::chained_path(ExtrusionEntityCollection* retval, bool no_reverse, ExtrusionRole role) const { if (this->entities.empty()) return; - this->chained_path_from(this->entities.front()->first_point(), retval, no_reverse, role, orig_indices); + this->chained_path_from(this->entities.front()->first_point(), retval, no_reverse, role); } ExtrusionEntityCollection ExtrusionEntityCollection::chained_path_from(Point start_near, bool no_reverse, ExtrusionRole role) const @@ -95,7 +93,7 @@ ExtrusionEntityCollection ExtrusionEntityCollection::chained_path_from(Point sta return coll; } -void ExtrusionEntityCollection::chained_path_from(Point start_near, ExtrusionEntityCollection* retval, bool no_reverse, ExtrusionRole role, std::vector* orig_indices) const +void ExtrusionEntityCollection::chained_path_from(Point start_near, ExtrusionEntityCollection* retval, bool no_reverse, ExtrusionRole role) const { if (this->no_sort) { *retval = *this; @@ -103,7 +101,6 @@ void ExtrusionEntityCollection::chained_path_from(Point start_near, ExtrusionEnt } retval->entities.reserve(this->entities.size()); - retval->orig_indices.reserve(this->entities.size()); // if we're asked to return the original indices, build a map std::map indices_map; @@ -122,8 +119,8 @@ void ExtrusionEntityCollection::chained_path_from(Point start_near, ExtrusionEnt ExtrusionEntity *entity = entity_src->clone(); my_paths.push_back(entity); - if (orig_indices != nullptr) - indices_map[entity] = &entity_src - &this->entities.front(); +// if (orig_indices != nullptr) +// indices_map[entity] = &entity_src - &this->entities.front(); } Points endpoints; @@ -142,8 +139,8 @@ void ExtrusionEntityCollection::chained_path_from(Point start_near, ExtrusionEnt if (start_index % 2 && !no_reverse && entity->can_reverse()) entity->reverse(); retval->entities.push_back(my_paths.at(path_index)); - if (orig_indices != nullptr) - orig_indices->push_back(indices_map[entity]); +// if (orig_indices != nullptr) +// orig_indices->push_back(indices_map[entity]); my_paths.erase(my_paths.begin() + path_index); endpoints.erase(endpoints.begin() + 2*path_index, endpoints.begin() + 2*path_index + 2); start_near = retval->entities.back()->last_point(); diff --git a/src/libslic3r/ExtrusionEntityCollection.hpp b/src/libslic3r/ExtrusionEntityCollection.hpp index 221afc453..e92fa156f 100644 --- a/src/libslic3r/ExtrusionEntityCollection.hpp +++ b/src/libslic3r/ExtrusionEntityCollection.hpp @@ -14,15 +14,14 @@ public: ExtrusionEntity* clone_move() override { return new ExtrusionEntityCollection(std::move(*this)); } ExtrusionEntitiesPtr entities; // we own these entities - std::vector orig_indices; // handy for XS bool no_sort; ExtrusionEntityCollection(): no_sort(false) {}; - ExtrusionEntityCollection(const ExtrusionEntityCollection &other) : orig_indices(other.orig_indices), no_sort(other.no_sort) { this->append(other.entities); } - ExtrusionEntityCollection(ExtrusionEntityCollection &&other) : entities(std::move(other.entities)), orig_indices(std::move(other.orig_indices)), no_sort(other.no_sort) {} + ExtrusionEntityCollection(const ExtrusionEntityCollection &other) : no_sort(other.no_sort) { this->append(other.entities); } + ExtrusionEntityCollection(ExtrusionEntityCollection &&other) : entities(std::move(other.entities)), no_sort(other.no_sort) {} explicit ExtrusionEntityCollection(const ExtrusionPaths &paths); ExtrusionEntityCollection& operator=(const ExtrusionEntityCollection &other); ExtrusionEntityCollection& operator=(ExtrusionEntityCollection &&other) - { this->entities = std::move(other.entities); this->orig_indices = std::move(other.orig_indices); this->no_sort = other.no_sort; return *this; } + { this->entities = std::move(other.entities); this->no_sort = other.no_sort; return *this; } ~ExtrusionEntityCollection() { clear(); } explicit operator ExtrusionPaths() const; @@ -67,9 +66,9 @@ public: void replace(size_t i, const ExtrusionEntity &entity); void remove(size_t i); ExtrusionEntityCollection chained_path(bool no_reverse = false, ExtrusionRole role = erMixed) const; - void chained_path(ExtrusionEntityCollection* retval, bool no_reverse = false, ExtrusionRole role = erMixed, std::vector* orig_indices = nullptr) const; + void chained_path(ExtrusionEntityCollection* retval, bool no_reverse = false, ExtrusionRole role = erMixed) const; ExtrusionEntityCollection chained_path_from(Point start_near, bool no_reverse = false, ExtrusionRole role = erMixed) const; - void chained_path_from(Point start_near, ExtrusionEntityCollection* retval, bool no_reverse = false, ExtrusionRole role = erMixed, std::vector* orig_indices = nullptr) const; + void chained_path_from(Point start_near, ExtrusionEntityCollection* retval, bool no_reverse = false, ExtrusionRole role = erMixed) const; void reverse(); Point first_point() const { return this->entities.front()->first_point(); } Point last_point() const { return this->entities.back()->last_point(); } diff --git a/src/libslic3r/KDTreeIndirect.hpp b/src/libslic3r/KDTreeIndirect.hpp new file mode 100644 index 000000000..f79dab9b3 --- /dev/null +++ b/src/libslic3r/KDTreeIndirect.hpp @@ -0,0 +1,228 @@ +// KD tree built upon external data set, referencing the external data by integer indices. + +#ifndef slic3r_KDTreeIndirect_hpp_ +#define slic3r_KDTreeIndirect_hpp_ + +#include +#include +#include + +#include "Utils.hpp" // for next_highest_power_of_2() + +namespace Slic3r { + +// KD tree for N-dimensional closest point search. +template +class KDTreeIndirect +{ +public: + static constexpr size_t NumDimensions = ANumDimensions; + using CoordinateFn = ACoordinateFn; + using CoordType = ACoordType; + static constexpr size_t npos = size_t(-1); + + KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {} + KDTreeIndirect(CoordinateFn coordinate, std::vector indices) : coordinate(coordinate) { this->build(std::move(indices)); } + KDTreeIndirect(CoordinateFn coordinate, std::vector &&indices) : coordinate(coordinate) { this->build(std::move(indices)); } + KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); } + KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {} + KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; } + void clear() { m_nodes.clear(); } + + void build(size_t num_indices) + { + std::vector indices; + indices.reserve(num_indices); + for (size_t i = 0; i < num_indices; ++ i) + indices.emplace_back(i); + this->build(std::move(indices)); + } + + void build(std::vector &&indices) + { + if (indices.empty()) + clear(); + else { + // Allocate a next highest power of 2 nodes, because the incomplete binary tree will not have the leaves filled strictly from the left. + m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos); + build_recursive(indices, 0, 0, 0, (int)(indices.size() - 1)); + } + indices.clear(); + } + + enum class VisitorReturnMask : unsigned int + { + CONTINUE_LEFT = 1, + CONTINUE_RIGHT = 2, + STOP = 4, + }; + template + unsigned int descent_mask(const CoordType &point_coord, const CoordType &search_radius, size_t idx, size_t dimension) const + { + CoordType dist = point_coord - this->coordinate(idx, dimension); + return (dist * dist < search_radius + CoordType(EPSILON)) ? + ((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) : + (dist < CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT); + } + + // Visitor is supposed to return a bit mask of VisitorReturnMask. + template + void visit(Visitor &visitor) const + { + return m_nodes.empty() ? npos : visit_recursive(0, 0, visitor); + } + + CoordinateFn coordinate; + +private: + // Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension. + void build_recursive(std::vector &input, size_t node, int dimension, int left, int right) + { + if (left > right) + return; + + assert(node < m_nodes.size()); + + if (left == right) { + // Insert a node into the balanced tree. + m_nodes[node] = input[left]; + return; + } + + // Partition the input sequence to two equal halves. + int center = (left + right) >> 1; + partition_input(input, dimension, left, right, center); + // Insert a node into the tree. + m_nodes[node] = input[center]; + // Partition the left and right subtrees. + size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension; + build_recursive(input, (node << 1) + 1, next_dimension, left, center - 1); + build_recursive(input, (node << 1) + 2, next_dimension, center + 1, right); + } + + // Partition the input m_nodes at k using QuickSelect method. + // https://en.wikipedia.org/wiki/Quickselect + void partition_input(std::vector &input, int dimension, int left, int right, int k) const + { + while (left < right) { + // Guess the k'th element. + // Pick the pivot as a median of first, center and last value. + // Sort first, center and last values. + int center = (left + right) >> 1; + auto left_value = this->coordinate(input[left], dimension); + auto center_value = this->coordinate(input[center], dimension); + auto right_value = this->coordinate(input[right], dimension); + if (center_value < left_value) { + std::swap(input[left], input[center]); + std::swap(left_value, center_value); + } + if (right_value < left_value) { + std::swap(input[left], input[right]); + std::swap(left_value, right_value); + } + if (right_value < center_value) { + std::swap(input[center], input[right]); + // No need to do that, result is not used. + // std::swap(center_value, right_value); + } + // Only two or three values are left and those are sorted already. + if (left + 3 > right) + break; + // left and right items are already at their correct positions. + // input[left].point[dimension] <= input[center].point[dimension] <= input[right].point[dimension] + // Move the pivot to the (right - 1) position. + std::swap(input[center], input[right - 1]); + // Pivot value. + double pivot = this->coordinate(input[right - 1], dimension); + // Partition the set based on the pivot. + int i = left; + int j = right - 1; + for (;;) { + // Skip left points that are already at correct positions. + // Search will certainly stop at position (right - 1), which stores the pivot. + while (this->coordinate(input[++ i], dimension) < pivot) ; + // Skip right points that are already at correct positions. + while (this->coordinate(input[-- j], dimension) > pivot && i < j) ; + if (i >= j) + break; + std::swap(input[i], input[j]); + } + // Restore pivot to the center of the sequence. + std::swap(input[i], input[right]); + // Which side the kth element is in? + if (k < i) + right = i - 1; + else if (k == i) + // Sequence is partitioned, kth element is at its place. + break; + else + left = i + 1; + } + } + + template + void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const + { + assert(! m_nodes.empty()); + if (node >= m_nodes.size() || m_nodes[node] == npos) + return; + + // Left / right child node index. + size_t left = (node << 1) + 1; + size_t right = left + 1; + unsigned int mask = visitor(m_nodes[node], dimension); + if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) { + size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension; + if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT) + visit_recursive(left, next_dimension, visitor); + if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT) + visit_recursive(right, next_dimension, visitor); + } + } + + std::vector m_nodes; +}; + +// Find a closest point using Euclidian metrics. +// Returns npos if not found. +template +size_t find_closest_point(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) +{ + struct Visitor { + using CoordType = typename KDTreeIndirectType::CoordType; + const KDTreeIndirectType &kdtree; + const PointType &point; + const FilterFn filter; + size_t min_idx = KDTreeIndirectType::npos; + CoordType min_dist = std::numeric_limits::max(); + + Visitor(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) : kdtree(kdtree), point(point), filter(filter) {} + unsigned int operator()(size_t idx, size_t dimension) { + if (this->filter(idx)) { + auto dist = CoordType(0); + for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++ i) { + CoordType d = point[i] - kdtree.coordinate(idx, i); + dist += d * d; + } + if (dist < min_dist) { + min_dist = dist; + min_idx = idx; + } + } + return kdtree.descent_mask(point[dimension], min_dist, idx, dimension); + } + } visitor(kdtree, point, filter); + + kdtree.visit(visitor); + return visitor.min_idx; +} + +template +size_t find_closest_point(const KDTreeIndirectType& kdtree, const PointType& point) +{ + return find_closest_point(kdtree, point, [](size_t) { return true; }); +} + +} // namespace Slic3r + +#endif /* slic3r_KDTreeIndirect_hpp_ */ diff --git a/src/libslic3r/MutablePriorityQueue.hpp b/src/libslic3r/MutablePriorityQueue.hpp index 82e992fd6..31946c707 100644 --- a/src/libslic3r/MutablePriorityQueue.hpp +++ b/src/libslic3r/MutablePriorityQueue.hpp @@ -13,21 +13,28 @@ public: {} ~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); } + void clear() { m_heap.clear(); } + void reserve(size_t cnt) { m_heap.reserve(cnt); } + void push(const T &item); + void push(T &&item); + void pop(); + T& top() { return m_heap.front(); } + void remove(size_t idx); + 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(); } + size_t size() const { return m_heap.size(); } + bool empty() const { return m_heap.empty(); } + + using iterator = typename std::vector::iterator; + using const_iterator = typename std::vector::const_iterator; + iterator begin() { return m_heap.begin(); } + iterator end() { return m_heap.end(); } + const_iterator cbegin() const { return m_heap.cbegin(); } + const_iterator cend() const { return m_heap.cend(); } protected: - inline void update_heap_up(size_t top, size_t bottom); - inline void update_heap_down(size_t top, size_t bottom); + void update_heap_up(size_t top, size_t bottom); + void update_heap_down(size_t top, size_t bottom); private: std::vector m_heap; diff --git a/src/libslic3r/PerimeterGenerator.cpp b/src/libslic3r/PerimeterGenerator.cpp index 0c16f4a1d..cd5354776 100644 --- a/src/libslic3r/PerimeterGenerator.cpp +++ b/src/libslic3r/PerimeterGenerator.cpp @@ -1,6 +1,8 @@ #include "PerimeterGenerator.hpp" #include "ClipperUtils.hpp" #include "ExtrusionEntityCollection.hpp" +#include "ShortestPath.hpp" + #include #include @@ -86,24 +88,24 @@ static ExtrusionPaths thick_polyline_to_extrusion_paths(const ThickPolyline &thi return paths; } -static ExtrusionEntityCollection variable_width(const ThickPolylines& polylines, ExtrusionRole role, Flow flow) +static void variable_width(const ThickPolylines& polylines, ExtrusionRole role, Flow flow, std::vector &out) { // This value determines granularity of adaptive width, as G-code does not allow // variable extrusion within a single move; this value shall only affect the amount // of segments, and any pruning shall be performed before we apply this tolerance. - ExtrusionEntityCollection coll; const float tolerance = float(scale_(0.05)); for (const ThickPolyline &p : polylines) { ExtrusionPaths paths = thick_polyline_to_extrusion_paths(p, role, flow, tolerance); // Append paths to collection. if (! paths.empty()) { if (paths.front().first_point() == paths.back().last_point()) - coll.append(ExtrusionLoop(std::move(paths))); - else - coll.append(std::move(paths)); + out.emplace_back(new ExtrusionLoop(std::move(paths))); + else { + for (ExtrusionPath &path : paths) + out.emplace_back(new ExtrusionPath(std::move(path))); + } } } - return coll; } // Hierarchy of perimeters. @@ -186,43 +188,47 @@ static ExtrusionEntityCollection traverse_loops(const PerimeterGenerator &perime paths.push_back(path); } - coll.append(ExtrusionLoop(paths, loop_role)); + coll.append(ExtrusionLoop(std::move(paths), loop_role)); } // Append thin walls to the nearest-neighbor search (only for first iteration) if (! thin_walls.empty()) { - ExtrusionEntityCollection tw = variable_width(thin_walls, erExternalPerimeter, perimeter_generator.ext_perimeter_flow); - coll.append(tw.entities); + variable_width(thin_walls, erExternalPerimeter, perimeter_generator.ext_perimeter_flow, coll.entities); thin_walls.clear(); } - // Sort entities into a new collection using a nearest-neighbor search, - // preserving the original indices which are useful for detecting thin walls. - ExtrusionEntityCollection sorted_coll; - coll.chained_path(&sorted_coll, false, erMixed, &sorted_coll.orig_indices); - - // traverse children and build the final collection - ExtrusionEntityCollection entities; - for (const size_t &idx : sorted_coll.orig_indices) { - if (idx >= loops.size()) { - // This is a thin wall. Let's get it from the sorted collection as it might have been reversed. - entities.append(std::move(*sorted_coll.entities[&idx - &sorted_coll.orig_indices.front()])); + // Traverse children and build the final collection. + Point zero_point(0, 0); + std::vector> chain = chain_extrusion_entities(coll.entities, &zero_point); + ExtrusionEntityCollection out; + for (const std::pair &idx : chain) { + assert(coll.entities[idx.first] != nullptr); + if (idx.first >= loops.size()) { + // This is a thin wall. + out.entities.reserve(out.entities.size() + 1); + out.entities.emplace_back(coll.entities[idx.first]); + coll.entities[idx.first] = nullptr; + if (idx.second) + out.entities.back()->reverse(); } else { - const PerimeterGeneratorLoop &loop = loops[idx]; - ExtrusionLoop eloop = *dynamic_cast(coll.entities[idx]); + const PerimeterGeneratorLoop &loop = loops[idx.first]; + assert(thin_walls.empty()); ExtrusionEntityCollection children = traverse_loops(perimeter_generator, loop.children, thin_walls); + out.entities.reserve(out.entities.size() + children.entities.size() + 1); + ExtrusionLoop *eloop = static_cast(coll.entities[idx.first]); + coll.entities[idx.first] = nullptr; if (loop.is_contour) { - eloop.make_counter_clockwise(); - entities.append(std::move(children.entities)); - entities.append(std::move(eloop)); + eloop->make_counter_clockwise(); + out.append(std::move(children.entities)); + out.entities.emplace_back(eloop); } else { - eloop.make_clockwise(); - entities.append(std::move(eloop)); - entities.append(std::move(children.entities)); + eloop->make_clockwise(); + out.entities.emplace_back(eloop); + out.append(std::move(children.entities)); } } } - return entities; + return out; } void PerimeterGenerator::process() @@ -445,8 +451,8 @@ void PerimeterGenerator::process() for (const ExPolygon &ex : gaps_ex) ex.medial_axis(max, min, &polylines); if (! polylines.empty()) { - ExtrusionEntityCollection gap_fill = variable_width(polylines, erGapFill, this->solid_infill_flow); - this->gap_fill->append(gap_fill.entities); + ExtrusionEntityCollection gap_fill; + variable_width(polylines, erGapFill, this->solid_infill_flow, gap_fill.entities); /* Make sure we don't infill narrow parts that are already gap-filled (we only consider this surface's gaps to reduce the diff() complexity). Growing actual extrusions ensures that gaps not filled by medial axis @@ -456,7 +462,8 @@ void PerimeterGenerator::process() //FIXME Vojtech: This grows by a rounded extrusion width, not by line spacing, // therefore it may cover the area, but no the volume. last = diff_ex(to_polygons(last), gap_fill.polygons_covered_by_width(10.f)); - } + this->gap_fill->append(std::move(gap_fill.entities)); + } } // create one more offset to be used as boundary for fill diff --git a/src/libslic3r/ShortestPath.cpp b/src/libslic3r/ShortestPath.cpp new file mode 100644 index 000000000..e98b0a58c --- /dev/null +++ b/src/libslic3r/ShortestPath.cpp @@ -0,0 +1,479 @@ +#include "ShortestPath.hpp" +#include "KDTreeIndirect.hpp" +#include "MutablePriorityQueue.hpp" + +#if 0 + #undef NDEBUG + #undef assert +#endif + +#include +#include + +namespace Slic3r { + +// Chain perimeters (always closed) and thin fills (closed or open) using a greedy algorithm. +// Solving a Traveling Salesman Problem (TSP) with the modification, that the sites are not always points, but points and segments. +// Solving using a greedy algorithm, where a shortest edge is added to the solution if it does not produce a bifurcation or a cycle. +// Return index and "reversed" flag. +// https://en.wikipedia.org/wiki/Multi-fragment_algorithm +// The algorithm builds a tour for the traveling salesman one edge at a time and thus maintains multiple tour fragments, each of which +// is a simple path in the complete graph of cities. At each stage, the algorithm selects the edge of minimal cost that either creates +// a new fragment, extends one of the existing paths or creates a cycle of length equal to the number of cities. +std::vector> chain_extrusion_entities(std::vector &entities, const Point *start_near) +{ + std::vector> out; + + if (entities.empty()) { + // Nothing to do. + } + else if (entities.size() == 1) + { + // Just sort the end points so that the first point visited is closest to start_near. + ExtrusionEntity *extrusion_entity = entities.front(); + out.emplace_back(0, extrusion_entity->can_reverse() && start_near != nullptr && + (extrusion_entity->last_point() - *start_near).cast().squaredNorm() < (extrusion_entity->first_point() - *start_near).cast().squaredNorm()); + } + else + { + // End points of entities for the KD tree closest point search. + // A single end point is inserted into the search structure for loops, two end points are entered for open paths. + struct EndPoint { + EndPoint(const Vec2d &pos) : pos(pos) {} + + Vec2d pos; + + // Identifier of the chain, to which this end point belongs. Zero means unassigned. + size_t chain_id = 0; + // Link to the closest currently valid end point. + EndPoint *edge_out = nullptr; + // Reverse of edge_out. As there may be multiple end points with the same edge_out, + // these other edge_in points are chained using the on_circle_prev / on_circle_next cyclic loop. + EndPoint *edge_in = nullptr; + EndPoint* on_circle_prev = nullptr; + EndPoint* on_circle_next = nullptr; + void on_circle_merge(EndPoint *other) + { + EndPoint *a = this; + EndPoint *b = other; + assert(a->validate()); + assert(b->validate()); + if (a->on_circle_next == nullptr) + std::swap(a, b); + if (a->on_circle_next == nullptr) { + a->on_circle_next = a->on_circle_prev = b; + b->on_circle_next = b->on_circle_prev = a; + } else if (b->on_circle_next == nullptr) { + b->on_circle_next = a; + b->on_circle_prev = a->on_circle_prev; + a->on_circle_prev = b; + b->on_circle_prev->on_circle_next = b; + } else { + EndPoint *next = a->on_circle_next; + EndPoint *prev = b->on_circle_prev; + a->on_circle_next = b; + b->on_circle_prev = a; + prev->on_circle_next = next; + next->on_circle_prev = prev; + } + assert(this->validate()); + } + void on_circle_detach() + { + if (this->on_circle_next) { + EndPoint *next = this->on_circle_next; + EndPoint *prev = this->on_circle_prev; + if (prev == next) { + next->on_circle_next = nullptr; + next->on_circle_prev = nullptr; + } else { + prev->on_circle_next = next; + next->on_circle_prev = prev; + } + assert(prev->validate()); + assert(next->validate()); + this->on_circle_next = this->on_circle_prev = nullptr; + } + assert(this->validate()); + } + bool on_circle_empty() const + { + assert((this->on_circle_prev == nullptr) == (this->on_circle_next == nullptr)); + assert(this->on_circle_prev == nullptr || (this->on_circle_prev != this && this->on_circle_next != this)); + return this->on_circle_next == nullptr; + } + +#ifndef NDEBUG + bool validate() + { + assert((this->on_circle_prev == nullptr) == (this->on_circle_next == nullptr)); + assert(this->on_circle_prev == nullptr || (this->on_circle_prev != this && this->on_circle_next != this)); + assert(this->edge_out == nullptr || edge_out->edge_in != nullptr); + assert(this->distance_out >= 0.); + assert(this->edge_in == nullptr || this->edge_in->edge_out == this); + // Point which is a member of path (chain_id > 0) must not be in circle of some edge_in. + assert(this->chain_id == 0 || this->on_circle_empty()); + if (! this->on_circle_empty()) { + // Iterate over the cycle and validate the loop. + std::set visited; + const EndPoint *ep = this; + bool edge_in_found = false; + do { + // This end point is visited for the first time. + assert(visited.insert(ep).second); + assert(ep->on_circle_next != ep); + assert(ep->on_circle_prev != ep); + assert(ep->on_circle_next->on_circle_prev == ep); + assert(ep->on_circle_prev->on_circle_next == ep); + assert(ep->edge_out != nullptr && ep->edge_out == this->edge_out); + if (ep->edge_out->edge_in == ep) + edge_in_found = true; + ep = ep->on_circle_next; + } while (ep != this); + assert(edge_in_found); + } + return true; + } +#endif /* NDEBUG */ + + // Distance to the next end point following the link. + // Zero value -> start of the final path. + double distance_out = std::numeric_limits::max(); + size_t heap_idx = std::numeric_limits::max(); + }; + std::vector end_points; + end_points.reserve(entities.size() * 2); + for (const ExtrusionEntity* const &entity : entities) { + end_points.emplace_back(entity->first_point().cast()); + end_points.emplace_back(entity->last_point().cast()); + } + + // Construct the closest point KD tree over end points of extrusion entities. + auto coordinate_fn = [&end_points](size_t idx, size_t dimension) -> double { return end_points[idx].pos[dimension]; }; + KDTreeIndirect<2, double, decltype(coordinate_fn)> kdtree(coordinate_fn, end_points.size()); + + // Helper to detect loops in already connected paths. + // Unique chain IDs are assigned to paths. If paths are connected, end points will not have their chain IDs updated, but the chain IDs + // will remember an "equivalent" chain ID, which is the lowest ID of all the IDs in the path, and the lowest ID is equivalent to itself. + class EquivalentChains { + public: + // Zero'th chain ID is invalid. + EquivalentChains(size_t reserve) { m_equivalent_with.reserve(reserve); m_equivalent_with.emplace_back(0); } + // Generate next equivalence class. + size_t next() { + m_equivalent_with.emplace_back(++ m_last_chain_id); + return m_last_chain_id; + } + // Get equivalence class for chain ID. + size_t operator()(size_t chain_id) { + if (chain_id != 0) { + for (size_t last = chain_id;;) { + size_t lower = m_equivalent_with[last]; + if (lower == last) { + m_equivalent_with[chain_id] = lower; + chain_id = lower; + break; + } + last = lower; + } + } + return chain_id; + } + size_t merge(size_t chain_id1, size_t chain_id2) { + size_t chain_id = std::min((*this)(chain_id1), (*this)(chain_id2)); + m_equivalent_with[chain_id1] = chain_id; + m_equivalent_with[chain_id2] = chain_id; + return chain_id; + } + +#ifndef NDEBUG + bool validate() + { + assert(m_last_chain_id > 0); + assert(m_last_chain_id + 1 == m_equivalent_with.size()); + for (size_t i = 0; i < m_equivalent_with.size(); ++ i) { + for (size_t last = i;;) { + size_t lower = m_equivalent_with[last]; + assert(lower <= last); + if (lower == last) + break; + last = lower; + } + } + return true; + } +#endif /* NDEBUG */ + + private: + // Unique chain ID assigned to chains of end points of entities. + size_t m_last_chain_id = 0; + std::vector m_equivalent_with; + } equivalent_chain(entities.size()); + + // Find the first end point closest to start_near. + EndPoint *first_point = nullptr; + size_t first_point_idx = std::numeric_limits::max(); + if (start_near != nullptr) { + size_t idx = find_closest_point(kdtree, start_near->cast()); + assert(idx != kdtree.npos); + assert(idx < end_points.size()); + first_point = &end_points[idx]; + first_point->distance_out = 0.; + first_point->chain_id = equivalent_chain.next(); + first_point_idx = idx; + } + +#ifndef NDEBUG + auto validate_graph = [&end_points, &equivalent_chain]() -> bool { + for (EndPoint& ep : end_points) + ep.validate(); + assert(equivalent_chain.validate()); + return true; + }; +#endif /* NDEBUG */ + + // Assign the closest point and distance to the end points. + assert(validate_graph()); + for (EndPoint &end_point : end_points) { + assert(end_point.edge_out == nullptr); + if (&end_point != first_point) { + size_t this_idx = &end_point - &end_points.front(); + // Find the closest point to this end_point, which lies on a different extrusion path (filtered by the lambda). + // Ignore the starting point as the starting point is considered to be occupied, no end point coud connect to it. + size_t next_idx = find_closest_point(kdtree, end_point.pos, + [this_idx, first_point_idx](size_t idx){ return idx != first_point_idx && (idx ^ this_idx) > 1; }); + assert(next_idx != kdtree.npos); + assert(next_idx < end_points.size()); + EndPoint &end_point2 = end_points[next_idx]; + end_point.edge_out = &end_point2; + if (end_point2.edge_in == nullptr) + end_point2.edge_in = &end_point; + else { + assert(end_point.on_circle_empty()); + assert(end_point2.edge_in->edge_out == &end_point2); + end_point.on_circle_merge(end_point2.edge_in); + } + end_point.distance_out = (end_point2.pos - end_point.pos).squaredNorm(); + } + assert(validate_graph()); + } + + // Initialize a heap of end points sorted by the lowest distance to the next valid point of a path. + auto queue = make_mutable_priority_queue( + [](EndPoint *ep, size_t idx){ ep->heap_idx = idx; }, + [](EndPoint *l, EndPoint *r){ return l->distance_out < r->distance_out; }); + queue.reserve(end_points.size() * 2 - 1); + for (EndPoint &ep : end_points) + if (first_point != &ep) + queue.push(&ep); + +#ifndef NDEBUG + auto validate_graph_and_queue = [&validate_graph, &end_points, &queue, first_point]() -> bool { + assert(validate_graph()); + for (EndPoint &ep : end_points) { + if (ep.heap_idx < queue.size()) { + // End point is on the heap. + assert(*(queue.cbegin() + ep.heap_idx) == &ep); + assert(ep.chain_id == 0); + // Point on the heap may only points to other points on the heap. + assert(ep.edge_in == nullptr || ep.edge_in ->heap_idx < queue.size()); + assert(ep.edge_out == nullptr || ep.edge_out->heap_idx < queue.size()); + } else { + // End point is NOT on the heap, therefore it is part of the output path. + assert(ep.heap_idx == std::numeric_limits::max()); + assert(ep.chain_id != 0); + assert(ep.on_circle_empty()); + if (&ep == first_point) { + assert(ep.edge_in == nullptr); + assert(ep.edge_out == nullptr); + } else { + assert(ep.edge_in != nullptr); + assert(ep.edge_out != nullptr); + assert(ep.edge_in != &ep); + assert(ep.edge_in == ep.edge_out); + assert(ep.edge_in->edge_out == &ep); + assert(ep.edge_out->edge_in == &ep); + assert(ep.edge_in->heap_idx == std::numeric_limits::max()); + // Detect loops. + for (EndPoint *pt = &ep; pt != nullptr;) { + // Out of queue. It is a final point. + assert(pt->heap_idx == std::numeric_limits::max()); + EndPoint *pt_other = &end_points[(pt - &end_points.front()) ^ 1]; + if (pt_other->heap_idx < queue.size()) + // The other side of this segment is undecided yet. + break; + pt = pt_other->edge_out; + } + } + } + } + for (EndPoint *ep : queue) + // Points in the queue are not connected yet. + assert(ep->chain_id == 0); + return true; + }; +#endif /* NDEBUG */ + + // Chain the end points: find (entities.size() - 1) shortest links not forming bifurcations or loops. + std::vector end_points_update; + end_points_update.reserve(16); + assert(entities.size() >= 2); + for (int iter = int(entities.size()) - 2;; -- iter) { + assert(validate_graph_and_queue()); + // Take the first end point, for which the link points to the currently closest valid neighbor. + EndPoint &end_point1 = *queue.top(); + assert(end_point1.edge_out != nullptr); + // No point on the queue may be connected yet. + assert(end_point1.chain_id == 0); + // Take the closest end point to the first end point, + EndPoint &end_point2 = *end_point1.edge_out; + // The closest point must not be connected yet. + assert(end_point2.chain_id == 0); + // If end_point1.edge_out == end_point2, then end_point2.edge_in == &end_point1, or end_point2.edge_in points to some point on loop of end_point1. + assert(end_point2.edge_in != nullptr); + // End points of the opposite ends of the segments. + size_t end_point1_other_chain_id = equivalent_chain(end_points[(&end_point1 - &end_points.front()) ^ 1].chain_id); + size_t end_point2_other_chain_id = equivalent_chain(end_points[(&end_point2 - &end_points.front()) ^ 1].chain_id); + if (end_point1_other_chain_id == end_point2_other_chain_id && end_point1_other_chain_id != 0) { + // This edge forms a loop. Update end_point1 and try another one. + ++ iter; + assert(end_point1.edge_out != nullptr); + assert(end_point1.edge_out->edge_in != nullptr); + assert(! end_point1.on_circle_empty() || end_point1.edge_out->edge_in == &end_point1); + end_point1.edge_out->edge_in = end_point1.on_circle_empty() ? nullptr : end_point1.on_circle_next; + end_point1.edge_out = nullptr; + if (! end_point1.on_circle_empty()) + end_point1.on_circle_detach(); + assert(validate_graph_and_queue()); + end_points_update.emplace_back(&end_point1); + } else { + // Remove the first and second point from the queue. + queue.pop(); + queue.remove(end_point2.heap_idx); + #ifndef NDEBUG + // Mark them as removed from the queue. + end_point1.heap_idx = std::numeric_limits::max(); + end_point2.heap_idx = std::numeric_limits::max(); + #endif /* NDEBUG */ + // Collect the other end points pointing to this one, detach them from the on_circle linked list. + for (EndPoint *pt_first : { end_point1.edge_in, end_point2.edge_in }) + if (pt_first != nullptr) { + EndPoint *pt = pt_first; + do { + if (pt != &end_point1 && pt != &end_point2) { + // Point is in the queue. + assert(pt->heap_idx < queue.size()); + // Point is not connected yet. + assert(pt->chain_id == 0); + end_points_update.emplace_back(pt); + pt->edge_out = nullptr; + } + EndPoint *next = pt->on_circle_next; + pt->on_circle_prev = nullptr; + pt->on_circle_next = nullptr; + pt = next; + } while (pt != nullptr && pt != pt_first); + } + // If end_point1 was on a circle, the circle belonged to end_point2.edge_in, which was broken in the loop above. + assert(end_point1.on_circle_empty()); + // If end_point2 pointed to end_point1, then end_point2 was on a circle that belonged to end_point1.edge_in, which was broken in the loop above. + //assert(end_point2.on_circle_empty() == (end_point2.edge_out == &end_point1)); + assert(end_point2.on_circle_empty() || end_point2.edge_out != nullptr); + end_point2.edge_out->edge_in = end_point2.on_circle_empty() ? nullptr : end_point2.on_circle_next; + // The end_point2.link may not necessarily point back to end_point1 due to numeric issues and points on circles. + // Update the link back. + end_point1.edge_out = &end_point2; + end_point1.edge_in = &end_point2; + end_point2.edge_out = &end_point1; + end_point2.edge_in = &end_point1; + end_point2.distance_out = end_point1.distance_out; + // Assign chain IDs to the newly connected end points, set equivalent_chain if two chains were merged. + size_t chain_id = + (end_point1_other_chain_id == 0) ? + ((end_point2_other_chain_id == 0) ? equivalent_chain.next() : end_point2_other_chain_id) : + ((end_point2_other_chain_id == 0) ? end_point1_other_chain_id : + (end_point1_other_chain_id == end_point2_other_chain_id) ? + end_point1_other_chain_id : + equivalent_chain.merge(end_point1_other_chain_id, end_point2_other_chain_id)); + end_point1.chain_id = chain_id; + end_point2.chain_id = chain_id; + if (! end_point2.on_circle_empty()) + end_point2.on_circle_detach(); + assert(validate_graph_and_queue()); + } +#ifndef NDEBUG + for (EndPoint *end_point : end_points_update) { + assert(end_point->edge_out == nullptr); + // Point is in the queue. + assert(end_point->heap_idx < queue.size()); + // Point is not connected yet. + assert(end_point->chain_id == 0); + } +#endif /* NDEBUG */ + if (iter == 0) { + // Last iteration. There shall be exactly one or two end points waiting to be connected. + if (first_point == nullptr) { + // Two unconnected points are the end points of the constructed path. + assert(end_points_update.size() == 2); + first_point = end_points_update.front(); + } else + assert(end_points_update.size() == 1); + // Mark both points as ends of the path. + for (EndPoint *end_point : end_points_update) + end_point->edge_in = end_point->edge_out = nullptr; + break; + } + // Update links, distances and queue positions of all points that used to point to end_point1 or end_point2. + for (EndPoint *end_point : end_points_update) { + size_t this_idx = end_point - &end_points.front(); + // Find the closest point to this end_point, which lies on a different extrusion path (filtered by the filter lambda). + size_t next_idx = find_closest_point(kdtree, end_point->pos, [&end_points, &equivalent_chain, this_idx](size_t idx) { + assert(end_points[this_idx].edge_out == nullptr); + assert(end_points[this_idx].chain_id == 0); + if ((idx ^ this_idx) <= 1 || end_points[idx].chain_id != 0) + // Points of the same segment shall not be connected, + // cannot connect to an already connected point (ideally those would be removed from the KD tree, but the update is difficult). + return false; + size_t chain1 = equivalent_chain(end_points[this_idx ^ 1].chain_id); + size_t chain2 = equivalent_chain(end_points[idx ^ 1].chain_id); + return chain1 != chain2 || chain1 == 0; + }); + assert(next_idx != kdtree.npos); + assert(next_idx < end_points.size()); + EndPoint &end_point2 = end_points[next_idx]; + end_point->edge_out = &end_point2; + if (end_point2.edge_in == nullptr) + end_point2.edge_in = end_point; + else { + assert(end_point->on_circle_empty()); + assert(end_point2.edge_in->edge_out == &end_point2); + end_point->on_circle_merge(end_point2.edge_in); + } + end_point->distance_out = (end_points[next_idx].pos - end_point->pos).squaredNorm(); + // Update position of this end point in the queue based on the distance calculated at the line above. + queue.update(end_point->heap_idx); + //FIXME Remove the other end point from the KD tree. + // As the KD tree update is expensive, do it only after some larger number of points is removed from the queue. + assert(validate_graph_and_queue()); + } + end_points_update.clear(); + } + assert(queue.size() == (first_point == nullptr) ? 1 : 2); + + // Now interconnect pairs of segments into a chain. + assert(first_point != nullptr); + do { + size_t first_point_id = first_point - &end_points.front(); + size_t extrusion_entity_id = first_point_id >> 1; + EndPoint *second_point = &end_points[first_point_id ^ 1]; + ExtrusionEntity *extrusion_entity = entities[extrusion_entity_id]; + out.emplace_back(extrusion_entity_id, extrusion_entity->can_reverse() && (first_point_id & 1)); + first_point = second_point->edge_out; + } while (first_point != nullptr); + } + + assert(out.size() == entities.size()); + return out; +} + +} // namespace Slic3r diff --git a/src/libslic3r/ShortestPath.hpp b/src/libslic3r/ShortestPath.hpp new file mode 100644 index 000000000..2a7e67688 --- /dev/null +++ b/src/libslic3r/ShortestPath.hpp @@ -0,0 +1,17 @@ +#ifndef slic3r_ShortestPath_hpp_ +#define slic3r_ShortestPath_hpp_ + +#include "libslic3r.h" +#include "ExtrusionEntity.hpp" +#include "Point.hpp" + +#include +#include + +namespace Slic3r { + +std::vector> chain_extrusion_entities(std::vector &entities, const Point *start_near = nullptr); + +} // namespace Slic3r + +#endif /* slic3r_ShortestPath_hpp_ */