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

src/libslic3r/GCode/OrderingStrategies.cpp

@@ -4,8 +4,10 @@
 #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"
 
 #include <algorithm>
 #include <cassert>
@@ -53,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;
+        best_i = pn; best_j = pn;
-        double best_gain = 0;
+        best_gain = 0;
 
-        for (size_t i = 0; i < pn; ++i) {
+        // |ac| < |ab|: c lies within the length of the edge leaving a.
-            const Vec2d& pi   = centers[path[i]].cast<double>();
+        for (size_t i = 0; i + 2 < pn; ++i)
-            const Vec2d& p_in = centers[path[(i + 1) % pn]].cast<double>();
+            visit_within(centers[path[i]].cast<double>(), elsq[i], [&](size_t c) { consider(i, pos[c]); });
-            double d_i = el[i];
+        // |bd| < |cd|: b lies within the length of the edge entering d.
-            double d_i_sq = elsq[i];
+        for (size_t j = 2; j < pn; ++j)
-
+            visit_within(centers[path[(j + 1) % pn]].cast<double>(), elsq[j], [&](size_t b) {
-            for (size_t j = i + 2; j < pn; ++j) {
+                if (pos[b] > 0)
-                size_t j_next = (j + 1) % pn;
+                    consider(pos[b] - 1, 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 (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;
-                }
-            }
-        }
 
         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();
@@ -120,9 +153,9 @@ bool tsp_remove_crossings(std::vector<size_t>& path, const Points& centers)
     // TSP helpers (2-opt, closing-edge rotation) that operate on the full cycle.
     size_t n_edges = pn;
 
-    // Scan for first crossing; returns {i, j} or {npos, npos} if none.
+    // Scan from edge `from` for the first pair of intersecting edges worth reversing; returns {i, j} or {npos, npos} if none.
-    auto find_crossing = [&]() -> std::pair<size_t, size_t> {
+    auto find_crossing = [&](size_t from) -> std::pair<size_t, size_t> {
-        for (size_t i = 0; i < n_edges; ++i) {
+        for (size_t i = from; i < n_edges; ++i) {
             const Point& ai = centers[path[i]];
             const Point& bi = centers[path[(i + 1) % pn]];
 
@@ -133,23 +166,40 @@ bool tsp_remove_crossings(std::vector<size_t>& path, const Points& centers)
                 const Point& aj = centers[path[j]];
                 const Point& bj = centers[path[(j + 1) % pn]];
 
-                if (!bboxes_overlap(ai, bi, aj, bj)) continue;
+                if (!bboxes_overlap(ai, bi, aj, bj) || !Geometry::segments_intersect(ai, bi, aj, bj))
-                if (Geometry::segments_intersect(ai, bi, aj, bj))
+                    continue;
+                // The reversal replaces edges (ai, bi) and (aj, bj) with (ai, aj) and (bi, bj). Edges that only touch
+                // or overlap collinearly, as at points sharing a position, may gain nothing from it, and reversing
+                // them would find the same pair again on every scan. A gain within SCALED_EPSILON, which nearly
+                // collinear crossings can have too, is not worth a reversal either.
+                if (ai.distance_to(bi) + aj.distance_to(bj) - ai.distance_to(aj) - bi.distance_to(bj) > SCALED_EPSILON)
                     return {i, j};
             }
         }
         return {std::numeric_limits<size_t>::max(), std::numeric_limits<size_t>::max()};
     };
 
-    // Process crossings one at a time: find first, reverse it, restart scan.
+    // Process crossings one at a time. Reversing path[ci+1..cj] replaces only edges ci and cj; the edges between
-    // Cap iterations to prevent infinite loops on collinear/overlapping segments.
+    // them are the same segments traversed backwards. The scan therefore resumes at edge ci rather than at edge 0,
-    int max_iters = static_cast<int>(pn * pn);
+    // so a pass over the tour costs one quadratic scan however many reversals it makes. Pairs before ci that
+    // involve a replaced edge are new too, so a scan that reaches the end starts over from edge 0, and the loop
+    // ends when a scan from edge 0 finds nothing.
+    // Every reversal shortens the cycle, so no tour repeats and the loop ends; the cap is a safety net.
+    size_t max_reversals = pn * pn;
     bool improved = false;
-    while (max_iters-- > 0) {
+    for (size_t from = 0;;) {
-        auto [ci, cj] = find_crossing();
+        auto [ci, cj] = find_crossing(from);
-        if (ci == std::numeric_limits<size_t>::max()) break;
+        if (ci == std::numeric_limits<size_t>::max()) {
+            if (from == 0)
+                break;
+            from = 0;
+            continue;
+        }
         improved = true;
         std::reverse(path.begin() + ci + 1, path.begin() + cj + 1);
+        if (--max_reversals == 0)
+            break;
+        from = ci;
     }
     return improved;
 }

src/libslic3r/GCode/OrderingStrategies.hpp

@@ -21,12 +21,12 @@ namespace Slic3r {
 
 // --- Path improvement (operate on index vectors into `centers`) ---
 
-// 2-opt improvement: reverses segments that reduce total cycle path length.
+// 2-opt improvement: reverses segments that reduce total cycle path length. Each of up to max_passes passes
-// Returns true if any improvement was made.
+// (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 any segment pair whose edges geometrically cross.
+// Crossing removal: reverse each pair of intersecting edges whose reversal shortens the cycle by more
-// Returns true if any crossing was removed.
+// than SCALED_EPSILON. Returns true if any pair was reversed.
 bool tsp_remove_crossings(std::vector<size_t>& path, const Points& centers);
 
 // The shortest cycle through the points, starting at point 0, by dynamic programming over subsets; for a few points only.

tests/libslic3r/test_ordering_strategies.cpp

@@ -1,6 +1,9 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/generators/catch_generators.hpp>
 #include <catch2/catch_all.hpp>
+#include <catch2/generators/catch_generators.hpp>
+#include <catch2/matchers/catch_matchers.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
 #include <vector>
 #include <cstddef>
 #include <math.h>
@@ -15,8 +18,11 @@
 #include "libslic3r/GCode/OrderingStrategies.hpp"
 #include "libslic3r/ShortestPath.hpp"
 #include "libslic3r/Geometry.hpp"
+#include "libslic3r/ShortestPath.hpp"
 
 #include <algorithm>
+#include <numeric>
+#include <random>
 #include <unordered_set>
 #include <numeric>
 #include <random>
@@ -51,6 +57,73 @@ static bool has_crossings(const std::vector<size_t>& path, const Points& centers
     return false;
 }
 
+// Intersecting edge pairs whose reversal would shorten the cycle by more than SCALED_EPSILON, which
+// tsp_remove_crossings must leave none of.
+static size_t count_shortening_crossings(const std::vector<size_t>& path, const Points& centers)
+{
+    size_t pn = path.size();
+    size_t count = 0;
+    for (size_t i = 0; i < pn; ++i) {
+        for (size_t j = i + 2; j < pn; ++j) {
+            if (i == 0 && j == pn - 1) continue;
+            const Point& ai = centers[path[i]];
+            const Point& bi = centers[path[(i + 1) % pn]];
+            const Point& aj = centers[path[j]];
+            const Point& bj = centers[path[(j + 1) % pn]];
+            if (Geometry::segments_intersect(ai, bi, aj, bj) &&
+                ai.distance_to(bi) + aj.distance_to(bj) - ai.distance_to(aj) - bi.distance_to(bj) > SCALED_EPSILON)
+                ++count;
+        }
+    }
+    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;
@@ -122,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());
@@ -136,6 +245,76 @@ TEST_CASE("tsp_remove_crossings eliminates crossings", "[TSPPostProcessing]") {
     REQUIRE(is_permutation(path, centers.size()));
 }
 
+TEST_CASE("tsp_remove_crossings leaves a tour alone when no reversal shortens it", "[TSPPostProcessing]") {
+    // Three points on a diagonal, the middle one doubled as island centroids snapped to the same 1 mm cell are,
+    // and a corner closing the tour. Edges 0-1 and 2-3 touch at the doubled point, and reversing between them
+    // only swaps the two copies.
+    Points centers;
+    centers.emplace_back(0, 0);
+    centers.emplace_back(3000000, 3000000);
+    centers.emplace_back(3000000, 3000000);
+    centers.emplace_back(6000000, 6000000);
+    centers.emplace_back(6000000, 0);
+    std::vector<size_t> path = {0, 1, 2, 3, 4};
+    REQUIRE(Geometry::segments_intersect(centers[0], centers[1], centers[2], centers[3]));
+
+    CHECK(!tsp_remove_crossings(path, centers));
+    CHECK(path == std::vector<size_t>{0, 1, 2, 3, 4});
+}
+
+TEST_CASE("tsp_remove_crossings keeps a crossing whose reversal gains less than SCALED_EPSILON", "[TSPPostProcessing]") {
+    // Two 100 mm edges crossing at a 10 nm offset: uncrossing them saves about 1e-6 scaled units of travel.
+    Points centers;
+    centers.emplace_back(0, 0);
+    centers.emplace_back(100000000, 10);
+    centers.emplace_back(100000000, 0);
+    centers.emplace_back(0, 10);
+    std::vector<size_t> path = {0, 1, 2, 3};
+    REQUIRE(Geometry::segments_intersect(centers[0], centers[1], centers[2], centers[3]));
+
+    CHECK(!tsp_remove_crossings(path, centers));
+    CHECK(path == std::vector<size_t>{0, 1, 2, 3});
+}
+
+TEST_CASE("tsp_remove_crossings reverses overlapping collinear edges when that shortens the tour", "[TSPPostProcessing]") {
+    // Points at x = 0, 3, 1 and 2 mm on one line. Tour 0-3-1-2 is 8 mm long, and its edges [0,3] and [1,2] overlap.
+    // The shortest tour through points on a line is twice their 3 mm span.
+    Points centers;
+    centers.emplace_back(0, 0);
+    centers.emplace_back(3000000, 0);
+    centers.emplace_back(1000000, 0);
+    centers.emplace_back(2000000, 0);
+    std::vector<size_t> path = {0, 1, 2, 3};
+    REQUIRE_THAT(tsp_cycle_path_length(path, centers), Catch::Matchers::WithinAbs(8000000., 1.));
+
+    CHECK(tsp_remove_crossings(path, centers));
+    REQUIRE(is_permutation(path, centers.size()));
+    CHECK_THAT(tsp_cycle_path_length(path, centers), Catch::Matchers::WithinAbs(6000000., 1.));
+}
+
+TEST_CASE("tsp_remove_crossings leaves no shortening crossing in a tour of thousands of grid-snapped points", "[TSPPostProcessing]") {
+    // 4000 island centroids on a 170 mm plate, snapped to 1 mm as GCode::process_layer does, so many of them share
+    // a position or a line. The nearest-neighbour tour the island ordering starts from has crossings to remove.
+    std::mt19937 rng(GENERATE(1u, 2u, 3u));
+    Points centers;
+    for (int i = 0; i < 4000; ++i) {
+        const double x = double(rng() % 171);
+        const double y = double(rng() % 171);
+        centers.push_back(Point::new_scale(x, y));
+    }
+    std::vector<size_t> path = chain_points(centers);
+    const size_t first = path.front();
+    const double length_before = tsp_cycle_path_length(path, centers);
+    REQUIRE(count_shortening_crossings(path, centers) > 0);
+
+    CHECK(tsp_remove_crossings(path, centers));
+    REQUIRE(is_permutation(path, centers.size()));
+    CHECK(count_shortening_crossings(path, centers) == 0);
+    CHECK(tsp_cycle_path_length(path, centers) < length_before);
+    // Callers that start the tour near a given point rely on the first node staying in place.
+    CHECK(path.front() == first);
+}
+
 
 
 TEST_CASE("tsp_rotate_minimize_closing shortens closing edge", "[TSPPostProcessing]") {