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

@@ -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

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<size_t>* 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<size_t>* 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<ExtrusionEntity*,size_t> 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)
+//        if (orig_indices != nullptr)
-        	indices_map[entity] = &entity_src - &this->entities.front();
+//        	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)
+//        if (orig_indices != nullptr)
-        	orig_indices->push_back(indices_map[entity]);
+//        	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();

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<size_t> 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(const ExtrusionEntityCollection &other) : 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(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<size_t>* 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<size_t>* 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(); }

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 <algorithm>
+#include <limits>
+#include <vector>
+
+#include "Utils.hpp" // for next_highest_power_of_2()
+
+namespace Slic3r {
+
+// KD tree for N-dimensional closest point search.
+template<size_t ANumDimensions, typename ACoordType, typename ACoordinateFn>
+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<size_t>   indices) : coordinate(coordinate) { this->build(std::move(indices)); }
+	KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t> &&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<size_t> 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<size_t> &&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<typename CoordType> 
+	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<typename Visitor>
+	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<size_t> &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 <left, right> at k using QuickSelect method.
+	// https://en.wikipedia.org/wiki/Quickselect
+	void partition_input(std::vector<size_t> &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<typename Visitor>
+	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<size_t> m_nodes;
+};
+
+// Find a closest point using Euclidian metrics.
+// Returns npos if not found.
+template<typename KDTreeIndirectType, typename PointType, typename FilterFn>
+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<CoordType>::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<typename KDTreeIndirectType, typename PointType>
+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_ */

src/libslic3r/MutablePriorityQueue.hpp

@@ -13,21 +13,28 @@ public:
 		{}
 	~MutablePriorityQueue()	{ clear(); }
 
-	inline void		clear()								{ m_heap.clear(); }
+	void		clear()								{ m_heap.clear(); }
-	inline void		reserve(size_t cnt) 				{ m_heap.reserve(cnt); }
+	void		reserve(size_t cnt) 				{ m_heap.reserve(cnt); }
-	inline void		push(const T &item);
+	void		push(const T &item);
-	inline void		push(T &&item);
+	void		push(T &&item);
-	inline void		pop();
+	void		pop();
-	inline T&		top()								{ return m_heap.front(); }
+	T&			top()								{ return m_heap.front(); }
-	inline void		remove(size_t idx);
+	void		remove(size_t idx);
-	inline void		update(size_t idx) 					{ T item = m_heap[idx]; remove(idx); push(item); }
+	void		update(size_t idx) 					{ T item = m_heap[idx]; remove(idx); push(item); }
 
-	inline size_t	size() const						{ return m_heap.size(); }
+	size_t		size() const						{ return m_heap.size(); }
-	inline bool		empty() const						{ return m_heap.empty(); }
+	bool		empty() const						{ return m_heap.empty(); }
+
+	using iterator		 = typename std::vector<T>::iterator;
+	using const_iterator = typename std::vector<T>::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);
+	void		update_heap_up(size_t top, size_t bottom);
-	inline void		update_heap_down(size_t top, size_t bottom);
+	void		update_heap_down(size_t top, size_t bottom);
 
 private:
 	std::vector<T>	m_heap;

src/libslic3r/PerimeterGenerator.cpp

@@ -1,6 +1,8 @@
 #include "PerimeterGenerator.hpp"
 #include "ClipperUtils.hpp"
 #include "ExtrusionEntityCollection.hpp"
+#include "ShortestPath.hpp"
+
 #include <cmath>
 #include <cassert>
 
@@ -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<ExtrusionEntity*> &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)));
+				out.emplace_back(new ExtrusionLoop(std::move(paths)));
-			else
+			else {
-				coll.append(std::move(paths));
+				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);
+        variable_width(thin_walls, erExternalPerimeter, perimeter_generator.ext_perimeter_flow, coll.entities);
-        coll.append(tw.entities);
         thin_walls.clear();
     }
     
