diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index c5db0accc..2b0988495 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -65,9 +65,9 @@ void LBVH::build( compute_mesh_aabb(mesh_aabb.min, mesh_aabb.max); - init_bvh(vertex_boxes, vertex_bvh); - init_bvh(edge_boxes, edge_bvh); - init_bvh(face_boxes, face_bvh); + init_bvh(vertex_boxes, vertex_bvh, vertex_rightmost_leaves); + init_bvh(edge_boxes, edge_bvh, edge_rightmost_leaves); + init_bvh(face_boxes, face_bvh, face_rightmost_leaves); // Copy edge and face vertex ids for access during traversal { @@ -199,7 +199,8 @@ namespace { } } // namespace -void LBVH::init_bvh(const AABBs& boxes, Nodes& lbvh) const +void LBVH::init_bvh( + const AABBs& boxes, Nodes& lbvh, RightmostLeaves& rightmost_leaves) const { if (boxes.empty()) { return; @@ -252,6 +253,10 @@ void LBVH::init_bvh(const AABBs& boxes, Nodes& lbvh) const assert(boxes.size() <= std::numeric_limits::max()); const int LEAF_OFFSET = int(boxes.size()) - 1; + if (rightmost_leaves.size() != lbvh.size()) { + rightmost_leaves.resize(lbvh.size()); + } + LBVH::ConstructionInfos construction_infos(lbvh.size()); { IPC_TOOLKIT_PROFILE_BLOCK("build_hierarchy"); @@ -269,6 +274,8 @@ void LBVH::init_bvh(const AABBs& boxes, Nodes& lbvh) const leaf_node.primitive_id = morton_codes[i].box_id; leaf_node.is_inner_marker = 0; lbvh[LEAF_OFFSET + i] = leaf_node; // Store leaf + // A leaf's rightmost leaf is itself + rightmost_leaves[LEAF_OFFSET + i] = i; } if (i < LEAF_OFFSET) { @@ -345,6 +352,11 @@ void LBVH::init_bvh(const AABBs& boxes, Nodes& lbvh) const lbvh[node_idx].aabb_max = child_a.aabb_max.max(child_b.aabb_max); + // Compute rightmost leaf: max of children's rightmost + rightmost_leaves[node_idx] = std::max( + rightmost_leaves[lbvh[node_idx].left], + rightmost_leaves[lbvh[node_idx].right]); + if (node_idx == 0) { break; // root node } @@ -360,8 +372,11 @@ void LBVH::clear() BroadPhase::clear(); // Clear BVH nodes vertex_bvh.clear(); + vertex_rightmost_leaves.clear(); edge_bvh.clear(); + edge_rightmost_leaves.clear(); face_bvh.clear(); + face_rightmost_leaves.clear(); // Clear vertex IDs edge_vertex_ids.clear(); @@ -369,7 +384,7 @@ void LBVH::clear() } namespace { - template + template inline void attempt_add_candidate( const LBVH::Node& query, const LBVH::Node& node, @@ -381,12 +396,6 @@ namespace { std::swap(i, j); } - if constexpr (triangular) { - if (i >= j) { - return; - } - } - if (!can_collide(i, j)) { return; } @@ -397,7 +406,9 @@ namespace { template void traverse_lbvh( const LBVH::Node& query, + const size_t query_leaf_idx, const LBVH::Nodes& lbvh, + const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates) { @@ -413,8 +424,11 @@ namespace { if (lbvh.size() == 1) { // Single node case (only root) assert(node.is_leaf()); // Only one node, so it must be a leaf + if constexpr (triangular) { + break; // No self-collision if only one node + } if (node.intersects(query)) { - attempt_add_candidate( + attempt_add_candidate( query, node, can_collide, candidates); } break; @@ -431,16 +445,29 @@ namespace { const LBVH::Node& child_l = lbvh[node.left]; const LBVH::Node& child_r = lbvh[node.right]; - const bool intersects_l = child_l.intersects(query); - const bool intersects_r = child_r.intersects(query); + bool intersects_l = child_l.intersects(query); + bool intersects_r = child_r.intersects(query); + + // Ignore overlap if the subtree is fully on the + // left-hand side of the query (triangular traversal only). + if constexpr (triangular) { + if (intersects_l + && rightmost_leaves[node.left] <= query_leaf_idx) { + intersects_l = false; + } + if (intersects_r + && rightmost_leaves[node.right] <= query_leaf_idx) { + intersects_r = false; + } + } // Query overlaps a leaf node => report collision. if (intersects_l && child_l.is_leaf()) { - attempt_add_candidate( + attempt_add_candidate( query, child_l, can_collide, candidates); } if (intersects_r && child_r.is_leaf()) { - attempt_add_candidate( + attempt_add_candidate( query, child_r, can_collide, candidates); } @@ -468,8 +495,10 @@ namespace { template void traverse_lbvh_simd( const LBVH::Node* queries, + const size_t first_query_leaf_idx, const size_t n_queries, const LBVH::Nodes& lbvh, + const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates) { @@ -518,6 +547,9 @@ namespace { if (lbvh.size() == 1) { // Single node case (only root) assert(node.is_leaf()); // Only one node, so it must be a leaf + if constexpr (triangular) { + break; // No self-collision if only one node + } // Check intersection with all queries simultaneously const xs::batch_bool intersects = (node.aabb_min.x() <= q_max_x) @@ -529,8 +561,7 @@ namespace { if (xs::any(intersects)) { for (int k = 0; k < n_queries; ++k) { if (intersects.get(k)) { - attempt_add_candidate< - Candidate, swap_order, triangular>( + attempt_add_candidate( queries[k], node, can_collide, candidates); } } @@ -552,7 +583,7 @@ namespace { // 1. Intersect multiple queries at once // (child_l.min <= query.max) && (query.min <= child_l.max) - const xs::batch_bool intersects_l = + xs::batch_bool intersects_l = (child_l.aabb_min.x() <= q_max_x) & (child_l.aabb_min.y() <= q_max_y) & (child_l.aabb_min.z() <= q_max_z) @@ -562,7 +593,7 @@ namespace { // 2. Intersect multiple queries at once // (child_r.min <= query.max) && (query.min <= child_r.max) - const xs::batch_bool intersects_r = + xs::batch_bool intersects_r = (child_r.aabb_min.x() <= q_max_x) & (child_r.aabb_min.y() <= q_max_y) & (child_r.aabb_min.z() <= q_max_z) @@ -570,24 +601,49 @@ namespace { & (q_min_y <= child_r.aabb_max.y()) & (q_min_z <= child_r.aabb_max.z()); + // Ignore overlap if the subtree is fully on the left-hand side + // of all queries (triangular traversal only). + // We use first_query_leaf_idx (the smallest query leaf index + // in the SIMD batch) for a conservative check: if all leaves + // in the subtree are <= the smallest query, they are also <= + // every other query in the batch. + if constexpr (triangular) { + if (rightmost_leaves[node.left] <= first_query_leaf_idx) { + intersects_l = xs::batch_bool(false); + } + if (rightmost_leaves[node.right] <= first_query_leaf_idx) { + intersects_r = xs::batch_bool(false); + } + } + const bool any_intersects_l = xs::any(intersects_l); const bool any_intersects_r = xs::any(intersects_r); // Query overlaps a leaf node => report collision if (any_intersects_l && child_l.is_leaf()) { for (int k = 0; k < n_queries; ++k) { + if constexpr (triangular) { + if (rightmost_leaves[node.left] + <= first_query_leaf_idx + k) { + continue; + } + } if (intersects_l.get(k)) { - attempt_add_candidate< - Candidate, swap_order, triangular>( + attempt_add_candidate( queries[k], child_l, can_collide, candidates); } } } if (any_intersects_r && child_r.is_leaf()) { for (int k = 0; k < n_queries; ++k) { + if constexpr (triangular) { + if (rightmost_leaves[node.right] + <= first_query_leaf_idx + k) { + continue; + } + } if (intersects_r.get(k)) { - attempt_add_candidate< - Candidate, swap_order, triangular>( + attempt_add_candidate( queries[k], child_r, can_collide, candidates); } } @@ -620,6 +676,7 @@ namespace { void independent_traversal( const LBVH::Nodes& source, const LBVH::Nodes& target, + const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, tbb::enumerable_thread_specific>& storage) { @@ -655,14 +712,14 @@ namespace { if constexpr (use_simd) { assert(actual_end - idx >= 1); traverse_lbvh_simd( - &source[source_leaf_offset + idx], + &source[source_leaf_offset + idx], idx, std::min(SIMD_SIZE, actual_end - idx), target, - can_collide, local_candidates); + rightmost_leaves, can_collide, local_candidates); } else { #endif traverse_lbvh( - source[source_leaf_offset + idx], target, - can_collide, local_candidates); + source[source_leaf_offset + idx], idx, target, + rightmost_leaves, can_collide, local_candidates); #ifdef IPC_TOOLKIT_WITH_SIMD } #endif @@ -675,6 +732,7 @@ template void LBVH::detect_candidates( const Nodes& source, const Nodes& target, + const RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates) { @@ -685,7 +743,7 @@ void LBVH::detect_candidates( tbb::enumerable_thread_specific> storage; independent_traversal( - source, target, can_collide, storage); + source, target, rightmost_leaves, can_collide, storage); merge_thread_local_vectors(storage, candidates); } @@ -699,7 +757,8 @@ void LBVH::detect_vertex_vertex_candidates( IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_vertex_candidates"); - detect_candidates(vertex_bvh, can_vertices_collide, candidates); + detect_candidates( + vertex_bvh, vertex_rightmost_leaves, can_vertices_collide, candidates); } void LBVH::detect_edge_vertex_candidates( @@ -714,7 +773,7 @@ void LBVH::detect_edge_vertex_candidates( // In 2D and for codimensional edge-vertex collisions, there are more // vertices than edges, so we want to iterate over the edges. detect_candidates( - edge_bvh, vertex_bvh, + edge_bvh, vertex_bvh, vertex_rightmost_leaves, std::bind(&LBVH::can_edge_vertex_collide, this, _1, _2), candidates); } @@ -728,8 +787,8 @@ void LBVH::detect_edge_edge_candidates( IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_edge_candidates"); detect_candidates( - edge_bvh, std::bind(&LBVH::can_edges_collide, this, _1, _2), - candidates); + edge_bvh, edge_rightmost_leaves, + std::bind(&LBVH::can_edges_collide, this, _1, _2), candidates); } void LBVH::detect_face_vertex_candidates( @@ -743,7 +802,7 @@ void LBVH::detect_face_vertex_candidates( // The ratio vertices:faces is 1:2, so we want to iterate over the vertices. detect_candidates( - vertex_bvh, face_bvh, + vertex_bvh, face_bvh, face_rightmost_leaves, std::bind(&LBVH::can_face_vertex_collide, this, _1, _2), candidates); } @@ -758,7 +817,7 @@ void LBVH::detect_edge_face_candidates( // The ratio edges:faces is 3:2, so we want to iterate over the faces. detect_candidates( - face_bvh, edge_bvh, + face_bvh, edge_bvh, edge_rightmost_leaves, std::bind(&LBVH::can_edge_face_collide, this, _1, _2), candidates); } @@ -771,8 +830,8 @@ void LBVH::detect_face_face_candidates( IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_face_face_candidates"); detect_candidates( - face_bvh, std::bind(&LBVH::can_faces_collide, this, _1, _2), - candidates); + face_bvh, face_rightmost_leaves, + std::bind(&LBVH::can_faces_collide, this, _1, _2), candidates); } // ============================================================================ diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp index cb356f352..8e7768b1e 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -80,6 +80,10 @@ class LBVH : public BroadPhase { using MortonCodeElements = std::vector>; + /// @brief For each node, the index of the rightmost leaf reachable from it. + /// Used to skip subtrees during triangular (self-collision) traversal. + using RightmostLeaves = std::vector>; + private: struct ConstructionInfo { /// @brief Parent to the parent @@ -150,14 +154,19 @@ class LBVH : public BroadPhase { /// @brief Initialize a LBVH from a set of boxes. /// @param[in] boxes Set of boxes to initialize the LBVH with. /// @param[out] bvh The LBVH to initialize. - void init_bvh(const AABBs& boxes, Nodes& lbvh) const; + /// @param[out] rightmost_leaves For each node, the rightmost leaf index reachable. Values are the leaf’s position in the Morton-sorted leaf array from [0, n) where n is the number of primitives. + void init_bvh( + const AABBs& boxes, + Nodes& lbvh, + RightmostLeaves& rightmost_leaves) const; /// @brief Detect candidate collisions between a LBVH and a sets of boxes. /// @tparam Candidate Type of candidate collision. /// @tparam swap_order Whether to swap the order of box id with the LBVH id when adding to the candidates. /// @tparam triangular Whether to consider (i, j) and (j, i) as the same. - /// @param[in] boxes The boxes to detect collisions with. - /// @param[in] bvh The LBVH to detect collisions with. + /// @param[in] source The LBVH to traverse. + /// @param[in] target The LBVH to traverse. + /// @param[in] rightmost_leaves The rightmost leaf indices for the target LBVH (used for self-collision skip). /// @param[in] can_collide Function to determine if two primitives can collide given their ids. /// @param[out] candidates The candidate collisions. template < @@ -167,17 +176,26 @@ class LBVH : public BroadPhase { static void detect_candidates( const Nodes& source, const Nodes& target, + const RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates); + /// @brief Detect candidate collisions between a single LBVH and itself. + /// @tparam Candidate Type of candidate collision. + /// @param[in] source_and_target The LBVH to traverse. + /// @param[in] rightmost_leaves The rightmost leaf indices for the LBVH (used for self-collision skip). + /// @param[in] can_collide Function to determine if two primitives can collide given their ids. + /// @param[out] candidates The candidate collisions. template static void detect_candidates( const Nodes& source_and_target, + const RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates) { detect_candidates( - source_and_target, source_and_target, can_collide, candidates); + source_and_target, source_and_target, rightmost_leaves, can_collide, + candidates); } // ------------------------------------------------------------------------- @@ -195,10 +213,22 @@ class LBVH : public BroadPhase { /// @brief BVH containing the vertices. Nodes vertex_bvh; + /// @brief Rightmost leaf indices for vertex BVH (used for self-collision skip). + /// Values are from [0, n) where n is the number of primitives. + /// Values are the leaf’s position in the Morton-sorted leaf array. + RightmostLeaves vertex_rightmost_leaves; /// @brief BVH containing the edges. Nodes edge_bvh; + /// @brief Rightmost leaf indices for edge BVH (used for self-collision skip). + /// Values are from [0, n) where n is the number of primitives. + /// Values are the leaf’s position in the Morton-sorted leaf array. + RightmostLeaves edge_rightmost_leaves; /// @brief BVH containing the faces. Nodes face_bvh; + /// @brief Rightmost leaf indices for face BVH (used for self-collision skip). + /// Values are from [0, n) where n is the number of primitives. + /// Values are the leaf’s position in the Morton-sorted leaf array. + RightmostLeaves face_rightmost_leaves; private: // Vectors with custom allocator to avoid initialization overhead diff --git a/src/ipc/ogc/feasible_region.cpp b/src/ipc/ogc/feasible_region.cpp index b9c973630..21d126b63 100644 --- a/src/ipc/ogc/feasible_region.cpp +++ b/src/ipc/ogc/feasible_region.cpp @@ -127,7 +127,7 @@ bool is_edge_edge_feasible( const Eigen::Vector3d xc = (vertices.row(ea1i) - vertices.row(ea0i)) * alpha + vertices.row(ea0i); - return ogc::check_vertex_feasible_region(mesh, vertices, xc, eb1i); + return ogc::check_vertex_feasible_region(mesh, vertices, xc, eb0i); } case EdgeEdgeDistanceType::EA_EB1: { @@ -141,7 +141,7 @@ bool is_edge_edge_feasible( case EdgeEdgeDistanceType::EA0_EB: { const double alpha = point_edge_closest_point( - vertices.row(eb0i), vertices.row(ea0i), vertices.row(ea1i)); + vertices.row(ea0i), vertices.row(eb0i), vertices.row(eb1i)); const Eigen::Vector3d xc = (vertices.row(eb1i) - vertices.row(eb0i)) * alpha + vertices.row(eb0i); @@ -150,7 +150,7 @@ bool is_edge_edge_feasible( case EdgeEdgeDistanceType::EA1_EB: { const double alpha = point_edge_closest_point( - vertices.row(eb1i), vertices.row(ea0i), vertices.row(ea1i)); + vertices.row(ea1i), vertices.row(eb0i), vertices.row(eb1i)); const Eigen::Vector3d xc = (vertices.row(eb1i) - vertices.row(eb0i)) * alpha + vertices.row(eb0i); diff --git a/tests/src/tests/broad_phase/test_lbvh.cpp b/tests/src/tests/broad_phase/test_lbvh.cpp index 6e71d4f03..3d9571dea 100644 --- a/tests/src/tests/broad_phase/test_lbvh.cpp +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -10,6 +10,7 @@ #include #include +#include #include @@ -281,4 +282,65 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") ipc::profiler().print(); ipc::profiler().clear(); #endif +} + +TEST_CASE( + "Benchmark LBVH::detect_edge_edge_candidates", + "[!benchmark][broad_phase][lbvh]") +{ + constexpr double inflation_radius = 0; + + std::string mesh_t0, mesh_t1; + SECTION("Two cubes") + { + mesh_t0 = "two-cubes-far.ply"; + mesh_t1 = "two-cubes-intersecting.ply"; + } + SECTION("Cloth-Ball") + { + mesh_t0 = "cloth_ball92.ply"; + mesh_t1 = "cloth_ball93.ply"; + } +#ifdef NDEBUG + SECTION("Armadillo-Rollers") + { + mesh_t0 = "armadillo-rollers/326.ply"; + mesh_t1 = "armadillo-rollers/327.ply"; + } + SECTION("Cloth-Funnel") + { + mesh_t0 = "cloth-funnel/227.ply"; + mesh_t1 = "cloth-funnel/228.ply"; + } + SECTION("N-Body-Simulation") + { + mesh_t0 = "n-body-simulation/balls16_18.ply"; + mesh_t1 = "n-body-simulation/balls16_19.ply"; + } + SECTION("Rod-Twist") + { + mesh_t0 = "rod-twist/3036.ply"; + mesh_t1 = "rod-twist/3037.ply"; + } +#endif + SECTION("Puffer-Ball") + { + mesh_t0 = "puffer-ball/20.ply"; + mesh_t1 = "puffer-ball/21.ply"; + } + + Eigen::MatrixXd vertices_t0, vertices_t1; + Eigen::MatrixXi edges, faces; + REQUIRE(tests::load_mesh(mesh_t0, vertices_t0, edges, faces)); + REQUIRE(tests::load_mesh(mesh_t1, vertices_t1, edges, faces)); + + const std::shared_ptr lbvh = std::make_shared(); + lbvh->build(vertices_t0, vertices_t1, edges, faces, inflation_radius); + + BENCHMARK("LBVH::detect_edge_edge_candidates") + { + std::vector ee_candidates; + lbvh->detect_edge_edge_candidates(ee_candidates); + return ee_candidates.size(); + }; } \ No newline at end of file