Merge remote-tracking branch 'origin/main' into fix/island-ordering-stall

# Conflicts:
#	src/libslic3r/GCode/OrderingStrategies.cpp

src/libslic3r/GCode/OrderingStrategies.cpp

@@ -4,6 +4,8 @@
 #include "OrderingStrategies.hpp"
 #include "../Geometry.hpp"
 #include "../ShortestPath.hpp"
+#include "libslic3r/BoundingBox.hpp"
+#include "libslic3r/KDTreeIndirect.hpp"
 #include "libslic3r/Point.hpp"
 #include "libslic3r/PrintConfig.hpp"
 #include "libslic3r/libslic3r.h"
@@ -54,47 +56,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();
@@ -121,31 +154,38 @@ 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.
-    auto find_crossing = [&]() -> std::pair<size_t, size_t> {
-        for (size_t i = 0; i < n_edges; ++i) {
-            const Point& ai = centers[path[i]];
-            const Point& bi = centers[path[(i + 1) % pn]];
+    constexpr size_t npos = std::numeric_limits<size_t>::max();
 
-            for (size_t j = i + 2; j < n_edges; ++j) {
-                // Skip the (0, pn-1) pair: edges (0,1) and (pn-1,0) share node 0.
-                if (i == 0 && j == pn - 1) continue;
-
-                const Point& aj = centers[path[j]];
-                const Point& bj = centers[path[(j + 1) % pn]];
-
-                if (!bboxes_overlap(ai, bi, aj, bj)) continue;
-                if (Geometry::segments_intersect(ai, bi, aj, bj))
-                    return {i, j};
-            }
-        }
-        return {std::numeric_limits<size_t>::max(), std::numeric_limits<size_t>::max()};
+    // Whether edges i and j intersect and reversing path[i+1..j] is worth it. 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.
+    auto worth_reversing = [&](size_t i, size_t j) {
+        // Skip the (0, pn-1) pair: edges (0,1) and (pn-1,0) share node 0.
+        if (i == 0 && j == pn - 1)
+            return false;
+        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]];
+        return bboxes_overlap(ai, bi, aj, bj) && 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;
     };
 
-    // For many islands, the same scan with the edges binned in a uniform grid over their boxes, so each edge is only tested against the edges sharing a
-    // cell with it - two edges whose boxes overlap always do. It returns the same crossing as the all-pairs scan
-    // (smallest i, then smallest j), so the result is unchanged. The all-pairs scan is quadratic in the edge count and
-    // runs again after every reversal; rebuilding the grid costs more than it saves below the threshold.
+    // Scan from edge `from` for the first pair worth reversing, smallest i and then smallest j; returns {i, j} or
+    // {npos, npos} if none.
+    auto find_crossing = [&](size_t from) -> std::pair<size_t, size_t> {
+        for (size_t i = from; i < n_edges; ++i)
+            for (size_t j = i + 2; j < n_edges; ++j)
+                if (worth_reversing(i, j))
+                    return {i, j};
+        return {npos, npos};
+    };
+
+    // For many islands, the same scan with the edges binned in a uniform grid over their boxes, so each edge is only
+    // tested against the edges sharing a cell with it - two edges whose boxes overlap always do. It returns the same
+    // pair as find_crossing(), so the result is unchanged. The all-pairs scan is quadratic in the edge count; rebuilding
+    // the grid costs more than it saves below the threshold.
     constexpr size_t grid_min_size = 500;
     BoundingBox extent;
     for (size_t idx : path)
@@ -162,67 +202,46 @@ bool tsp_remove_crossings(std::vector<size_t>& path, const Points& centers)
     };
     std::vector<std::vector<size_t>> edge_cells(size_t(grid_n) * grid_n);
 