-    // Sort entities into a new collection using a nearest-neighbor search,
+    // Traverse children and build the final collection.
-    // preserving the original indices which are useful for detecting thin walls.
+	Point zero_point(0, 0);
-    ExtrusionEntityCollection sorted_coll;
+	std::vector<std::pair<size_t, bool>> chain = chain_extrusion_entities(coll.entities, &zero_point);
-    coll.chained_path(&sorted_coll, false, erMixed, &sorted_coll.orig_indices);
+    ExtrusionEntityCollection out;
-    
+    for (const std::pair<size_t, bool> &idx : chain) {
-    // traverse children and build the final collection
+		assert(coll.entities[idx.first] != nullptr);
-    ExtrusionEntityCollection entities;
+        if (idx.first >= loops.size()) {
-    for (const size_t &idx : sorted_coll.orig_indices) {
+            // This is a thin wall.
-        if (idx >= loops.size()) {
+			out.entities.reserve(out.entities.size() + 1);
-            // This is a thin wall. Let's get it from the sorted collection as it might have been reversed.
+            out.entities.emplace_back(coll.entities[idx.first]);
-            entities.append(std::move(*sorted_coll.entities[&idx - &sorted_coll.orig_indices.front()]));
+			coll.entities[idx.first] = nullptr;
+            if (idx.second)
+				out.entities.back()->reverse();
         } else {
-            const PerimeterGeneratorLoop &loop = loops[idx];
+            const PerimeterGeneratorLoop &loop = loops[idx.first];
-            ExtrusionLoop eloop = *dynamic_cast<ExtrusionLoop*>(coll.entities[idx]);
+            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<ExtrusionLoop*>(coll.entities[idx.first]);
+            coll.entities[idx.first] = nullptr;
             if (loop.is_contour) {
-                eloop.make_counter_clockwise();
+                eloop->make_counter_clockwise();
-                entities.append(std::move(children.entities));
+                out.append(std::move(children.entities));
-                entities.append(std::move(eloop));
+                out.entities.emplace_back(eloop);
             } else {
-                eloop.make_clockwise();
+                eloop->make_clockwise();
-                entities.append(std::move(eloop));
+                out.entities.emplace_back(eloop);
-                entities.append(std::move(children.entities));
+                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);
+				ExtrusionEntityCollection gap_fill;
-                this->gap_fill->append(gap_fill.entities);
+				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

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 <cmath>
+#include <cassert>
+
+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<std::pair<size_t, bool>> chain_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point *start_near)
+{
+	std::vector<std::pair<size_t, bool>> 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<double>().squaredNorm() < (extrusion_entity->first_point() - *start_near).cast<double>().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<const EndPoint*> 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<double>::max();
+			size_t    heap_idx = std::numeric_limits<size_t>::max();
+		};
+	    std::vector<EndPoint> end_points;
+	    end_points.reserve(entities.size() * 2);
+	    for (const ExtrusionEntity* const &entity : entities) {
+	    	end_points.emplace_back(entity->first_point().cast<double>());
+	    	end_points.emplace_back(entity->last_point().cast<double>());
+	    }
+
+	    // 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<size_t> 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<size_t>::max();
+		if (start_near != nullptr) {
+			size_t idx = find_closest_point(kdtree, start_near->cast<double>());
+			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*>(
+			[](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<size_t>::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<size_t>::max());
+						// Detect loops.
+						for (EndPoint *pt = &ep; pt != nullptr;) {
+							// Out of queue. It is a final point.
+							assert(pt->heap_idx == std::numeric_limits<size_t>::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<EndPoint*> 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<size_t>::max();
+				end_point2.heap_idx = std::numeric_limits<size_t>::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

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 <utility>
+#include <vector>
+
+namespace Slic3r {
+
+std::vector<std::pair<size_t, bool>> chain_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point *start_near = nullptr);
+
+} // namespace Slic3r
+
+#endif /* slic3r_ShortestPath_hpp_ */