From e02aed31d259c030dae05fed1d5c0a5cd958a144 Mon Sep 17 00:00:00 2001 From: Pavel Mikus Date: Wed, 5 Oct 2022 14:50:22 +0200 Subject: [PATCH] Added new query to AABBTree: all primitives (triangles/lines) within radius --- src/libslic3r/AABBTreeIndirect.hpp | 60 ++++++++++++++++++++++++++- src/libslic3r/AABBTreeLines.hpp | 49 +++++++++++++++------- tests/libslic3r/test_aabbindirect.cpp | 20 +++++++++ 3 files changed, 114 insertions(+), 15 deletions(-) diff --git a/src/libslic3r/AABBTreeIndirect.hpp b/src/libslic3r/AABBTreeIndirect.hpp index 3db74b8b91..b4b74ef138 100644 --- a/src/libslic3r/AABBTreeIndirect.hpp +++ b/src/libslic3r/AABBTreeIndirect.hpp @@ -519,7 +519,7 @@ namespace detail { const VectorType origin; inline VectorType closest_point_to_origin(size_t primitive_index, - ScalarType& squared_distance){ + ScalarType& squared_distance) const { const auto &triangle = this->faces[primitive_index]; VectorType closest_point = closest_point_to_triangle(origin, this->vertices[triangle(0)].template cast(), @@ -613,6 +613,37 @@ namespace detail { return up_sqr_d; } + template + static inline void indexed_primitives_within_distance_squared_recurisve(const IndexedPrimitivesDistancerType &distancer, + size_t node_idx, + Scalar squared_distance_limit, + std::vector &found_primitives_indices) + { + const auto &node = distancer.tree.node(node_idx); + assert(node.is_valid()); + if (node.is_leaf()) { + Scalar sqr_dist; + distancer.closest_point_to_origin(node.idx, sqr_dist); + if (sqr_dist < squared_distance_limit) { found_primitives_indices.push_back(node.idx); } + } else { + size_t left_node_idx = node_idx * 2 + 1; + size_t right_node_idx = left_node_idx + 1; + const auto &node_left = distancer.tree.node(left_node_idx); + const auto &node_right = distancer.tree.node(right_node_idx); + assert(node_left.is_valid()); + assert(node_right.is_valid()); + + if (node_left.bbox.squaredExteriorDistance(distancer.origin) < squared_distance_limit) { + indexed_primitives_within_distance_squared_recurisve(distancer, left_node_idx, squared_distance_limit, + found_primitives_indices); + } + if (node_right.bbox.squaredExteriorDistance(distancer.origin) < squared_distance_limit) { + indexed_primitives_within_distance_squared_recurisve(distancer, right_node_idx, squared_distance_limit, + found_primitives_indices); + } + } + } + } // namespace detail // Build a balanced AABB Tree over an indexed triangles set, balancing the tree @@ -799,6 +830,33 @@ inline bool is_any_triangle_in_radius( return hit_point.allFinite(); } +// Returns all triangles within the given radius limit +template +inline std::vector all_triangles_in_radius( + // Indexed triangle set - 3D vertices. + const std::vector &vertices, + // Indexed triangle set - triangular faces, references to vertices. + const std::vector &faces, + // AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices. + const TreeType &tree, + // Point to which the distances on the indexed triangle set is searched for. + const VectorType &point, + //Square of maximum distance in which triangles are searched for + typename VectorType::Scalar max_distance_squared) +{ + auto distancer = detail::IndexedTriangleSetDistancer + { vertices, faces, tree, point }; + + if(tree.empty()) + { + return {}; + } + + std::vector found_triangles{}; + detail::indexed_primitives_within_distance_squared_recurisve(distancer, size_t(0), max_distance_squared, found_triangles); + return found_triangles; +} + // Traverse the tree and return the index of an entity whose bounding box // contains a given point. Returns size_t(-1) when the point is outside. diff --git a/src/libslic3r/AABBTreeLines.hpp b/src/libslic3r/AABBTreeLines.hpp index 89a8a7cd58..8432feda74 100644 --- a/src/libslic3r/AABBTreeLines.hpp +++ b/src/libslic3r/AABBTreeLines.hpp @@ -6,6 +6,7 @@ #include "libslic3r/AABBTreeIndirect.hpp" #include "libslic3r/Line.hpp" #include +#include namespace Slic3r { @@ -26,7 +27,7 @@ struct IndexedLinesDistancer { const VectorType origin; inline VectorType closest_point_to_origin(size_t primitive_index, - ScalarType &squared_distance) { + ScalarType &squared_distance) const { Vec nearest_point; const LineType &line = lines[primitive_index]; squared_distance = line_alg::distance_to_squared(line, origin.template cast(), &nearest_point); @@ -90,20 +91,35 @@ inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over // Finding a closest line, its closest point and squared distance to the closest point // Returns squared distance to the closest point or -1 if the input is empty. template -inline typename VectorType::Scalar squared_distance_to_indexed_lines( - const std::vector &lines, - const TreeType &tree, - const VectorType &point, - size_t &hit_idx_out, - Eigen::PlainObjectBase &hit_point_out) - { - using Scalar = typename VectorType::Scalar; - auto distancer = detail::IndexedLinesDistancer - { lines, tree, point }; +inline typename VectorType::Scalar squared_distance_to_indexed_lines(const std::vector &lines, + const TreeType &tree, + const VectorType &point, + size_t &hit_idx_out, + Eigen::PlainObjectBase &hit_point_out) +{ + using Scalar = typename VectorType::Scalar; + auto distancer = detail::IndexedLinesDistancer{lines, tree, point}; return tree.empty() ? - Scalar(-1) : - AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), - std::numeric_limits::infinity(), hit_idx_out, hit_point_out); + Scalar(-1) : + AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), + std::numeric_limits::infinity(), + hit_idx_out, hit_point_out); +} + +// Returns all lines within the given radius limit +template +inline std::vector all_lines_in_radius(const std::vector &lines, + const TreeType &tree, + const VectorType &point, + typename VectorType::Scalar max_distance_squared) +{ + auto distancer = detail::IndexedLinesDistancer{lines, tree, point}; + + if (tree.empty()) { return {}; } + + std::vector found_lines{}; + AABBTreeIndirect::detail::indexed_primitives_within_distance_squared_recurisve(distancer, size_t(0), max_distance_squared, found_lines); + return found_lines; } template class LinesDistancer @@ -158,6 +174,11 @@ public: return dist; } + std::vector all_lines_in_radius(const Vec<2, typename LineType::Scalar> &point, Floating radius) + { + return all_lines_in_radius(this->lines, this->tree, point, radius * radius); + } + const LineType &get_line(size_t line_idx) const { return lines[line_idx]; } const std::vector &get_lines() const { return lines; } diff --git a/tests/libslic3r/test_aabbindirect.cpp b/tests/libslic3r/test_aabbindirect.cpp index 87cf80943a..def349042e 100644 --- a/tests/libslic3r/test_aabbindirect.cpp +++ b/tests/libslic3r/test_aabbindirect.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -87,6 +88,25 @@ TEST_CASE("Creating a several 2d lines, testing closest point query", "[AABBIndi REQUIRE(hit_point_out.y() == Approx(0.5)); } +TEST_CASE("Creating a several 2d lines, testing all lines in radius query", "[AABBIndirect]") +{ + std::vector lines { }; + lines.push_back(Linef(Vec2d(0.0, 0.0), Vec2d(10.0, 0.0))); + lines.push_back(Linef(Vec2d(-10.0, 10.0), Vec2d(10.0, -10.0))); + lines.push_back(Linef(Vec2d(-2.0, -1.0), Vec2d(-2.0, 1.0))); + lines.push_back(Linef(Vec2d(-1.0, -1.0), Vec2d(-1.0, -1.0))); + lines.push_back(Linef(Vec2d(1.0, 1.0), Vec2d(1.0, 1.0))); + + auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(lines); + + auto indices = AABBTreeLines::all_lines_in_radius(lines, tree, Vec2d{1.0,1.0}, 4.0); + + REQUIRE(std::find(indices.begin(),indices.end(), 0) != indices.end()); + REQUIRE(std::find(indices.begin(),indices.end(), 1) != indices.end()); + REQUIRE(std::find(indices.begin(),indices.end(), 4) != indices.end()); + REQUIRE(indices.size() == 3); +} + #if 0 #include "libslic3r/EdgeGrid.hpp" #include