diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index 58e1c41dd..e7c395dc6 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -10,6 +10,7 @@ #include #include // std::min/max +#include #define IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE // else use unordered_set @@ -103,112 +104,160 @@ void HashGrid::insert_box( } } -template -void HashGrid::detect_candidates( - const std::vector& items0, - const std::vector& items1, - const AABBs& boxes0, - const AABBs& boxes1, - const std::function& can_collide, - std::vector& candidates) const -{ - // Entries with the same key means they share a cell (that cell index - // hashes to the same key) and should be flagged for low-level intersection - // testing. We loop over the entire sorted set of (key,value) pairs - // creating Candidate entries for pairs with the same key - - // 1. Soft merge of items (assuming items are sorted) - size_t num_items = items0.size() + items1.size(); - std::vector merged_item_indices; - merged_item_indices.reserve(num_items); +namespace { + // Join sorted cell runs in bounded batches, then enumerate pairs inside + // each common cell. Large cells retain a 2D parallel split. + template + void detect_cell_pairs( + const std::vector& items0, + const std::vector& items1, + const AABBs& boxes0, + const AABBs& boxes1, + const std::function& can_collide, + std::vector& candidates) { - long i = 0, j = 0; - while (i < items0.size() && j < items1.size()) { - if (items0[i] < items1[j]) { - merged_item_indices.push_back(-(i++) - 1); - } else { - merged_item_indices.push_back(j++); - } - } - while (i < items0.size()) { - merged_item_indices.push_back(-(i++) - 1); - } - while (j < items1.size()) { - merged_item_indices.push_back(j++); - } - } - assert(merged_item_indices.size() == num_items); - - const auto get_item = [&](long i) -> const HashItem& { - return i < 0 ? items0[-(i + 1)] : items1[i]; - }; + struct CellRange { + long begin, end; + }; + struct CellPair { + CellRange first, second; + }; + using Cell = std::conditional_t; + + // Bound temporary storage independently of the number of occupied + // cells. Same-type relations need only one range per cell. + constexpr size_t max_cells_per_batch = 65536; + const size_t max_cells = SameItems + ? items0.size() / 2 + : std::min(items0.size(), items1.size()); + std::vector cells; + cells.reserve(std::min(max_cells_per_batch, max_cells)); - // 2. Enumerate hash collisions #ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; #else - tbb::enumerable_thread_specific> storage; + tbb::enumerable_thread_specific> storage; #endif - - tbb::parallel_for( - tbb::blocked_range2d(0L, num_items - 1, 0L, num_items), - [&](const tbb::blocked_range2d& r) { + const auto emit = [&](long begin0, long end0, long begin1, long end1) { auto& local_candidates = storage.local(); - - // i < j - long i_end = std::min(r.rows().end(), r.cols().end()); - for (long i = r.rows().begin(); i < i_end; i++) { - const long idx0 = merged_item_indices[i]; - const HashItem& item0 = get_item(idx0); - - // i < r.cols().end() → i + 1 <= r.cols().end() - long j_begin = std::max(i + 1, r.cols().begin()); - for (long j = j_begin; j < r.cols().end(); j++) { - const long idx1 = merged_item_indices[j]; - const HashItem& item1 = get_item(idx1); - - if (item0.key != item1.key) { - break; // This avoids a brute force comparison - } - - long id0 = item0.id, id1 = item1.id; - if (idx0 >= 0 && idx1 < 0) { - std::swap(id0, id1); - } else if (idx0 >= 0 || idx1 < 0) { - continue; - } - assert(id0 < boxes0.size() && id1 < boxes1.size()); - - if (!can_collide(id0, id1)) { + if constexpr (SameItems) { + end0 = std::min(end0, end1); + } + for (long a = begin0; a < end0; ++a) { + const auto id0 = items0[a].id; + const AABB& box0 = boxes0[id0]; + const long first = SameItems ? std::max(a + 1, begin1) : begin1; + for (long b = first; b < end1; ++b) { + const auto id1 = items1[b].id; + if (!can_collide(id0, id1) + || !box0.intersects(boxes1[id1])) { continue; } - - if (boxes0[id0].intersects(boxes1[id1])) { #ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - local_candidates.emplace_back(id0, id1); + local_candidates.emplace_back(id0, id1); #else - local_candidates.emplace(id0, id1); + local_candidates.emplace(id0, id1); #endif + } + } + }; + const auto process_cell = [&](long begin0, long end0, long begin1, + long end1) { + if (end0 - begin0 <= 64 && end1 - begin1 <= 64) { + emit(begin0, end0, begin1, end1); + } else { + tbb::parallel_for( + tbb::blocked_range2d( + begin0, end0, 64, begin1, end1, 64), + [&](const auto& r) { + emit( + r.rows().begin(), r.rows().end(), r.cols().begin(), + r.cols().end()); + }); + } + }; + const auto process_batch = [&]() { + tbb::parallel_for( + tbb::blocked_range(0, cells.size()), + [&](const auto& range) { + for (size_t k = range.begin(); k < range.end(); ++k) { + const auto& c = cells[k]; + if constexpr (SameItems) { + process_cell(c.begin, c.end, c.begin, c.end); + } else { + process_cell( + c.first.begin, c.first.end, c.second.begin, + c.second.end); + } } + }); + cells.clear(); + }; + + const auto run_end = [](const auto& items, long begin) { + long end = begin + 1; + while (end < items.size() && items[end].key == items[begin].key) { + ++end; + } + return end; + }; + long i = 0, j = 0; + while (i < items0.size() && j < items1.size()) { + if constexpr (SameItems) { + const long ie = run_end(items0, i); + if (ie - i > 1) { + cells.push_back({ i, ie }); } + i = j = ie; + } else { + if (items0[i].key < items1[j].key) { + i = run_end(items0, i); + continue; + } + if (items1[j].key < items0[i].key) { + j = run_end(items1, j); + continue; + } + const long ie = run_end(items0, i); + const long je = run_end(items1, j); + cells.push_back({ { i, ie }, { j, je } }); + i = ie; + j = je; } - }); - + if (cells.size() == max_cells_per_batch) { + process_batch(); + } + } + if (!cells.empty()) { + process_batch(); + } #ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - merge_thread_local_vectors(storage, candidates); - - // Remove the duplicate candidates - tbb::parallel_sort(candidates.begin(), candidates.end()); - auto new_end = std::unique(candidates.begin(), candidates.end()); - candidates.erase(new_end, candidates.end()); + merge_thread_local_vectors(storage, candidates); + tbb::parallel_sort(candidates.begin(), candidates.end()); + candidates.erase( + std::unique(candidates.begin(), candidates.end()), + candidates.end()); #else - unordered_set candidates_set; - merge_thread_local_unordered_sets(storage, candidates_set); - - candidates.reserve(candidates_set.size()); - candidates.insert( - candidates.end(), candidates_set.begin(), candidates_set.end()); + unordered_set candidates_set; + merge_thread_local_unordered_sets(storage, candidates_set); + candidates.reserve(candidates_set.size()); + candidates.insert( + candidates.end(), candidates_set.begin(), candidates_set.end()); #endif + } +} // namespace + +template +void HashGrid::detect_candidates( + const std::vector& items0, + const std::vector& items1, + const AABBs& boxes0, + const AABBs& boxes1, + const std::function& can_collide, + std::vector& candidates) const +{ + detect_cell_pairs( + items0, items1, boxes0, boxes1, can_collide, candidates); } template @@ -218,69 +267,8 @@ void HashGrid::detect_candidates( const std::function& can_collide, std::vector& candidates) const { - // Entries with the same key means they share a cell (that cell index - // hashes to the same key) and should be flagged for low-level - // intersection testing. So we loop over the entire sorted set of - // (key,value) pairs creating Candidate entries for pairs with the same key - -#ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - tbb::enumerable_thread_specific> storage; -#else - tbb::enumerable_thread_specific> storage; -#endif - - tbb::parallel_for( - tbb::blocked_range2d(0L, items.size() - 1, 0L, items.size()), - [&](const tbb::blocked_range2d& r) { - auto& local_candidates = storage.local(); - - // i < j - long i_end = std::min(r.rows().end(), r.cols().end()); - for (long i = r.rows().begin(); i < i_end; i++) { - const HashItem& item0 = items[i]; - const AABB& box0 = boxes[item0.id]; - - // i < r.cols().end() → i + 1 <= r.cols().end() - long j_begin = std::max(i + 1, r.cols().begin()); - assert(j_begin > i); - for (long j = j_begin; j < r.cols().end(); j++) { - const HashItem& item1 = items[j]; - - if (item0.key != item1.key) { - break; // This avoids a brute force comparison - } - - if (!can_collide(item0.id, item1.id)) { - continue; - } - - const AABB& box1 = boxes[item1.id]; - if (box0.intersects(box1)) { -#ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - local_candidates.emplace_back(item0.id, item1.id); -#else - local_candidates.emplace(item0.id, item1.id); -#endif - } - } - } - }); - -#ifdef IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE - merge_thread_local_vectors(storage, candidates); - - // Remove the duplicate candidates - tbb::parallel_sort(candidates.begin(), candidates.end()); - auto new_end = std::unique(candidates.begin(), candidates.end()); - candidates.erase(new_end, candidates.end()); -#else - unordered_set candidates_set; - merge_thread_local_unordered_sets(storage, candidates_set); - - candidates.reserve(candidates_set.size()); - candidates.insert( - candidates.end(), candidates_set.begin(), candidates_set.end()); -#endif + detect_cell_pairs( + items, items, boxes, boxes, can_collide, candidates); } void HashGrid::detect_vertex_vertex_candidates( diff --git a/tests/src/tests/broad_phase/benchmark_broad_phase.cpp b/tests/src/tests/broad_phase/benchmark_broad_phase.cpp index 49260d6ec..a13f22978 100644 --- a/tests/src/tests/broad_phase/benchmark_broad_phase.cpp +++ b/tests/src/tests/broad_phase/benchmark_broad_phase.cpp @@ -10,8 +10,60 @@ #include #include +#include + using namespace ipc; +namespace { +class HashGridTraversalBenchmark : public HashGrid { +public: + void prepare(size_t count, bool sparse) + { + clear(); + dim = 3; + vertex_boxes.resize(count); + vertex_items.reserve(count); + // Prebuilt sorted cell runs isolate traversal from grid construction. + for (size_t i = 0; i < count; ++i) { + const long cell = sparse ? static_cast(i / 2) : 0; + const double x = + sparse ? static_cast(cell) + (i % 2 ? .75 : .25) : 0.; + auto& box = vertex_boxes[i]; + box.min = Eigen::Array3d(x, 0., 0.); + box.max = box.min; + box.vertex_ids = { { static_cast(i), -1, -1 } }; + vertex_items.emplace_back(cell, static_cast(i)); + } + if (!sparse) { + can_vertices_collide = [](size_t, size_t) { return false; }; + } + } +}; +} // namespace + +TEST_CASE( + "Benchmark HashGrid traversal on sparse and reject-heavy cells", + "[!benchmark][broad_phase][hash_grid][traversal]") +{ + const int threads = GENERATE(1, 16); + const bool sparse = GENERATE(false, true); + const tbb::global_control limit( + tbb::global_control::max_allowed_parallelism, threads); + HashGridTraversalBenchmark grid; + grid.prepare(sparse ? 1048578 : 4096, sparse); + std::vector candidates; + BENCHMARK( + fmt::format( + "HashGrid {} ({} threads)", sparse ? "sparse" : "reject-all", + threads)) + { + grid.detect_vertex_vertex_candidates(candidates); + return candidates.size(); + }; + grid.detect_vertex_vertex_candidates(candidates); + CHECK(candidates.empty()); +} + TEST_CASE("Benchmark broad phase on simple scenes", "[!benchmark][broad_phase]") { Eigen::MatrixXd V0, V1; diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index e30ea47e3..d30c117e7 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -11,10 +12,243 @@ #include #include +#include +#include #include using namespace ipc; +namespace { +class SingleCellHashGrid : public HashGrid { +public: + using HashGrid::build; + +protected: + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) override + { + BroadPhase::build(edges, faces); + Eigen::Array3d lo, hi; + compute_mesh_aabb(lo, hi); + resize(lo, hi, (hi - lo).maxCoeff() + 1.); + insert_boxes(); + } +}; + +class UnitCellHashGrid : public HashGrid { +public: + using HashGrid::build; + +protected: + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) override + { + BroadPhase::build(edges, faces); + Eigen::Array3d lo, hi; + compute_mesh_aabb(lo, hi); + resize(lo.floor(), hi.ceil(), 1.); + insert_boxes(); + } +}; + +using CandidateIDs = std::array; +CandidateIDs candidate_ids(const VertexVertexCandidate& c) +{ + return { { c.vertex0_id, c.vertex1_id } }; +} +CandidateIDs candidate_ids(const EdgeVertexCandidate& c) +{ + return { { c.edge_id, c.vertex_id } }; +} +CandidateIDs candidate_ids(const EdgeEdgeCandidate& c) +{ + return { { c.edge0_id, c.edge1_id } }; +} +CandidateIDs candidate_ids(const FaceVertexCandidate& c) +{ + return { { c.face_id, c.vertex_id } }; +} +CandidateIDs candidate_ids(const EdgeFaceCandidate& c) +{ + return { { c.edge_id, c.face_id } }; +} +CandidateIDs candidate_ids(const FaceFaceCandidate& c) +{ + return { { c.face0_id, c.face1_id } }; +} + +template +std::vector +candidate_ids(const std::vector& candidates) +{ + std::vector result; + result.reserve(candidates.size()); + for (const auto& candidate : candidates) { + result.push_back(candidate_ids(candidate)); + } + return result; +} + +void check_all_hash_grid_relations(HashGrid& grid, BruteForce& reference) +{ +#define COMPARE_RELATION(TYPE, METHOD) \ + { \ + std::vector actual, expected; \ + grid.METHOD(actual); \ + reference.METHOD(expected); \ + CHECK(std::is_sorted(actual.begin(), actual.end())); \ + std::sort(expected.begin(), expected.end()); \ + CHECK( \ + std::adjacent_find(actual.begin(), actual.end()) == actual.end()); \ + CHECK(candidate_ids(actual) == candidate_ids(expected)); \ + } + COMPARE_RELATION(VertexVertexCandidate, detect_vertex_vertex_candidates) + COMPARE_RELATION(EdgeVertexCandidate, detect_edge_vertex_candidates) + COMPARE_RELATION(EdgeEdgeCandidate, detect_edge_edge_candidates) + COMPARE_RELATION(FaceVertexCandidate, detect_face_vertex_candidates) + COMPARE_RELATION(EdgeFaceCandidate, detect_edge_face_candidates) + COMPARE_RELATION(FaceFaceCandidate, detect_face_face_candidates) +#undef COMPARE_RELATION +} +} // namespace + +TEST_CASE( + "HashGrid cell groups preserve all candidate relations", + "[broad_phase][hash_grid][regression]") +{ + const int dim = GENERATE(2, 3); + const bool swept = GENERATE(false, true); + const int filter = GENERATE(0, 1, 2); + const bool single_cell = GENERATE(false, true); + CAPTURE(dim, swept, filter, single_cell); + Eigen::MatrixXd V(132, dim); + for (int i = 0; i < V.rows(); ++i) { + V(i, 0) = (i % 11) * .02 - .1; + V(i, 1) = ((i / 11) % 7) * .03 - .08; + if (dim == 3) { + V(i, 2) = (i % 5) * .01; + } + } + Eigen::MatrixXd V1 = V; + for (int i = 0; i < V.rows(); ++i) { + V1(i, 0) += (i % 3 - 1) * .04; + } + Eigen::MatrixXi E(100, 2), F(80, 3); + for (int i = 0; i < E.rows(); ++i) { + E.row(i) << i % V.rows(), (i + 3) % V.rows(); + } + for (int i = 0; i < F.rows(); ++i) { + F.row(i) << (i * 3) % V.rows(), (i * 3 + 1) % V.rows(), + (i * 3 + 2) % V.rows(); + } + HashGrid ordinary; + SingleCellHashGrid dense; + HashGrid& grid = single_cell ? static_cast(dense) : ordinary; + BruteForce reference; + if (filter == 1) { + grid.can_vertices_collide = [](size_t a, size_t b) { + return (a + b) % 3 != 0 && a % 5 != b % 5; + }; + } else if (filter == 2) { + grid.can_vertices_collide = [](size_t, size_t) { return false; }; + } + reference.can_vertices_collide = grid.can_vertices_collide; + for (double inflation : { 0., .02, .3 }) { + CAPTURE(inflation); + if (swept) { + grid.build(V, V1, E, F, inflation); + reference.build(V, V1, E, F, inflation); + } else { + grid.build(V, E, F, inflation); + reference.build(V, E, F, inflation); + } + check_all_hash_grid_relations(grid, reference); + } + grid.clear(); + reference.clear(); + check_all_hash_grid_relations(grid, reference); + grid.build(V, E, F, .02); + reference.build(V, E, F, .02); + check_all_hash_grid_relations(grid, reference); +} + +TEST_CASE( + "HashGrid cell groups handle empty primitive relations", + "[broad_phase][hash_grid][regression]") +{ + Eigen::MatrixXd V(5, 3); + V << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 1; + Eigen::MatrixXi E(0, 2), F(0, 3); + const int primitives = GENERATE(0, 1, 2); + if (primitives == 1) { + E.resize(1, 2); + E << 0, 1; + } + if (primitives == 2) { + F.resize(1, 3); + F << 0, 1, 2; + } + HashGrid grid; + BruteForce reference; + grid.build(V, E, F, .02); + reference.build(V, E, F, .02); + check_all_hash_grid_relations(grid, reference); +} + +TEST_CASE( + "HashGrid cell batches preserve exact candidate IDs and order", + "[broad_phase][hash_grid][regression]") +{ + // Exercise full and partial batches for same-type and cross-type queries. + const int num_cells = GENERATE(65535, 65536, 65537); + Eigen::MatrixXd V(4 * num_cells, 3); + Eigen::MatrixXi E(num_cells, 2), F(0, 3); + std::vector expected_vv, expected_ev; + expected_vv.reserve(2 * num_cells); + expected_ev.reserve(2 * num_cells); + for (int c = 0; c < num_cells; ++c) { + V.row(4 * c) << c + .25, .25, .25; + V.row(4 * c + 1) = V.row(4 * c); + V.row(4 * c + 2) << c + .75, .25, .25; + V.row(4 * c + 3) = V.row(4 * c + 2); + E.row(c) << 4 * c, 4 * c + 2; + expected_vv.push_back({ { 4 * c, 4 * c + 1 } }); + expected_vv.push_back({ { 4 * c + 2, 4 * c + 3 } }); + expected_ev.push_back({ { c, 4 * c + 1 } }); + expected_ev.push_back({ { c, 4 * c + 3 } }); + } + UnitCellHashGrid grid; + grid.build(V, E, F); + std::vector vv; + std::vector ev; + grid.detect_vertex_vertex_candidates(vv); + grid.detect_edge_vertex_candidates(ev); + CHECK(candidate_ids(vv) == expected_vv); + CHECK(candidate_ids(ev) == expected_ev); +} + +TEST_CASE( + "HashGrid filters cell pairs before testing AABB overlap", + "[broad_phase][hash_grid][regression]") +{ + Eigen::MatrixXd V(2, 3); + V << 0, 0, 0, 1, 0, 0; + Eigen::MatrixXi E(0, 2), F(0, 3); + std::atomic calls { 0 }; + SingleCellHashGrid grid; + grid.can_vertices_collide = [&](size_t, size_t) { + ++calls; + return false; + }; + grid.build(V, E, F); + std::vector candidates; + grid.detect_vertex_vertex_candidates(candidates); + CHECK(candidates.empty()); + CHECK(calls.load() == 1); +} + void test_face_face_broad_phase( const CollisionMesh& mesh, const Eigen::MatrixXd& V0, @@ -314,4 +548,4 @@ TEST_CASE("Create broad phase", "[broad_phase]") // The sentinel is not a method. CHECK_THROWS(create_broad_phase(BroadPhaseMethod::NUM_BROAD_PHASE_METHODS)); -} \ No newline at end of file +}