From e65ab90c169bcd5df93b039610a64ebdf0a093b7 Mon Sep 17 00:00:00 2001
From: bubnikv <bubnikv@gmail.com>
Date: Fri, 27 Sep 2019 09:51:07 +0200
Subject: [PATCH] Fix of G-code path planning: Infill lines were incorrectly
 ordered for islands with another islands in their holes.

Improvement of chaining of infill lines for 3D honeycomb, Gyroid and
Honeycomb infill: New TSP chaining algorithm is used.
---
 src/libslic3r/Fill/Fill3DHoneycomb.cpp | 13 ++++-------
 src/libslic3r/Fill/FillGyroid.cpp      |  7 ++----
 src/libslic3r/Fill/FillHoneycomb.cpp   | 14 +++++------
 src/libslic3r/Fill/FillRectilinear.cpp | 13 ++++-------
 src/libslic3r/GCode.cpp                | 32 ++++++++++++++++++--------
 src/libslic3r/Polyline.cpp             |  6 -----
 src/libslic3r/Polyline.hpp             |  3 ++-
 src/libslic3r/ShortestPath.cpp         | 14 +++++++++++
 src/libslic3r/ShortestPath.hpp         |  2 ++
 9 files changed, 58 insertions(+), 46 deletions(-)

diff --git a/src/libslic3r/Fill/Fill3DHoneycomb.cpp b/src/libslic3r/Fill/Fill3DHoneycomb.cpp
index 6a37e4369..44267d3a4 100644
--- a/src/libslic3r/Fill/Fill3DHoneycomb.cpp
+++ b/src/libslic3r/Fill/Fill3DHoneycomb.cpp
@@ -1,5 +1,5 @@
 #include "../ClipperUtils.hpp"
-#include "../PolylineCollection.hpp"
+#include "../ShortestPath.hpp"
 #include "../Surface.hpp"
 
 #include "Fill3DHoneycomb.hpp"
@@ -175,27 +175,24 @@ void Fill3DHoneycomb::_fill_surface_single(
                 std::swap(expolygon_off, expolygons_off.front());
             }
         }
-        Polylines chained = PolylineCollection::chained_path_from(
-            std::move(polylines), 
-            PolylineCollection::leftmost_point(polylines), false); // reverse allowed
         bool first = true;
-        for (Polylines::iterator it_polyline = chained.begin(); it_polyline != chained.end(); ++ it_polyline) {
+        for (Polyline &polyline : chain_infill_polylines(std::move(polylines))) {
             if (! first) {
                 // Try to connect the lines.
                 Points &pts_end = polylines_out.back().points;
-                const Point &first_point = it_polyline->points.front();
+                const Point &first_point = polyline.points.front();
                 const Point &last_point = pts_end.back();
                 // TODO: we should also check that both points are on a fill_boundary to avoid 
                 // connecting paths on the boundaries of internal regions
                 if ((last_point - first_point).cast<double>().norm() <= 1.5 * distance && 
                     expolygon_off.contains(Line(last_point, first_point))) {
                     // Append the polyline.
-                    pts_end.insert(pts_end.end(), it_polyline->points.begin(), it_polyline->points.end());
+                    pts_end.insert(pts_end.end(), polyline.points.begin(), polyline.points.end());
                     continue;
                 }
             }
             // The lines cannot be connected.
-            polylines_out.emplace_back(std::move(*it_polyline));
+            polylines_out.emplace_back(std::move(polyline));
             first = false;
         }
     }
diff --git a/src/libslic3r/Fill/FillGyroid.cpp b/src/libslic3r/Fill/FillGyroid.cpp
index 04319bb26..09f0782bc 100644
--- a/src/libslic3r/Fill/FillGyroid.cpp
+++ b/src/libslic3r/Fill/FillGyroid.cpp
@@ -1,5 +1,5 @@
 #include "../ClipperUtils.hpp"
-#include "../PolylineCollection.hpp"
+#include "../ShortestPath.hpp"
 #include "../Surface.hpp"
 #include <cmath>
 #include <algorithm>
