From 12a54251c9534162cdb90d46982daa943f4f9372 Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Mon, 9 May 2022 12:51:58 +0200 Subject: [PATCH] Extend kdtree with k-nearest and bounding box queries Also add test to verify it --- src/libslic3r/CMakeLists.txt | 1 + src/libslic3r/KDTreeIndirect.hpp | 463 ++++++++++++++---------- src/libslic3r/PointGrid.hpp | 74 ++++ tests/libslic3r/CMakeLists.txt | 1 + tests/libslic3r/test_kdtreeindirect.cpp | 142 ++++++++ 5 files changed, 495 insertions(+), 186 deletions(-) create mode 100644 src/libslic3r/PointGrid.hpp create mode 100644 tests/libslic3r/test_kdtreeindirect.cpp diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index d2ecefb9c..242378d6d 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -212,6 +212,7 @@ set(SLIC3R_SOURCES PrintObject.cpp PrintObjectSlice.cpp PrintRegion.cpp + PointGrid.hpp PNGReadWrite.hpp PNGReadWrite.cpp QuadricEdgeCollapse.cpp diff --git a/src/libslic3r/KDTreeIndirect.hpp b/src/libslic3r/KDTreeIndirect.hpp index 36a157456..37c10827b 100644 --- a/src/libslic3r/KDTreeIndirect.hpp +++ b/src/libslic3r/KDTreeIndirect.hpp @@ -11,231 +11,276 @@ namespace Slic3r { +enum class VisitorReturnMask : unsigned int { + CONTINUE_LEFT = 1, + CONTINUE_RIGHT = 2, + STOP = 4, +}; + // KD tree for N-dimensional closest point search. template class KDTreeIndirect { public: - static constexpr size_t NumDimensions = ANumDimensions; - using CoordinateFn = ACoordinateFn; - using CoordType = ACoordType; + static constexpr size_t NumDimensions = ANumDimensions; + using CoordinateFn = ACoordinateFn; + using CoordType = ACoordType; // Following could be static constexpr size_t, but that would not link in C++11 enum : size_t { npos = size_t(-1) }; - KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {} - KDTreeIndirect(CoordinateFn coordinate, std::vector indices) : coordinate(coordinate) { this->build(std::move(indices)); } - KDTreeIndirect(CoordinateFn coordinate, std::vector &&indices) : coordinate(coordinate) { this->build(std::move(indices)); } - KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); } - KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {} - KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; } - void clear() { m_nodes.clear(); } + KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {} + KDTreeIndirect(CoordinateFn coordinate, std::vector indices) : coordinate(coordinate) { this->build(indices); } + KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); } + KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {} + KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; } + void clear() { m_nodes.clear(); } - void build(size_t num_indices) - { - std::vector indices; - indices.reserve(num_indices); - for (size_t i = 0; i < num_indices; ++ i) - indices.emplace_back(i); - this->build(std::move(indices)); - } + void build(size_t num_indices) + { + std::vector indices; + indices.reserve(num_indices); + for (size_t i = 0; i < num_indices; ++ i) + indices.emplace_back(i); + this->build(indices); + } - void build(std::vector &&indices) - { - if (indices.empty()) - clear(); - else { - // Allocate enough memory for a full binary tree. - m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos); - build_recursive(indices, 0, 0, 0, indices.size() - 1); - } - indices.clear(); - } + void build(std::vector &indices) + { + if (indices.empty()) + clear(); + else { + // Allocate enough memory for a full binary tree. + m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos); + build_recursive(indices, 0, 0, 0, indices.size() - 1); + } + indices.clear(); + } - enum class VisitorReturnMask : unsigned int - { - CONTINUE_LEFT = 1, - CONTINUE_RIGHT = 2, - STOP = 4, - }; - template - unsigned int descent_mask(const CoordType &point_coord, const CoordType &search_radius, size_t idx, size_t dimension) const - { - CoordType dist = point_coord - this->coordinate(idx, dimension); - return (dist * dist < search_radius + CoordType(EPSILON)) ? - // The plane intersects a hypersphere centered at point_coord of search_radius. - ((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) : - // The plane does not intersect the hypersphere. - (dist > CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT); - } + template + unsigned int descent_mask(const CoordType &point_coord, const CoordType &search_radius, size_t idx, size_t dimension) const + { + CoordType dist = point_coord - this->coordinate(idx, dimension); + return (dist * dist < search_radius + CoordType(EPSILON)) ? + // The plane intersects a hypersphere centered at point_coord of search_radius. + ((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) : + // The plane does not intersect the hypersphere. + (dist > CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT); + } - // Visitor is supposed to return a bit mask of VisitorReturnMask. - template - void visit(Visitor &visitor) const - { + // Visitor is supposed to return a bit mask of VisitorReturnMask. + template + void visit(Visitor &visitor) const + { visit_recursive(0, 0, visitor); - } + } - CoordinateFn coordinate; + CoordinateFn coordinate; private: - // Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension. - void build_recursive(std::vector &input, size_t node, const size_t dimension, const size_t left, const size_t right) - { - if (left > right) - return; + // Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension. + void build_recursive(std::vector &input, size_t node, const size_t dimension, const size_t left, const size_t right) + { + if (left > right) + return; - assert(node < m_nodes.size()); + assert(node < m_nodes.size()); - if (left == right) { - // Insert a node into the balanced tree. - m_nodes[node] = input[left]; - return; - } + if (left == right) { + // Insert a node into the balanced tree. + m_nodes[node] = input[left]; + return; + } - // Partition the input to left / right pieces of the same length to produce a balanced tree. - size_t center = (left + right) / 2; - partition_input(input, dimension, left, right, center); - // Insert a node into the tree. - m_nodes[node] = input[center]; - // Build up the left / right subtrees. - size_t next_dimension = dimension; - if (++ next_dimension == NumDimensions) - next_dimension = 0; - if (center > left) - build_recursive(input, node * 2 + 1, next_dimension, left, center - 1); - build_recursive(input, node * 2 + 2, next_dimension, center + 1, right); - } + // Partition the input to left / right pieces of the same length to produce a balanced tree. + size_t center = (left + right) / 2; + partition_input(input, dimension, left, right, center); + // Insert a node into the tree. + m_nodes[node] = input[center]; + // Build up the left / right subtrees. + size_t next_dimension = dimension; + if (++ next_dimension == NumDimensions) + next_dimension = 0; + if (center > left) + build_recursive(input, node * 2 + 1, next_dimension, left, center - 1); + build_recursive(input, node * 2 + 2, next_dimension, center + 1, right); + } - // Partition the input m_nodes at "k" and "dimension" using the QuickSelect method: - // https://en.wikipedia.org/wiki/Quickselect - // Items left of the k'th item are lower than the k'th item in the "dimension", - // items right of the k'th item are higher than the k'th item in the "dimension", - void partition_input(std::vector &input, const size_t dimension, size_t left, size_t right, const size_t k) const - { - while (left < right) { - size_t center = (left + right) / 2; - CoordType pivot; - { - // Bubble sort the input[left], input[center], input[right], so that a median of the three values - // will end up in input[center]. - CoordType left_value = this->coordinate(input[left], dimension); - CoordType center_value = this->coordinate(input[center], dimension); - CoordType right_value = this->coordinate(input[right], dimension); - if (left_value > center_value) { - std::swap(input[left], input[center]); - std::swap(left_value, center_value); - } - if (left_value > right_value) { - std::swap(input[left], input[right]); - right_value = left_value; - } - if (center_value > right_value) { - std::swap(input[center], input[right]); - center_value = right_value; - } - pivot = center_value; - } - if (right <= left + 2) - // The interval is already sorted. - break; - size_t i = left; - size_t j = right - 1; - std::swap(input[center], input[j]); - // Partition the set based on the pivot. - for (;;) { - // Skip left points that are already at correct positions. - // Search will certainly stop at position (right - 1), which stores the pivot. - while (this->coordinate(input[++ i], dimension) < pivot) ; - // Skip right points that are already at correct positions. - while (this->coordinate(input[-- j], dimension) > pivot && i < j) ; - if (i >= j) - break; - std::swap(input[i], input[j]); - } - // Restore pivot to the center of the sequence. - std::swap(input[i], input[right - 1]); - // Which side the kth element is in? - if (k < i) - right = i - 1; - else if (k == i) - // Sequence is partitioned, kth element is at its place. - break; - else - left = i + 1; - } - } + // Partition the input m_nodes at "k" and "dimension" using the QuickSelect method: + // https://en.wikipedia.org/wiki/Quickselect + // Items left of the k'th item are lower than the k'th item in the "dimension", + // items right of the k'th item are higher than the k'th item in the "dimension", + void partition_input(std::vector &input, const size_t dimension, size_t left, size_t right, const size_t k) const + { + while (left < right) { + size_t center = (left + right) / 2; + CoordType pivot; + { + // Bubble sort the input[left], input[center], input[right], so that a median of the three values + // will end up in input[center]. + CoordType left_value = this->coordinate(input[left], dimension); + CoordType center_value = this->coordinate(input[center], dimension); + CoordType right_value = this->coordinate(input[right], dimension); + if (left_value > center_value) { + std::swap(input[left], input[center]); + std::swap(left_value, center_value); + } + if (left_value > right_value) { + std::swap(input[left], input[right]); + right_value = left_value; + } + if (center_value > right_value) { + std::swap(input[center], input[right]); + center_value = right_value; + } + pivot = center_value; + } + if (right <= left + 2) + // The interval is already sorted. + break; + size_t i = left; + size_t j = right - 1; + std::swap(input[center], input[j]); + // Partition the set based on the pivot. + for (;;) { + // Skip left points that are already at correct positions. + // Search will certainly stop at position (right - 1), which stores the pivot. + while (this->coordinate(input[++ i], dimension) < pivot) ; + // Skip right points that are already at correct positions. + while (this->coordinate(input[-- j], dimension) > pivot && i < j) ; + if (i >= j) + break; + std::swap(input[i], input[j]); + } + // Restore pivot to the center of the sequence. + std::swap(input[i], input[right - 1]); + // Which side the kth element is in? + if (k < i) + right = i - 1; + else if (k == i) + // Sequence is partitioned, kth element is at its place. + break; + else + left = i + 1; + } + } - template - void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const - { - assert(! m_nodes.empty()); - if (node >= m_nodes.size() || m_nodes[node] == npos) - return; + template + void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const + { + assert(! m_nodes.empty()); + if (node >= m_nodes.size() || m_nodes[node] == npos) + return; - // Left / right child node index. - size_t left = node * 2 + 1; - size_t right = left + 1; - unsigned int mask = visitor(m_nodes[node], dimension); - if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) { - size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension; - if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT) - visit_recursive(left, next_dimension, visitor); - if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT) - visit_recursive(right, next_dimension, visitor); - } - } + // Left / right child node index. + size_t left = node * 2 + 1; + size_t right = left + 1; + unsigned int mask = visitor(m_nodes[node], dimension); + if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) { + size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension; + if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT) + visit_recursive(left, next_dimension, visitor); + if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT) + visit_recursive(right, next_dimension, visitor); + } + } - std::vector m_nodes; + std::vector m_nodes; }; // Find a closest point using Euclidian metrics. // Returns npos if not found. -template -size_t find_closest_point(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) +template +std::array find_closest_points( + const KDTreeIndirect &kdtree, + const PointType &point, + FilterFn filter) { - using CoordType = typename KDTreeIndirectType::CoordType; + using Tree = KDTreeIndirect; - struct Visitor { - const KDTreeIndirectType &kdtree; - const PointType &point; - const FilterFn filter; - size_t min_idx = KDTreeIndirectType::npos; - CoordType min_dist = std::numeric_limits::max(); + struct Visitor + { + const Tree &kdtree; + const PointType &point; + const FilterFn filter; - Visitor(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) : kdtree(kdtree), point(point), filter(filter) {} - unsigned int operator()(size_t idx, size_t dimension) { - if (this->filter(idx)) { - auto dist = CoordType(0); - for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++ i) { - CoordType d = point[i] - kdtree.coordinate(idx, i); - dist += d * d; - } - if (dist < min_dist) { - min_dist = dist; - min_idx = idx; - } - } - return kdtree.descent_mask(point[dimension], min_dist, idx, dimension); - } - } visitor(kdtree, point, filter); + std::array, K> results; - kdtree.visit(visitor); - return visitor.min_idx; + Visitor(const Tree &kdtree, const PointType &point, FilterFn filter) + : kdtree(kdtree), point(point), filter(filter) + { + results.fill(std::make_pair(Tree::npos, + std::numeric_limits::max())); + } + unsigned int operator()(size_t idx, size_t dimension) + { + if (this->filter(idx)) { + auto dist = CoordT(0); + for (size_t i = 0; i < D; ++i) { + CoordT d = point[i] - kdtree.coordinate(idx, i); + dist += d * d; + } + + auto res = std::make_pair(idx, dist); + auto it = std::lower_bound(results.begin(), results.end(), + res, [](auto &r1, auto &r2) { + return r1.second < r2.second; + }); + + if (it != results.end()) { + std::rotate(it, std::prev(results.end()), results.end()); + *it = res; + } + } + return kdtree.descent_mask(point[dimension], + results.front().second, idx, + dimension); + } + } visitor(kdtree, point, filter); + + kdtree.visit(visitor); + std::array ret; + for (size_t i = 0; i < K; i++) ret[i] = visitor.results[i].first; + + return ret; +} + +template +std::array find_closest_points( + const KDTreeIndirect &kdtree, const PointType &point) +{ + return find_closest_points(kdtree, point, [](size_t) { return true; }); +} + +template +size_t find_closest_point(const KDTreeIndirect &kdtree, + const PointType &point, + FilterFn filter) +{ + return find_closest_points<1>(kdtree, point, filter)[0]; } template size_t find_closest_point(const KDTreeIndirectType& kdtree, const PointType& point) { - return find_closest_point(kdtree, point, [](size_t) { return true; }); + return find_closest_point(kdtree, point, [](size_t) { return true; }); } // Find nearby points (spherical neighbourhood) using Euclidian metrics. template std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const PointType ¢er, - const typename KDTreeIndirectType::CoordType& max_distance, FilterFn filter) - { + const typename KDTreeIndirectType::CoordType& max_distance, FilterFn filter) +{ using CoordType = typename KDTreeIndirectType::CoordType; struct Visitor { @@ -247,7 +292,7 @@ std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const P Visitor(const KDTreeIndirectType &kdtree, const PointType& center, const CoordType &max_distance, FilterFn filter) : - kdtree(kdtree), center(center), max_distance_squared(max_distance*max_distance), filter(filter) { + kdtree(kdtree), center(center), max_distance_squared(max_distance*max_distance), filter(filter) { } unsigned int operator()(size_t idx, size_t dimension) { if (this->filter(idx)) { @@ -260,7 +305,7 @@ std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const P result.push_back(idx); } } - return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension); + return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension); } } visitor(kdtree, center, max_distance, filter); @@ -270,13 +315,59 @@ std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const P template std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const PointType ¢er, - const typename KDTreeIndirectType::CoordType& max_distance) - { + const typename KDTreeIndirectType::CoordType& max_distance) +{ return find_nearby_points(kdtree, center, max_distance, [](size_t) { return true; }); } +// Find nearby points (spherical neighbourhood) using Euclidian metrics. +template +std::vector find_nearby_points(const KDTreeIndirectType &kdtree, + const PointType &bb_min, + const PointType &bb_max, + FilterFn filter) +{ + struct Visitor { + const KDTreeIndirectType &kdtree; + const PointType &bb_min, &bb_max; + const FilterFn filter; + std::vector result; + + Visitor(const KDTreeIndirectType &kdtree, const PointType& bbmin, const PointType& bbmax, + FilterFn filter) : + kdtree(kdtree), bb_min{bbmin}, bb_max{bbmax}, filter(filter) { + } + unsigned int operator()(size_t idx, size_t dimension) { + unsigned int ret = + static_cast(VisitorReturnMask::CONTINUE_LEFT) | + static_cast(VisitorReturnMask::CONTINUE_RIGHT); + + if (this->filter(idx)) { + PointType p; + bool contains = true; + for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++i) { + p(i) = kdtree.coordinate(idx, i); + contains = contains && bb_min(i) <= p(i) && p(i) <= bb_max(i); + } + + if (p(dimension) < bb_min(dimension)) + ret = static_cast(VisitorReturnMask::CONTINUE_RIGHT); + if (p(dimension) > bb_max(dimension)) + ret = static_cast(VisitorReturnMask::CONTINUE_LEFT); + + if (contains) + result.emplace_back(idx); + } + + return ret; + } + } visitor(kdtree, bb_min, bb_max, filter); + + kdtree.visit(visitor); + return visitor.result; +} } // namespace Slic3r diff --git a/src/libslic3r/PointGrid.hpp b/src/libslic3r/PointGrid.hpp new file mode 100644 index 000000000..acffd2b87 --- /dev/null +++ b/src/libslic3r/PointGrid.hpp @@ -0,0 +1,74 @@ +#ifndef POINTGRID_HPP +#define POINTGRID_HPP + +#include +#include +#include + +namespace Slic3r { + +template +class PointGrid { + Vec3i m_size; + std::vector> m_data; + const int XY; + +public: + explicit PointGrid(std::vector> data, const Vec3i &size) + : m_data(std::move(data)), m_size{size}, XY{m_size.x() * m_size.y()} + {} + + const Vec<3, T> & get(size_t idx) const { return m_data[idx]; } + const Vec<3, T> & get(const Vec3i &coord) const + { + return m_data[get_idx(coord)]; + } + + size_t get_idx(const Vec3i &coord) const + { + size_t ret = coord.z() * XY + coord.y() * m_size.x() + coord.x(); + + return ret; + } + + Vec3i get_coord(size_t idx) const { + size_t iz = idx / XY; + size_t iy = (idx / m_size.x()) % m_size.y(); + size_t ix = idx % m_size.x(); + + return {ix, iy, iz}; + } + + const std::vector> & data() const { return m_data; } + size_t point_count() const { return m_data.size(); } + bool empty() const { return m_data.empty(); } +}; + +template +PointGrid point_grid(Ex policy, + const BoundingBox3Base> &bounds, + const Vec<3, CoordT> &stride) +{ + Vec3i numpts = Vec3i::Zero(); + + for (int n = 0; n < 3; ++n) + numpts(n) = (bounds.max(n) - bounds.min(n)) / stride(n); + + std::vector> out(numpts.x() * numpts.y() * numpts.z()); + + size_t XY = numpts[X] * numpts[Y]; + + execution::for_each(policy, size_t(0), out.size(), [&](size_t i) { + size_t iz = i / XY; + size_t iy = (i / numpts[X]) % numpts[Y]; + size_t ix = i % numpts[X]; + + out[i] = Vec<3, CoordT>(ix * stride.x(), iy * stride.y(), iz * stride.z()); + }); + + return PointGrid{std::move(out), numpts}; +} + +} // namespace Slic3r + +#endif // POINTGRID_HPP diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index a3984b34b..57e473647 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -4,6 +4,7 @@ add_executable(${_TEST_NAME}_tests ${_TEST_NAME}_tests.cpp test_3mf.cpp test_aabbindirect.cpp + test_kdtreeindirect.cpp test_clipper_offset.cpp test_clipper_utils.cpp test_color.cpp diff --git a/tests/libslic3r/test_kdtreeindirect.cpp b/tests/libslic3r/test_kdtreeindirect.cpp new file mode 100644 index 000000000..28d409bbb --- /dev/null +++ b/tests/libslic3r/test_kdtreeindirect.cpp @@ -0,0 +1,142 @@ +#include + +#include "libslic3r/KDTreeIndirect.hpp" +#include "libslic3r/Execution/ExecutionSeq.hpp" +#include "libslic3r/BoundingBox.hpp" +#include "libslic3r/PointGrid.hpp" + +using namespace Slic3r; + +//template +//struct Within { // Wrapper for the `within` predicate that counts calls. + +// kdtree::Within pred; + +// Within(G box): pred{box} {} + +// // Number of times the predicate was called +// mutable size_t call_count = 0; + +// std::pair operator() (const Vec3f &p, size_t dim) +// { +// ++call_count; + +// return pred(p, dim); +// } +//}; + +static double volume(const BoundingBox3Base &box) +{ + auto sz = box.size(); + return sz.x() * sz.y() * sz.z(); +} + +static double volume(const Eigen::AlignedBox &box) +{ + return box.volume(); +} + +TEST_CASE("Test kdtree query for a Box", "[KDTreeIndirect]") +{ + auto vol = BoundingBox3Base{{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}}; + + auto pgrid = point_grid(ex_seq, vol, Vec3f{0.1f, 0.1f, 0.1f}); + + REQUIRE(!pgrid.empty()); + + auto coordfn = [&pgrid] (size_t i, size_t D) { return pgrid.get(i)(int(D)); }; + KDTreeIndirect<3, float, decltype(coordfn)> tree{coordfn, pgrid.point_count()}; + + std::vector out; + + auto qbox = BoundingBox3Base{Vec3f{0.f, 0.f, 0.f}, Vec3f{.5f, .5f, .5f}}; + + size_t call_count = 0; + out = find_nearby_points(tree, qbox.min, qbox.max, [&call_count](size_t) { + call_count++; + return true; + }); + + // Output shall be non-empty + REQUIRE(!out.empty()); + + std::sort(out.begin(), out.end()); + + // No duplicates allowed in the output + auto it = std::unique(out.begin(), out.end()); + REQUIRE(it == out.end()); + + // Test if inside points are in the output and outside points are not. + bool succ = true; + for (size_t i = 0; i < pgrid.point_count(); ++i) { + auto foundit = std::find(out.begin(), out.end(), i); + bool contains = qbox.contains(pgrid.get(i)); + succ = succ && contains ? foundit != out.end() : foundit == out.end(); + + if (!succ) { + std::cout << "invalid point: " << i << " " << pgrid.get(i).transpose() + << std::endl; + break; + } + } + + REQUIRE(succ); + + // Test for the expected cost of the query. + double gridvolume = volume(vol); + double queryvolume = volume(qbox); + double volratio = (queryvolume / gridvolume); + REQUIRE(call_count < 3 * volratio * pgrid.point_count()); + REQUIRE(call_count < pgrid.point_count()); +} + +//TEST_CASE("Test kdtree query for a Sphere", "[KDTreeIndirect]") { +// auto vol = BoundingBox3Base{{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}}; + +// auto pgrid = point_grid(ex_seq, vol, Vec3f{0.1f, 0.1f, 0.1f}); + +// REQUIRE(!pgrid.empty()); + +// auto coordfn = [&pgrid] (size_t i, size_t D) { return pgrid.get(i)(int(D)); }; +// kdtree::KDTreeIndirect<3, float, decltype(coordfn)> tree{coordfn, pgrid.point_count()}; + +// std::vector out; + +// auto querysphere = kdtree::Sphere{Vec3f{5.f, 5.f, 5.f}, 2.f}; + +// auto pred = Within(querysphere); + +// kdtree::query(tree, pred, std::back_inserter(out)); + +// // Output shall be non-empty +// REQUIRE(!out.empty()); + +// std::sort(out.begin(), out.end()); + +// // No duplicates allowed in the output +// auto it = std::unique(out.begin(), out.end()); +// REQUIRE(it == out.end()); + +// // Test if inside points are in the output and outside points are not. +// bool succ = true; +// for (size_t i = 0; i < pgrid.point_count(); ++i) { +// auto foundit = std::find(out.begin(), out.end(), i); +// bool contains = (querysphere.center - pgrid.get(i)).squaredNorm() < pred.pred.r2; +// succ = succ && contains ? foundit != out.end() : foundit == out.end(); + +// if (!succ) { +// std::cout << "invalid point: " << i << " " << pgrid.get(i).transpose() +// << std::endl; +// break; +// } +// } + +// REQUIRE(succ); + +// // Test for the expected cost of the query. +// double gridvolume = volume(vol); +// double queryvolume = volume(querysphere); +// double volratio = (queryvolume / gridvolume); +// REQUIRE(pred.call_count < 3 * volratio * pgrid.point_count()); +// REQUIRE(pred.call_count < pgrid.point_count()); +//}