1 files changed, 60 insertions, 2 deletions

diff --git a/src/libslic3r/ShortestPath.cpp b/src/libslic3r/ShortestPath.cpp
index 0aa897fd7..3d5903df1 100644
--- a/src/libslic3r/ShortestPath.cpp
+++ b/src/libslic3r/ShortestPath.cpp
@@ -38,11 +38,12 @@ std::vector<std::pair<size_t, bool>> chain_segments_closest_point(std::vector<En
     	// 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, this_point.pos,
 			[this_idx, &end_points, &could_reverse_func](size_t idx) {
-				return (idx ^ this_idx) > 1 && end_points[idx].chain_id == 0 && ((idx ^ 1) == 0 || could_reverse_func(idx >> 1));
+				return (idx ^ this_idx) > 1 && end_points[idx].chain_id == 0 && ((idx & 1) == 0 || could_reverse_func(idx >> 1));
 		});
 		assert(next_idx < end_points.size());
 		EndPointType &end_point = end_points[next_idx];
 		end_point.chain_id = 1;
+		assert((next_idx & 1) == 0 || could_reverse_func(next_idx >> 1));
 		out.emplace_back(next_idx / 2, (next_idx & 1) != 0);
 		this_idx = next_idx ^ 1;
 	}
@@ -165,7 +166,9 @@ std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals
 		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->template cast<double>());
+            size_t idx = find_closest_point(kdtree, start_near->template cast<double>(),
+				// Don't start with a reverse segment, if flipping of the segment is not allowed.
+				[&could_reverse_func](size_t idx) { return (idx & 1) == 0 || could_reverse_func(idx >> 1); });
 			assert(idx < end_points.size());
 			first_point = &end_points[idx];
 			first_point->distance_out = 0.;
@@ -1970,4 +1973,59 @@ std::vector<const PrintInstance*> chain_print_object_instances(const Print &prin
 	return out;
 }
 
+Polylines chain_lines(const std::vector<Line> &lines, const double point_distance_epsilon)
+{
+    // Create line end point lookup.
+    struct LineEnd {
+        LineEnd(const Line *line, bool start) : line(line), start(start) {}
+        const Line      *line;
+        // Is it the start or end point?
+        bool             start;
+        const Point&     point() const { return start ? line->a : line->b; }
+        const Point&     other_point() const { return start ? line->b : line->a; }
+        LineEnd          other_end() const { return LineEnd(line, ! start); }
+        bool operator==(const LineEnd &rhs) const { return this->line == rhs.line && this->start == rhs.start; }
+    };
+    struct LineEndAccessor {
+        const Point* operator()(const LineEnd &pt) const { return &pt.point(); }
+    };
+    typedef ClosestPointInRadiusLookup<LineEnd, LineEndAccessor> ClosestPointLookupType;
+    ClosestPointLookupType closest_end_point_lookup(point_distance_epsilon);
+    for (const Line &line : lines) {
+        closest_end_point_lookup.insert(LineEnd(&line, true));
+        closest_end_point_lookup.insert(LineEnd(&line, false));
+    }
+
+    // Chain the lines.
+    std::vector<char> line_consumed(lines.size(), false);
+    static const double point_distance_epsilon2 = point_distance_epsilon * point_distance_epsilon;
+    Polylines out;
+    for (const Line &seed : lines)
+        if (! line_consumed[&seed - lines.data()]) {
+            line_consumed[&seed - lines.data()] = true;
+            closest_end_point_lookup.erase(LineEnd(&seed, false));
+            closest_end_point_lookup.erase(LineEnd(&seed, true));
+            Polyline pl { seed.a, seed.b };
+            for (size_t round = 0; round < 2; ++ round) {
+                for (;;) {
+                    auto [line_end, dist2] = closest_end_point_lookup.find(pl.last_point());
+                    if (line_end == nullptr || dist2 >= point_distance_epsilon2)
+                        // Cannot extent in this direction.
+                        break;
+                    // Average the last point.
+                    pl.points.back() = (0.5 * (pl.points.back().cast<double>() + line_end->point().cast<double>())).cast<coord_t>();
+                    // and extend with the new line segment.
+                    pl.points.emplace_back(line_end->other_point());
+                    closest_end_point_lookup.erase(*line_end);
+                    closest_end_point_lookup.erase(line_end->other_end());
+                    line_consumed[line_end->line - lines.data()] = true;
+                }
+                // reverse and try the oter direction.
+                pl.reverse();
+            }
+            out.emplace_back(std::move(pl));
+        }
+    return out;
+}
+
 } // namespace Slic3r