@@ -166,11 +166,8 @@ void FillGyroid::_fill_surface_single(
                 std::swap(expolygon_off, expolygons_off.front());
             }
         }
-        Polylines chained = PolylineCollection::chained_path_from(
-            std::move(polylines), 
-            PolylineCollection::leftmost_point(polylines), false); // reverse allowed
         bool first = true;
-        for (Polyline &polyline : chained) {
+        for (Polyline &polyline : chain_infill_polylines(std::move(polylines))) {
             if (! first) {
                 // Try to connect the lines.
                 Points &pts_end = polylines_out.back().points;
diff --git a/src/libslic3r/Fill/FillHoneycomb.cpp b/src/libslic3r/Fill/FillHoneycomb.cpp
index cbfe926f2..be569c2d8 100644
--- a/src/libslic3r/Fill/FillHoneycomb.cpp
+++ b/src/libslic3r/Fill/FillHoneycomb.cpp
@@ -1,5 +1,5 @@
 #include "../ClipperUtils.hpp"
-#include "../PolylineCollection.hpp"
+#include "../ShortestPath.hpp"
 #include "../Surface.hpp"
 
 #include "FillHoneycomb.hpp"
@@ -93,22 +93,20 @@ void FillHoneycomb::_fill_surface_single(
 
         // connect paths
         if (! paths.empty()) { // prevent calling leftmost_point() on empty collections
-            Polylines chained = PolylineCollection::chained_path_from(
-                std::move(paths), 
-                PolylineCollection::leftmost_point(paths), false);
+            Polylines chained = chain_infill_polylines(std::move(paths));
             assert(paths.empty());
             paths.clear();
-            for (Polylines::iterator it_path = chained.begin(); it_path != chained.end(); ++ it_path) {
+            for (Polyline &path : chained) {
                 if (! paths.empty()) {
                     // distance between first point of this path and last point of last path
-                    double distance = (it_path->first_point() - paths.back().last_point()).cast<double>().norm();
+                    double distance = (path.first_point() - paths.back().last_point()).cast<double>().norm();
                     if (distance <= m.hex_width) {
-                        paths.back().points.insert(paths.back().points.end(), it_path->points.begin(), it_path->points.end());
+                        paths.back().points.insert(paths.back().points.end(), path.points.begin(), path.points.end());
                         continue;
                     }
                 }
                 // Don't connect the paths.
-                paths.push_back(*it_path);
+                paths.push_back(std::move(path));
             }
         }
         
diff --git a/src/libslic3r/Fill/FillRectilinear.cpp b/src/libslic3r/Fill/FillRectilinear.cpp
index 205eb1b66..d535ba29b 100644
--- a/src/libslic3r/Fill/FillRectilinear.cpp
+++ b/src/libslic3r/Fill/FillRectilinear.cpp
@@ -1,6 +1,6 @@
 #include "../ClipperUtils.hpp"
 #include "../ExPolygon.hpp"
-#include "../PolylineCollection.hpp"
+#include "../ShortestPath.hpp"
 #include "../Surface.hpp"
 
 #include "FillRectilinear.hpp"
@@ -92,15 +92,12 @@ void FillRectilinear::_fill_surface_single(
                 std::swap(expolygon_off, expolygons_off.front());
             }
         }
-        Polylines chained = PolylineCollection::chained_path_from(
-            std::move(polylines), 
-            PolylineCollection::leftmost_point(polylines), false); // reverse allowed
         bool first = true;
-        for (Polylines::iterator it_polyline = chained.begin(); it_polyline != chained.end(); ++ it_polyline) {
+        for (Polyline &polyline : chain_infill_polylines(std::move(polylines))) {
             if (! first) {
                 // Try to connect the lines.
                 Points &pts_end = polylines_out.back().points;
-                const Point &first_point = it_polyline->points.front();
+                const Point &first_point = polyline.points.front();
                 const Point &last_point = pts_end.back();
                 // Distance in X, Y.
                 const Vector distance = last_point - first_point;
@@ -109,12 +106,12 @@ void FillRectilinear::_fill_surface_single(
                 if (this->_can_connect(std::abs(distance(0)), std::abs(distance(1))) && 
                     expolygon_off.contains(Line(last_point, first_point))) {
                     // Append the polyline.
-                    pts_end.insert(pts_end.end(), it_polyline->points.begin(), it_polyline->points.end());
+                    pts_end.insert(pts_end.end(), polyline.points.begin(), polyline.points.end());
                     continue;
                 }
             }
             // The lines cannot be connected.
-            polylines_out.emplace_back(std::move(*it_polyline));
+            polylines_out.emplace_back(std::move(polyline));
             first = false;
         }
     }
diff --git a/src/libslic3r/GCode.cpp b/src/libslic3r/GCode.cpp
index 8bd42e59a..82a4995c5 100644
--- a/src/libslic3r/GCode.cpp
+++ b/src/libslic3r/GCode.cpp
@@ -1807,6 +1807,17 @@ void GCode::process_layer(
             layer_surface_bboxes.reserve(n_slices);
             for (const ExPolygon &expoly : layer.slices.expolygons)
                 layer_surface_bboxes.push_back(get_extents(expoly.contour));
+            // Traverse the slices in an increasing order of bounding box size, so that the islands inside another islands are tested first,
+            // so we can just test a point inside ExPolygon::contour and we may skip testing the holes.
+            std::vector<size_t> slices_test_order;
+            slices_test_order.reserve(n_slices);
+            for (size_t i = 0; i < n_slices; ++ i)
+            	slices_test_order.emplace_back(i);
+            std::sort(slices_test_order.begin(), slices_test_order.end(), [&layer_surface_bboxes](int i, int j) {
+            	const Vec2d s1 = layer_surface_bboxes[i].size().cast<double>();
+            	const Vec2d s2 = layer_surface_bboxes[j].size().cast<double>();
+            	return s1.x() * s1.y() < s2.x() * s2.y();
+            });
             auto point_inside_surface = [&layer, &layer_surface_bboxes](const size_t i, const Point &point) { 
                 const BoundingBox &bbox = layer_surface_bboxes[i];
                 return point(0) >= bbox.min(0) && point(0) < bbox.max(0) &&
@@ -1854,16 +1865,19 @@ void GCode::process_layer(
                                     extruder,
                                     &layer_to_print - layers.data(),
                                     layers.size(), n_slices+1);
-                                for (size_t i = 0; i <= n_slices; ++i)
+                                for (size_t i = 0; i <= n_slices; ++ i) {
+									bool   last = i == n_slices;
+                                	size_t island_idx = last ? n_slices : slices_test_order[i];
                                     if (// fill->first_point does not fit inside any slice
-                                        i == n_slices ||
+										last ||
                                         // fill->first_point fits inside ith slice
-                                        point_inside_surface(i, fill->first_point())) {
-                                        if (islands[i].by_region.empty())
-                                            islands[i].by_region.assign(print.regions().size(), ObjectByExtruder::Island::Region());
-                                        islands[i].by_region[region_id].append(entity_type, fill, entity_overrides, layer_to_print.object()->copies().size());
+                                        point_inside_surface(island_idx, fill->first_point())) {
+                                        if (islands[island_idx].by_region.empty())
+                                            islands[island_idx].by_region.assign(print.regions().size(), ObjectByExtruder::Island::Region());
+                                        islands[island_idx].by_region[region_id].append(entity_type, fill, entity_overrides, layer_to_print.object()->copies().size());
                                         break;
                                     }
+                                }
                             }
                         }
                     }
@@ -2574,12 +2588,10 @@ std::string GCode::extrude_infill(const Print &print, const std::vector<ObjectBy
     std::string gcode;
     for (const ObjectByExtruder::Island::Region &region : by_region) {
         m_config.apply(print.regions()[&region - &by_region.front()]->config());
-		ExtrusionEntityCollection chained = region.infills.chained_path_from(m_last_pos, false);
-        for (ExtrusionEntity *fill : chained.entities) {
+        for (ExtrusionEntity *fill : region.infills.chained_path_from(m_last_pos, false).entities) {
             auto *eec = dynamic_cast<ExtrusionEntityCollection*>(fill);
             if (eec) {
-				ExtrusionEntityCollection chained2 = eec->chained_path_from(m_last_pos, false);
-				for (ExtrusionEntity *ee : chained2.entities)
+				for (ExtrusionEntity *ee : eec->chained_path_from(m_last_pos, false).entities)
                     gcode += this->extrude_entity(*ee, "infill");
             } else
                 gcode += this->extrude_entity(*fill, "infill");
diff --git a/src/libslic3r/Polyline.cpp b/src/libslic3r/Polyline.cpp
index af155468a..531383ae1 100644
--- a/src/libslic3r/Polyline.cpp
+++ b/src/libslic3r/Polyline.cpp
@@ -23,12 +23,6 @@ Polyline::operator Line() const
     return Line(this->points.front(), this->points.back());
 }
 
-Point
-Polyline::last_point() const
-{
-    return this->points.back();
-}
-
 Point
 Polyline::leftmost_point() const
 {
diff --git a/src/libslic3r/Polyline.hpp b/src/libslic3r/Polyline.hpp
index e17fafd62..d595f252d 100644
--- a/src/libslic3r/Polyline.hpp
+++ b/src/libslic3r/Polyline.hpp
@@ -62,7 +62,8 @@ public:
 
     operator Polylines() const;
     operator Line() const;
-    Point last_point() const;
+    Point    last_point() const override { return this->points.back(); }
+
     Point leftmost_point() const;
     virtual Lines lines() const;
     void clip_end(double distance);
diff --git a/src/libslic3r/ShortestPath.cpp b/src/libslic3r/ShortestPath.cpp
index 61762b66d..b31a9d1f3 100644
--- a/src/libslic3r/ShortestPath.cpp
+++ b/src/libslic3r/ShortestPath.cpp
@@ -339,6 +339,20 @@ std::vector<size_t> chain_points(const Points &points, Point *start_near)
 	return out;
 }
 
+Polylines chain_infill_polylines(Polylines &polylines)
+{
+	auto segment_end_point = [&polylines](size_t idx, bool first_point) -> const Point& { return first_point ? polylines[idx].first_point() : polylines[idx].last_point(); };
+	std::vector<std::pair<size_t, bool>> ordered = chain_segments<Point, decltype(segment_end_point)>(segment_end_point, polylines.size(), nullptr);
+	Polylines out;
+	out.reserve(polylines.size());
+	for (auto &segment_and_reversal : ordered) {
+		out.emplace_back(std::move(polylines[segment_and_reversal.first]));
+		if (segment_and_reversal.second)
+			out.back().reverse();
+	}
+	return out;	
+}
+
 template<class T> static inline T chain_path_items(const Points &points, const T &items)
 {
 	auto segment_end_point = [&points](size_t idx, bool /* first_point */) -> const Point& { return points[idx]; };
diff --git a/src/libslic3r/ShortestPath.hpp b/src/libslic3r/ShortestPath.hpp
index 3a1b1af13..b4d0b4737 100644
--- a/src/libslic3r/ShortestPath.hpp
+++ b/src/libslic3r/ShortestPath.hpp
@@ -18,6 +18,8 @@ std::vector<std::pair<size_t, bool>> chain_extrusion_entities(std::vector<Extrus
 void                                 reorder_extrusion_entities(std::vector<ExtrusionEntity*> &entities, std::vector<std::pair<size_t, bool>> &chain);
 void                                 chain_and_reorder_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point *start_near = nullptr);
 
+Polylines 							 chain_infill_polylines(Polylines &src);
+
 std::vector<ClipperLib::PolyNode*>	 chain_clipper_polynodes(const Points &points, const std::vector<ClipperLib::PolyNode*> &items);
 
 // Chain instances of print objects by an approximate shortest path.