Visit seam candidates as the search finds them

Collecting every candidate within the radius into a vector cost more
than the search itself. Same order, so the seams are unchanged;
align_seam_points ~19.6 s at 0.1 mm / 2000k, was ~21.

src/libslic3r/GCode/SeamPlacer.cpp

@@ -1117,21 +1117,21 @@ std::optional<std::pair<size_t, size_t>> SeamPlacer::find_next_seam_in_layer(
     const size_t layer_idx, const float max_distance,
     const SeamPlacerImpl::SeamComparator &comparator) const {
   using namespace SeamPlacerImpl;
-  std::vector<size_t> nearby_points_indices = find_nearby_points(*layers[layer_idx].points_tree, projected_position,
-                                                                 max_distance);
-
-  if (nearby_points_indices.empty()) {
-    return {};
-  }
-
-  size_t best_nearby_point_index = nearby_points_indices[0];
-  size_t nearest_point_index = nearby_points_indices[0];
-
-  // Now find best nearby point, nearest point, and corresponding indices
-  for (const size_t &nearby_point_index : nearby_points_indices) {
+  // Find the best nearby point and the nearest one. A layer of a fine relief has tens of thousands of candidates within
+  // the radius, so they are looked at as the search finds them rather than collected into a vector first.
+  constexpr size_t none = std::numeric_limits<size_t>::max();
+  size_t best_nearby_point_index = none;
+  size_t nearest_point_index = none;
+  visit_nearby_points(*layers[layer_idx].points_tree, projected_position, max_distance,
+                      [&layers, &comparator, &projected_position, layer_idx, &best_nearby_point_index, &nearest_point_index]
+                      (size_t nearby_point_index) {
+    if (best_nearby_point_index == none) {
+      // The first point found starts both, as the first of the collected ones did.
+      best_nearby_point_index = nearest_point_index = nearby_point_index;
+    }
     const SeamCandidate &point = layers[layer_idx].points[nearby_point_index];
     if (point.perimeter.finalized) {
-      continue; // skip over finalized perimeters, try to find some that is not finalized
+      return; // skip over finalized perimeters, try to find some that is not finalized
     }
     if (comparator.is_first_better(point, layers[layer_idx].points[best_nearby_point_index],
                                    projected_position.head<2>())
@@ -1143,6 +1143,10 @@ std::optional<std::pair<size_t, size_t>> SeamPlacer::find_next_seam_in_layer(
         || layers[layer_idx].points[nearest_point_index].perimeter.finalized) {
       nearest_point_index = nearby_point_index;
     }
+  });
+
+  if (best_nearby_point_index == none) {
+    return {};
   }
 
   const SeamCandidate &best_nearby_point = layers[layer_idx].points[best_nearby_point_index];

src/libslic3r/KDTreeIndirect.hpp

@@ -313,6 +313,36 @@ std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const P
     return visitor.result;
 }
 
+// Visits the points within max_distance of center, in the order find_nearby_points() would collect them, and hands
+// each of them to `visitor_fn` instead of returning them all: a search over a dense set spends more on collecting the
+// points into a vector than on the search itself, and its caller usually keeps only a few of them.
+template<typename KDTreeIndirectType, typename PointType, typename VisitorFn>
+void visit_nearby_points(const KDTreeIndirectType &kdtree, const PointType &center,
+                         const typename KDTreeIndirectType::CoordType &max_distance, VisitorFn visitor_fn)
+{
+    using CoordType = typename KDTreeIndirectType::CoordType;
+
+    struct Visitor {
+        const KDTreeIndirectType &kdtree;
+        const PointType           center;
+        const CoordType           max_distance_squared;
+        VisitorFn                 visitor_fn;
+
+        unsigned int operator()(size_t idx, size_t dimension) {
+            auto dist = CoordType(0);
+            for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++i) {
+                CoordType d = center[i] - kdtree.coordinate(idx, i);
+                dist += d * d;
+            }
+            if (dist < max_distance_squared)
+                visitor_fn(idx);
+            return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension);
+        }
+    } visitor { kdtree, center, max_distance * max_distance, visitor_fn };
+
+    kdtree.visit(visitor);
+}
+
 template<typename KDTreeIndirectType, typename PointType>
 std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const PointType &center,
                                        const typename KDTreeIndirectType::CoordType& max_distance)