-    auto find_crossing_grid = [&]() -> std::pair<size_t, size_t> {
+    auto find_crossing_grid = [&](size_t from) -> std::pair<size_t, size_t> {
         for (std::vector<size_t>& cell : edge_cells)
             cell.clear();
         for (size_t j = 0; j < n_edges; ++j)
             for_cells(centers[path[j]], centers[path[(j + 1) % pn]], [&](int cell) { edge_cells[cell].emplace_back(j); });
 
-        for (size_t i = 0; i < n_edges; ++i) {
-            const Point& ai = centers[path[i]];
-            const Point& bi = centers[path[(i + 1) % pn]];
-
-            size_t first_j = std::numeric_limits<size_t>::max();
-            for_cells(ai, bi, [&](int cell) {
-                for (size_t j : edge_cells[cell]) {
-                    if (j < i + 2 || j >= first_j) continue;
-                    // Skip the (0, pn-1) pair: edges (0,1) and (pn-1,0) share node 0.
-                    if (i == 0 && j == pn - 1) continue;
-
-                    const Point& aj = centers[path[j]];
-                    const Point& bj = centers[path[(j + 1) % pn]];
-
-                    if (!bboxes_overlap(ai, bi, aj, bj)) continue;
-                    if (Geometry::segments_intersect(ai, bi, aj, bj))
+        for (size_t i = from; i < n_edges; ++i) {
+            size_t first_j = npos;
+            for_cells(centers[path[i]], centers[path[(i + 1) % pn]], [&](int cell) {
+                for (size_t j : edge_cells[cell])
+                    if (j >= i + 2 && j < first_j && worth_reversing(i, j))
                         first_j = j;
-                }
             });
-            if (first_j != std::numeric_limits<size_t>::max())
+            if (first_j != npos)
                 return {i, first_j};
         }
-        return {std::numeric_limits<size_t>::max(), std::numeric_limits<size_t>::max()};
+        return {npos, npos};
     };
 
-    // Process crossings one at a time: find first, reverse it, restart scan.
-    // Cap iterations to prevent infinite loops on collinear/overlapping segments.
-    const int max_iters = static_cast<int>(pn * pn);
+    // Process crossings one at a time. Reversing path[ci+1..cj] replaces only edges ci and cj; the edges between
+    // them are the same segments traversed backwards. The scan therefore resumes at edge ci rather than at edge 0,
+    // so a pass over the tour costs one 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;
-    // Reversing between two segments that only touch or overlap along a line need not remove the intersection, so on
-    // islands laid out on a regular grid (a tiled texture, an array of parts) the loop can cycle through the same
-    // orderings until the pn * pn cap. Once an ordering repeats the rest of the loop is periodic, so only the steps
-    // to the ordering the capped loop would have stopped on are taken.
-    std::unordered_map<uint64_t, int> seen_paths; // path hash -> reversals done when it was reached
-    const auto path_hash = [&path]() {
-        uint64_t h = 1469598103934665603ull; // FNV-1a
-        for (size_t idx : path)
-            h = (h ^ uint64_t(idx)) * 1099511628211ull;
-        return h;
-    };
-    const auto reverse_first_crossing = [&]() {
-        auto [ci, cj] = pn >= grid_min_size ? find_crossing_grid() : find_crossing();
-        if (ci == std::numeric_limits<size_t>::max())
-            return false;
-        std::reverse(path.begin() + ci + 1, path.begin() + cj + 1);
-        return true;
-    };
-    seen_paths.emplace(path_hash(), 0);
-    for (int iter = 1; iter <= max_iters && reverse_first_crossing(); ++iter) {
-        improved = true;
-        if (auto [it, inserted] = seen_paths.emplace(path_hash(), iter); !inserted) {
-            for (int steps = (max_iters - iter) % (iter - it->second); steps > 0; --steps)
-                reverse_first_crossing();
-            break;
+    for (size_t from = 0;;) {
+        auto [ci, cj] = pn >= grid_min_size ? find_crossing_grid(from) : find_crossing(from);
+        if (ci == npos) {
+            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.
-// 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 any segment pair whose edges geometrically cross.
-// Returns true if any crossing was removed.
+// Crossing removal: reverse each pair of intersecting edges whose reversal shortens the cycle by more
+// 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]") {