Speed up 2-opt island ordering on layers with thousands of islands

src/libslic3r/GCode/OrderingStrategies.cpp

@@ -4,6 +4,7 @@
 #include "OrderingStrategies.hpp"
 #include "../Geometry.hpp"
 #include "../ShortestPath.hpp"
+#include "libslic3r/KDTreeIndirect.hpp"
 #include "libslic3r/Point.hpp"
 #include "libslic3r/PrintConfig.hpp"
 #include "libslic3r/libslic3r.h"
@@ -54,47 +55,78 @@ bool tsp_2opt_improve(std::vector<size_t>& path, const Points& centers, int max_
     };
     std::vector<double> elsq = recompute_edges_sq();
 
+    // Reversing path[i+1..j] replaces edges (a, b) = (path[i], path[i+1]) and (c, d) = (path[j], path[j+1]) with
+    // (a, c) and (b, d). That can only shorten the cycle if a new edge is shorter than the old one it pairs with:
+    // |ac| < |ab| or |bd| < |cd|. Rather than test every pair, each pass finds those pairs with radius queries
+    // around a and d, which keeps a pass near linear on tours of thousands of nodes. It still applies the move a
+    // full scan would: the largest gain, and the lowest (i, j) among equal gains.
+    auto coordinate_fn = [&centers](size_t idx, size_t dimension) -> double { return double(centers[idx](dimension)); };
+    KDTreeIndirect<2, double, decltype(coordinate_fn)> kdtree(coordinate_fn, path);
+    // Position of each node in the path.
+    std::vector<size_t> pos(centers.size());
+    for (size_t k = 0; k < pn; ++k)
+        pos[path[k]] = k;
+
+    // Calls fn(node) for the nodes of the tour within sqrt(radius_sq) of center. The search radius is padded so that
+    // rounding cannot leave out a node the gain test accepts; that test makes the exact cut.
+    auto visit_within = [&](const Vec2d& center, double radius_sq, auto&& fn) {
+        const double search_sq = radius_sq * (1. + 1e-9) + 1.;
+        auto visitor = [&](size_t idx, size_t dimension) {
+            if ((centers[idx].cast<double>() - center).squaredNorm() < search_sq)
+                fn(idx);
+            return kdtree.descent_mask(center[dimension], search_sq, idx, dimension);
+        };
+        kdtree.visit(visitor);
+    };
+
+    size_t best_i = pn, best_j = pn;
+    double best_gain = 0;
+    auto consider = [&](size_t i, size_t j) {
+        // Skip the swap that would reverse the entire cycle (removes both
+        // edges (0,1) and (pn-1,0), equivalent to traversing the cycle backwards).
+        if (j < i + 2 || (i == 0 && j == pn - 1)) return;
+
+        const Vec2d& pi   = centers[path[i]].cast<double>();
+        const Vec2d& p_in = centers[path[i + 1]].cast<double>();
+        const Vec2d& pj   = centers[path[j]].cast<double>();
+        const Vec2d& p_jn = centers[path[(j + 1) % pn]].cast<double>();
+
+        // Early rejection using squared distances (avoids 2 sqrt calls).
+        double new_a_sq = (pj - pi).squaredNorm();
+        double new_b_sq = (p_jn - p_in).squaredNorm();
+        if (new_a_sq >= elsq[i] && new_b_sq >= elsq[j]) return;
+
+        double new_a = std::sqrt(new_a_sq);
+        double new_b = std::sqrt(new_b_sq);
+        double gain = el[i] + el[j] - new_a - new_b;
+
+        if (gain > best_gain || (gain == best_gain && best_i != pn && std::make_pair(i, j) < std::make_pair(best_i, best_j))) {
+            best_gain = gain;
+            best_i = i; best_j = j;
+        }
+    };
+
     bool improved = false;
     for (int pass = 0; max_passes <= 0 || pass < max_passes; ++pass) {
-        size_t best_i = pn, best_j = pn;
-        double best_gain = 0;
+        best_i = pn; best_j = pn;
+        best_gain = 0;
 
-        for (size_t i = 0; i < pn; ++i) {
-            const Vec2d& pi   = centers[path[i]].cast<double>();
-            const Vec2d& p_in = centers[path[(i + 1) % pn]].cast<double>();
-            double d_i = el[i];
-            double d_i_sq = elsq[i];
-
-            for (size_t j = i + 2; j < pn; ++j) {
-                size_t j_next = (j + 1) % pn;
-                // Skip the swap that would reverse the entire cycle (removes both
-                // edges (0,1) and (pn-1,0), equivalent to traversing the cycle backwards).
-                if (i == 0 && j_next == 0) continue;
-
-                const Vec2d& pj   = centers[path[j]].cast<double>();
-                const Vec2d& p_jn = centers[path[j_next]].cast<double>();
-                double d_j = el[j];
-
-                // Early rejection using squared distances (avoids 2 sqrt calls).
-                double new_a_sq = (pj - pi).squaredNorm();
-                double new_b_sq = (p_jn - p_in).squaredNorm();
-                if (new_a_sq >= d_i_sq && new_b_sq >= elsq[j]) continue;
-
-                double new_a = std::sqrt(new_a_sq);
-                double new_b = std::sqrt(new_b_sq);
-                double gain = d_i + d_j - new_a - new_b;
-
-                if (gain > best_gain) {
-                    best_gain = gain;
-                    best_i = i; best_j = j;
-                }
-            }
-        }
+        // |ac| < |ab|: c lies within the length of the edge leaving a.
+        for (size_t i = 0; i + 2 < pn; ++i)
+            visit_within(centers[path[i]].cast<double>(), elsq[i], [&](size_t c) { consider(i, pos[c]); });
+        // |bd| < |cd|: b lies within the length of the edge entering d.
+        for (size_t j = 2; j < pn; ++j)
+            visit_within(centers[path[(j + 1) % pn]].cast<double>(), elsq[j], [&](size_t b) {
+                if (pos[b] > 0)
+                    consider(pos[b] - 1, j);
+            });
 
         if (best_i == pn) break;
         improved = true;
         // Reverse the best swap segment
         std::reverse(path.begin() + best_i + 1, path.begin() + best_j + 1);
+        for (size_t k = best_i + 1; k <= best_j; ++k)
+            pos[path[k]] = k;
 
         // Recompute edge lengths after reversal
         el = recompute_edges();

src/libslic3r/GCode/OrderingStrategies.hpp

@@ -21,8 +21,8 @@ namespace Slic3r {
 
 // --- Path improvement (operate on index vectors into `centers`) ---
 
-// 2-opt improvement: reverses segments that reduce total cycle path length.
-// Returns true if any improvement was made.
+// 2-opt improvement: reverses segments that reduce total cycle path length. Each of up to max_passes passes
+// (unlimited if max_passes <= 0) applies the move that shortens the cycle most. Returns true if any improvement was made.
 bool tsp_2opt_improve(std::vector<size_t>& path, const Points& centers, int max_passes = 10);
 
 // Crossing removal: reverse each pair of intersecting edges whose reversal shortens the cycle by more

tests/libslic3r/test_ordering_strategies.cpp

@@ -21,6 +21,7 @@
 #include "libslic3r/ShortestPath.hpp"
 
 #include <algorithm>
+#include <numeric>
 #include <random>
 #include <unordered_set>
 #include <numeric>
@@ -77,6 +78,52 @@ static size_t count_shortening_crossings(const std::vector<size_t>& path, const
     return count;
 }
 
+// Exhaustive best-improvement 2-opt: each pass tests every pair of edges and applies the move with the largest
+// gain, the first (i, j) on a tie. tsp_2opt_improve must make the same moves.
+static bool exhaustive_2opt(std::vector<size_t>& path, const Points& centers, int max_passes)
+{
+    size_t pn = path.size();
+    if (pn <= 2) return false;
+    std::vector<double> el(pn), elsq(pn);
+    auto recompute = [&]() {
+        for (size_t i = 0; i < pn; ++i) {
+            el[i]   = (centers[path[i]].cast<double>() - centers[path[(i + 1) % pn]].cast<double>()).norm();
+            elsq[i] = (centers[path[i]].cast<double>() - centers[path[(i + 1) % pn]].cast<double>()).squaredNorm();
+        }
+    };
+    recompute();
+    bool improved = false;
+    for (int pass = 0; max_passes <= 0 || pass < max_passes; ++pass) {
+        size_t best_i = pn, best_j = pn;
+        double best_gain = 0;
+        for (size_t i = 0; i < pn; ++i) {
+            const Vec2d pi   = centers[path[i]].cast<double>();
+            const Vec2d p_in = centers[path[(i + 1) % pn]].cast<double>();
+            for (size_t j = i + 2; j < pn; ++j) {
+                size_t j_next = (j + 1) % pn;
+                if (i == 0 && j_next == 0) continue;
+                const Vec2d pj   = centers[path[j]].cast<double>();
+                const Vec2d p_jn = centers[path[j_next]].cast<double>();
+                double new_a_sq = (pj - pi).squaredNorm();
+                double new_b_sq = (p_jn - p_in).squaredNorm();
+                if (new_a_sq >= elsq[i] && new_b_sq >= elsq[j]) continue;
+                double new_a = std::sqrt(new_a_sq);
+                double new_b = std::sqrt(new_b_sq);
+                double gain = el[i] + el[j] - new_a - new_b;
+                if (gain > best_gain) {
+                    best_gain = gain;
+                    best_i = i; best_j = j;
+                }
+            }
+        }
+        if (best_i == pn) break;
+        improved = true;
+        std::reverse(path.begin() + best_i + 1, path.begin() + best_j + 1);
+        recompute();
+    }
+    return improved;
+}
+
 static bool is_permutation(const std::vector<size_t>& path, size_t n)
 {
     if (path.size() != n) return false;
@@ -148,6 +195,42 @@ TEST_CASE("tsp_2opt_improve reduces path length", "[TSPPostProcessing]") {
     CHECK(after <= before);
 }
 
+TEST_CASE("tsp_2opt_improve makes the same moves as an exhaustive best-improvement scan", "[TSPPostProcessing]") {
+    // 300 points in a 170000-unit square: random on a 1000-unit grid like island centroids snapped to 1 mm (many
+    // share a position or a line), random at full resolution, or a 20 x 15 lattice with a 1000-unit pitch, whose
+    // congruent cells give many moves exactly the same gain. The coordinates are small enough that every squared
+    // distance is an exact double, so both implementations compute bit-identical gains even where the compiler
+    // fuses multiply-adds. The tour is either the nearest-neighbour tour the island ordering starts from, or the
+    // input order, whose long edges make every radius query large. Both the default pass cap and an uncapped run
+    // to a 2-opt local optimum are compared.
+    enum class Layout { Snapped, Fine, Lattice };
+    const Layout layout  = GENERATE(Layout::Snapped, Layout::Fine, Layout::Lattice);
+    const bool nn_tour   = GENERATE(true, false);
+    const int max_passes = GENERATE(10, 0);
+    std::mt19937 rng(GENERATE(1u, 2u));
+    Points centers;
+    for (int i = 0; i < 300; ++i) {
+        if (layout == Layout::Lattice) {
+            centers.emplace_back(coord_t(i % 20) * 1000, coord_t(i / 20) * 1000);
+        } else {
+            const coord_t x = layout == Layout::Snapped ? coord_t(rng() % 171) * 1000 : coord_t(rng() % 170001);
+            const coord_t y = layout == Layout::Snapped ? coord_t(rng() % 171) * 1000 : coord_t(rng() % 170001);
+            centers.emplace_back(x, y);
+        }
+    }
+    std::vector<size_t> path(centers.size());
+    if (nn_tour)
+        path = chain_points(centers);
+    else
+        std::iota(path.begin(), path.end(), size_t(0));
+
+    std::vector<size_t> expected = path;
+    REQUIRE(exhaustive_2opt(expected, centers, max_passes));
+
+    CHECK(tsp_2opt_improve(path, centers, max_passes));
+    CHECK(path == expected);
+}
+
 TEST_CASE("tsp_remove_crossings eliminates crossings", "[TSPPostProcessing]") {
     Points centers = make_random_16();
     std::vector<size_t> path(centers.size());