Add node ranking for increased nearest neighbour performance, currently failing tests for k > 1

Octree/include/CGAL/Octree.h

@@ -383,6 +383,11 @@ public:
     // Invoking the recursive function adds those points to the vector (passed by reference)
     nearest_k_neighbours_recursive(search_point, points_list, m_root, std::numeric_limits<FT>::max());
 
+    // Sort the list by distance
+    std::sort(points_list.begin(), points_list.end(), [=](auto &left, auto &right) {
+      return left.second < right.second;
+    });
+
     // Add all the points found to the output
     for (auto &item : points_list)
       *output++ = item.first;
@@ -531,10 +536,34 @@ private: // functions :
 
       // If the node has children
 
-      // Search each of them
+      // Create a list of the child nodes
+      std::vector<std::pair<typename Node::Index, FT>> children_with_distances;
+      children_with_distances.reserve(8);
+
+      // Check each node
       for (int index = 0; index < 8; ++index) {
         auto &n = node[index];
 
+        // If the node isn't empty
+        if (0 < std::distance(node.value().begin(), node.value().end())) {
+
+          // Find the distance of the node's center
+          auto node_center_distance = CGAL::squared_distance(p, compute_barycenter_position(n));
+
+          // Add this node to the list
+          children_with_distances.emplace_back(index, node_center_distance);
+        }
+      }
+
+      // Sort the children by their distance
+      std::sort(children_with_distances.begin(), children_with_distances.end(), [=](auto &left, auto &right) {
+        return left.second < right.second;
+      });
+
+      // Search each of them
+      for (auto child : children_with_distances) {
+        auto &n = node[child.first.to_ulong()];
+
         // Check whether this node is capable of containing closer points
         if (do_intersect(n, Sphere{p, largest_radius_squared_found + 0.1 /*TODO: This is my epsilon*/})) {
 

Octree/test/Octree/test_octree_nearest_neighbour.cpp

@@ -123,7 +123,6 @@ void kdtree_vs_octree(std::size_t dataset_size) {
   Kd_tree kd_tree(points.points().begin(), points.points().end());
   {
     Kd_tree_search search(kd_tree, random_point, K);
-    std::cout << search.begin()->first << std::endl;
     for (auto p : search)
       kd_tree_nearest_neighbours.push_back(p.first);
   }
@@ -143,7 +142,6 @@ void kdtree_vs_octree(std::size_t dataset_size) {
   Octree octree(points, point_map);
   octree.refine(10, 1);
   {
-    // TODO: Write a nearest-neighbor implementation and use it here
     octree.nearest_k_neighbours(random_point, K, std::back_inserter(octree_nearest_neighbours));
   }
 
@@ -159,6 +157,7 @@ void kdtree_vs_octree(std::size_t dataset_size) {
   // Check that they produce the same answer
   for (int j = 0; j < K; ++j) {
 
+    std::cout << j << std::endl;
     assert(octree_nearest_neighbours[j] == kd_tree_nearest_neighbours[j]);
   }
 }