tests/libslic3r/CMakeLists.txt

@@ -25,6 +25,7 @@ add_executable(${_TEST_NAME}_tests
     test_filament_mixer.cpp
     test_fill_plane_path.cpp
     test_geometry.cpp
+    test_kdtree.cpp
     test_multimaterial_segmentation.cpp
     test_placeholder_parser.cpp
     test_polygon.cpp

tests/libslic3r/test_kdtree.cpp

@@ -0,0 +1,66 @@
+#include <catch2/catch_all.hpp>
+
+#include <random>
+#include <vector>
+
+#include "libslic3r/KDTreeIndirect.hpp"
+#include "libslic3r/Point.hpp"
+
+using namespace Slic3r;
+
+TEST_CASE("Visiting the nearby points gives what collecting them gives", "[KDTree]") {
+    std::mt19937                          rng(19937);
+    std::uniform_real_distribution<float> coord(-50.f, 50.f);
+    // Points in a box, so that a radius search returns anything from none of them to all of them.
+    std::vector<Vec3f> points(2000);
+    for (Vec3f &p : points)
+        p = Vec3f(coord(rng), coord(rng), coord(rng));
+
+    auto coordinate = [&points](size_t idx, size_t dimension) { return points[idx](int(dimension)); };
+    KDTreeIndirect<3, float, decltype(coordinate)> tree(coordinate);
+    std::vector<size_t> indices(points.size());
+    std::iota(indices.begin(), indices.end(), 0);
+    tree.build(indices);
+
+    const float radius = GENERATE(0.5f, 5.f, 25.f, 200.f);
+    for (int i = 0; i < 20; ++ i) {
+        const Vec3f center(coord(rng), coord(rng), coord(rng));
+
+        const std::vector<size_t> collected = find_nearby_points(tree, center, radius);
+        std::vector<size_t>       visited;
+        visit_nearby_points(tree, center, radius, [&visited](size_t idx) { visited.emplace_back(idx); });
+
+        // Same points, and in the same order: a caller that keeps the first of several equally good ones
+        // must get the same answer either way.
+        REQUIRE(visited == collected);
+    }
+}
+
+TEST_CASE("A radius search returns every point within the radius and no other", "[KDTree]") {
+    std::mt19937                          rng(2024);
+    std::uniform_real_distribution<float> coord(-20.f, 20.f);
+    std::vector<Vec3f> points(500);
+    for (Vec3f &p : points)
+        p = Vec3f(coord(rng), coord(rng), coord(rng));
+
+    auto coordinate = [&points](size_t idx, size_t dimension) { return points[idx](int(dimension)); };
+    KDTreeIndirect<3, float, decltype(coordinate)> tree(coordinate);
+    std::vector<size_t> indices(points.size());
+    std::iota(indices.begin(), indices.end(), 0);
+    tree.build(indices);
+
+    const Vec3f center(1.f, -2.f, 3.f);
+    const float radius = 7.f;
+
+    std::vector<size_t> expected;
+    for (size_t i = 0; i < points.size(); ++ i)
+        if ((points[i] - center).squaredNorm() < radius * radius)
+            expected.emplace_back(i);
+
+    std::vector<size_t> visited;
+    visit_nearby_points(tree, center, radius, [&visited](size_t idx) { visited.emplace_back(idx); });
+    std::sort(visited.begin(), visited.end());
+
+    REQUIRE(! expected.empty());
+    REQUIRE(visited == expected);
+}