diff --git a/Octree/include/CGAL/Octree.h b/Octree/include/CGAL/Octree.h index 457c1657706..8a9ac95c15f 100644 --- a/Octree/include/CGAL/Octree.h +++ b/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::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> 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*/})) { diff --git a/Octree/test/Octree/test_octree_nearest_neighbour.cpp b/Octree/test/Octree/test_octree_nearest_neighbour.cpp index ae3e57f6f45..e36b592a66d 100644 --- a/Octree/test/Octree/test_octree_nearest_neighbour.cpp +++ b/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]); } }