From 12a54251c9534162cdb90d46982daa943f4f9372 Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Mon, 9 May 2022 12:51:58 +0200 Subject: [PATCH 1/4] 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()); +//} From fed317f27b5b78e34bdde1ec70a330665027ec88 Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Mon, 9 May 2022 13:11:01 +0200 Subject: [PATCH 2/4] Change std::nan("") to proper nan constants --- src/libslic3r/AABBTreeIndirect.hpp | 4 ++-- src/libslic3r/Arrange.hpp | 2 +- src/libslic3r/Optimize/Optimizer.hpp | 6 +++--- src/libslic3r/SLA/Rotfinder.cpp | 10 +++++----- src/libslic3r/SLA/SupportTreeBuildsteps.cpp | 2 +- src/libslic3r/SLAPrintSteps.cpp | 2 +- src/libslic3r/libslic3r.h | 6 ++++++ src/slic3r/GUI/DoubleSlider.cpp | 2 +- src/slic3r/GUI/GLCanvas3D.hpp | 2 +- tests/sla_print/sla_test_utils.cpp | 2 +- 10 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/libslic3r/AABBTreeIndirect.hpp b/src/libslic3r/AABBTreeIndirect.hpp index 3ea18de8d..31101697f 100644 --- a/src/libslic3r/AABBTreeIndirect.hpp +++ b/src/libslic3r/AABBTreeIndirect.hpp @@ -771,8 +771,8 @@ inline bool is_any_triangle_in_radius( auto distancer = detail::IndexedTriangleSetDistancer { vertices, faces, tree, point }; - size_t hit_idx; - VectorType hit_point = VectorType::Ones() * (std::nan("")); + size_t hit_idx; + VectorType hit_point = VectorType::Ones() * (NaN); if(tree.empty()) { diff --git a/src/libslic3r/Arrange.hpp b/src/libslic3r/Arrange.hpp index 0ff87c88d..47171007a 100644 --- a/src/libslic3r/Arrange.hpp +++ b/src/libslic3r/Arrange.hpp @@ -15,7 +15,7 @@ class CircleBed { double radius_; public: - inline CircleBed(): center_(0, 0), radius_(std::nan("")) {} + inline CircleBed(): center_(0, 0), radius_(NaNd) {} explicit inline CircleBed(const Point& c, double r): center_(c), radius_(r) {} inline double radius() const { return radius_; } diff --git a/src/libslic3r/Optimize/Optimizer.hpp b/src/libslic3r/Optimize/Optimizer.hpp index 8ae55c61c..bf95d9ee0 100644 --- a/src/libslic3r/Optimize/Optimizer.hpp +++ b/src/libslic3r/Optimize/Optimizer.hpp @@ -41,13 +41,13 @@ template using Bounds = std::array; class StopCriteria { // If the absolute value difference between two scores. - double m_abs_score_diff = std::nan(""); + double m_abs_score_diff = NaNd; // If the relative value difference between two scores. - double m_rel_score_diff = std::nan(""); + double m_rel_score_diff = NaNd; // Stop if this value or better is found. - double m_stop_score = std::nan(""); + double m_stop_score = NaNd; // A predicate that if evaluates to true, the optimization should terminate // and the best result found prior to termination should be returned. diff --git a/src/libslic3r/SLA/Rotfinder.cpp b/src/libslic3r/SLA/Rotfinder.cpp index 196646dc9..847e638e6 100644 --- a/src/libslic3r/SLA/Rotfinder.cpp +++ b/src/libslic3r/SLA/Rotfinder.cpp @@ -76,7 +76,7 @@ struct Facestats { // Try to guess the number of support points needed to support a mesh double get_misalginment_score(const TriangleMesh &mesh, const Transform3f &tr) { - if (mesh.its.vertices.empty()) return std::nan(""); + if (mesh.its.vertices.empty()) return NaNd; auto accessfn = [&mesh, &tr](size_t fi) { Facestats fc{get_transformed_triangle(mesh, tr, fi)}; @@ -117,7 +117,7 @@ inline double get_supportedness_score(const Facestats &fc) // Try to guess the number of support points needed to support a mesh double get_supportedness_score(const TriangleMesh &mesh, const Transform3f &tr) { - if (mesh.its.vertices.empty()) return std::nan(""); + if (mesh.its.vertices.empty()) return NaNd; auto accessfn = [&mesh, &tr](size_t fi) { Facestats fc{get_transformed_triangle(mesh, tr, fi)}; @@ -149,10 +149,10 @@ float find_ground_level(const TriangleMesh &mesh, return execution::reduce(ex_tbb, size_t(0), vsize, zmin, minfn, accessfn, granularity); } -float get_supportedness_onfloor_score(const TriangleMesh &mesh, - const Transform3f & tr) +double get_supportedness_onfloor_score(const TriangleMesh &mesh, + const Transform3f &tr) { - if (mesh.its.vertices.empty()) return std::nan(""); + if (mesh.its.vertices.empty()) return NaNd; size_t Nthreads = std::thread::hardware_concurrency(); diff --git a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp index aa69fdc77..e4ca3f59c 100644 --- a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp +++ b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp @@ -654,7 +654,7 @@ void SupportTreeBuildsteps::filter() for (const SupportPoint &sp : m_support_pts) { m_thr(); heads.emplace_back( - std::nan(""), + NaNd, sp.head_front_radius, 0., m_cfg.head_penetration_mm, diff --git a/src/libslic3r/SLAPrintSteps.cpp b/src/libslic3r/SLAPrintSteps.cpp index de3e3ae96..fc73aa078 100644 --- a/src/libslic3r/SLAPrintSteps.cpp +++ b/src/libslic3r/SLAPrintSteps.cpp @@ -1036,7 +1036,7 @@ void SLAPrint::Steps::merge_slices_and_eval_stats() { // Estimated printing time // A layers count o the highest object if (printer_input.size() == 0) - print_statistics.estimated_print_time = std::nan(""); + print_statistics.estimated_print_time = NaNd; else { print_statistics.estimated_print_time = estim_time; print_statistics.layers_times = layers_times; diff --git a/src/libslic3r/libslic3r.h b/src/libslic3r/libslic3r.h index 2131a9233..06cad840e 100644 --- a/src/libslic3r/libslic3r.h +++ b/src/libslic3r/libslic3r.h @@ -331,6 +331,12 @@ public: inline bool empty() const { return size() == 0; } }; +template> +constexpr T NaN = std::numeric_limits::quiet_NaN(); + +constexpr float NaNf = NaN; +constexpr double NaNd = NaN; + } // namespace Slic3r #endif diff --git a/src/slic3r/GUI/DoubleSlider.cpp b/src/slic3r/GUI/DoubleSlider.cpp index 20a3d8396..dda50ec05 100644 --- a/src/slic3r/GUI/DoubleSlider.cpp +++ b/src/slic3r/GUI/DoubleSlider.cpp @@ -1163,7 +1163,7 @@ void Control::draw_ruler(wxDC& dc) } }; - double short_tick = std::nan(""); + double short_tick = NaNd; int tick = 0; double value = 0.0; size_t sequence = 0; diff --git a/src/slic3r/GUI/GLCanvas3D.hpp b/src/slic3r/GUI/GLCanvas3D.hpp index 09659faea..1876500e2 100644 --- a/src/slic3r/GUI/GLCanvas3D.hpp +++ b/src/slic3r/GUI/GLCanvas3D.hpp @@ -822,7 +822,7 @@ public: class WipeTowerInfo { protected: - Vec2d m_pos = {std::nan(""), std::nan("")}; + Vec2d m_pos = {NaNd, NaNd}; double m_rotation = 0.; BoundingBoxf m_bb; friend class GLCanvas3D; diff --git a/tests/sla_print/sla_test_utils.cpp b/tests/sla_print/sla_test_utils.cpp index d98a92037..a58b33ca0 100644 --- a/tests/sla_print/sla_test_utils.cpp +++ b/tests/sla_print/sla_test_utils.cpp @@ -386,7 +386,7 @@ long raster_pxsum(const sla::RasterGrayscaleAA &raster) double raster_white_area(const sla::RasterGrayscaleAA &raster) { - if (raster.resolution().pixels() == 0) return std::nan(""); + if (raster.resolution().pixels() == 0) return NaNd; auto res = raster.resolution(); double a = 0; From bbf398f2dc93d4789a05b2eeda93406e1305fa17 Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Mon, 9 May 2022 14:13:12 +0200 Subject: [PATCH 3/4] Fix AABB query in hollowing print step --- src/libslic3r/AABBTreeIndirect.hpp | 12 ++++++------ src/libslic3r/SLAPrintSteps.cpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/libslic3r/AABBTreeIndirect.hpp b/src/libslic3r/AABBTreeIndirect.hpp index 31101697f..014b49af1 100644 --- a/src/libslic3r/AABBTreeIndirect.hpp +++ b/src/libslic3r/AABBTreeIndirect.hpp @@ -828,22 +828,22 @@ struct Intersecting> { template auto intersecting(const G &g) { return Intersecting{g}; } -template struct Containing {}; +template struct Within {}; // Intersection predicate specialization for box-box intersections template -struct Containing> { +struct Within> { Eigen::AlignedBox box; - Containing(const Eigen::AlignedBox &bb): box{bb} {} + Within(const Eigen::AlignedBox &bb): box{bb} {} bool operator() (const typename Tree::Node &node) const { - return box.contains(node.bbox); + return node.is_leaf() ? box.contains(node.bbox) : box.intersects(node.bbox); } }; -template auto containing(const G &g) { return Containing{g}; } +template auto within(const G &g) { return Within{g}; } namespace detail { @@ -858,7 +858,7 @@ void traverse_recurse(const Tree &tree, if (!pred(tree.node(idx))) return; if (tree.node(idx).is_leaf()) { - callback(tree.node(idx).idx); + callback(tree.node(idx)); } else { // call this with left and right node idx: diff --git a/src/libslic3r/SLAPrintSteps.cpp b/src/libslic3r/SLAPrintSteps.cpp index fc73aa078..c3ec3e399 100644 --- a/src/libslic3r/SLAPrintSteps.cpp +++ b/src/libslic3r/SLAPrintSteps.cpp @@ -408,9 +408,9 @@ void SLAPrint::Steps::drill_holes(SLAPrintObject &po) AABBTreeIndirect::traverse( tree, AABBTreeIndirect::intersecting(ebb), - [&part_to_drill, &hollowed_mesh](size_t faceid) + [&part_to_drill, &hollowed_mesh](const auto& node) { - part_to_drill.indices.emplace_back(hollowed_mesh.its.indices[faceid]); + part_to_drill.indices.emplace_back(hollowed_mesh.its.indices[node.idx]); }); auto cgal_meshpart = MeshBoolean::cgal::triangle_mesh_to_cgal( From 34ba45dde404f85fb9a647e5ef0e16f691e8b37f Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Mon, 9 May 2022 14:20:29 +0200 Subject: [PATCH 4/4] Small fix in Execution.hpp --- src/libslic3r/Execution/Execution.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libslic3r/Execution/Execution.hpp b/src/libslic3r/Execution/Execution.hpp index dcfd86bde..57ad4b41b 100644 --- a/src/libslic3r/Execution/Execution.hpp +++ b/src/libslic3r/Execution/Execution.hpp @@ -30,8 +30,8 @@ template using AsTraits = Traits>; // Each execution policy should declare two types of mutexes. A a spin lock and // a blocking mutex. These types should satisfy the BasicLockable concept. -template using SpinningMutex = typename Traits::SpinningMutex; -template using BlockingMutex = typename Traits::BlockingMutex; +template using SpinningMutex = typename AsTraits::SpinningMutex; +template using BlockingMutex = typename AsTraits::BlockingMutex; // Query the available threads for concurrency. template >