diff --git a/.clang-format b/.clang-format index a16b74015..357149634 100644 --- a/.clang-format +++ b/.clang-format @@ -1,5 +1,4 @@ --- -Language: Cpp BasedOnStyle: WebKit AlignAfterOpenBracket: AlwaysBreak AlignTrailingComments: @@ -38,6 +37,5 @@ IncludeCategories: SortPriority: 1 CaseSensitive: true PackConstructorInitializers: CurrentLine -RemoveEmptyLinesInUnwrappedLines: true SortIncludes: CaseInsensitive ... diff --git a/CMakeLists.txt b/CMakeLists.txt index 612f1ff9e..3ec77c505 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,15 +82,16 @@ if(CMAKE_CUDA_COMPILER) else() option(IPC_TOOLKIT_WITH_CUDA "Enable CUDA CCD" OFF) endif() -option(IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION "Use rational edge-triangle intersection check" OFF) -option(IPC_TOOLKIT_WITH_ROBIN_MAP "Use Tessil's robin-map rather than std maps" ON) -option(IPC_TOOLKIT_WITH_ABSEIL "Use Abseil's hash functions" ON) -option(IPC_TOOLKIT_WITH_FILIB "Use filib for interval arithmetic" ON) -option(IPC_TOOLKIT_WITH_INEXACT_CCD "Use the original inexact CCD method of IPC" OFF) +option(IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION "Use rational edge-triangle intersection check" OFF) +option(IPC_TOOLKIT_WITH_ROBIN_MAP "Use Tessil's robin-map rather than std maps" ON) +option(IPC_TOOLKIT_WITH_ABSEIL "Use Abseil's hash functions" ON) +option(IPC_TOOLKIT_WITH_FILIB "Use filib for interval arithmetic" ON) +option(IPC_TOOLKIT_WITH_INEXACT_CCD "Use the original inexact CCD method of IPC" OFF) +option(IPC_TOOLKIT_WITH_PROFILER "Enable performance profiler" ${IPC_TOOLKIT_TOPLEVEL_PROJECT}) # Advanced options -option(IPC_TOOLKIT_WITH_SIMD "Enable SIMD" OFF) -option(IPC_TOOLKIT_WITH_CODE_COVERAGE "Enable coverage reporting" OFF) +option(IPC_TOOLKIT_WITH_SIMD "Enable SIMD" OFF) +option(IPC_TOOLKIT_WITH_CODE_COVERAGE "Enable coverage reporting" OFF) mark_as_advanced(IPC_TOOLKIT_WITH_SIMD) # This does not work reliably mark_as_advanced(IPC_TOOLKIT_WITH_CODE_COVERAGE) # This is used in GitHub Actions @@ -207,6 +208,12 @@ if(IPC_TOOLKIT_WITH_FILIB) target_link_libraries(ipc_toolkit PUBLIC filib::filib) endif() +if(IPC_TOOLKIT_WITH_PROFILER) + # Add nlohmann/json for the profiler + include(json) + target_link_libraries(ipc_toolkit PRIVATE nlohmann_json::nlohmann_json) +endif() + # Extra warnings (link last for highest priority) include(ipc_toolkit_warnings) target_link_libraries(ipc_toolkit PRIVATE ipc::toolkit::warnings) diff --git a/python/examples/find_ipctk.py b/python/examples/find_ipctk.py new file mode 100644 index 000000000..8572b99da --- /dev/null +++ b/python/examples/find_ipctk.py @@ -0,0 +1,4 @@ +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parents[1])) +from _find_ipctk import ipctk # noqa diff --git a/python/examples/lbvh.py b/python/examples/lbvh.py new file mode 100644 index 000000000..21f88b3a6 --- /dev/null +++ b/python/examples/lbvh.py @@ -0,0 +1,77 @@ +from find_ipctk import ipctk +import meshio +import polyscope as ps +from polyscope import imgui +import numpy as np + +import pathlib + +mesh = meshio.read(pathlib.Path( + __file__).parents[2] / "tests/data/puffer-ball/20.ply") + +lbvh = ipctk.LBVH() +lbvh.build(mesh.points, np.array([], dtype=int), mesh.cells_dict["triangle"]) + +ps.init() + +ps.set_give_focus_on_show(True) + +ps_mesh = ps.register_surface_mesh( + "bunny", + mesh.points, + mesh.cells_dict["triangle"] +) + +nodes = lbvh.vertex_nodes + + +def traverse_lbvh(node, max_depth): + if node.is_inner and max_depth > 0: + V_left, E_left = traverse_lbvh(nodes[node.left], max_depth - 1) + V_right, E_right = traverse_lbvh(nodes[node.right], max_depth - 1) + return np.vstack([V_left, V_right]), np.vstack([E_left, E_right + V_left.shape[0]]) + + E = np.array([ + [0, 1], + [0, 2], + [0, 3], + [1, 5], + [1, 4], + [2, 4], + [2, 6], + [3, 5], + [3, 6], + [7, 4], + [7, 5], + [7, 6], + ]) + V = np.array([ + node.aabb_min, + [node.aabb_min[0], node.aabb_min[1], node.aabb_max[2]], + [node.aabb_min[0], node.aabb_max[1], node.aabb_min[2]], + [node.aabb_max[0], node.aabb_min[1], node.aabb_min[2]], + [node.aabb_min[0], node.aabb_max[1], node.aabb_max[2]], + [node.aabb_max[0], node.aabb_min[1], node.aabb_max[2]], + [node.aabb_max[0], node.aabb_max[1], node.aabb_min[2]], + node.aabb_max + ]) + return V, E + + +max_depth = 0 +bvh_nodes, bvh_edges = traverse_lbvh(nodes[0], max_depth=max_depth) + +ps.register_curve_network("bvh", bvh_nodes, bvh_edges) + + +def foo(): + global max_depth + changed, max_depth = imgui.SliderInt("max depth", max_depth, 0, 20) + if changed: + bvh_nodes, bvh_edges = traverse_lbvh(nodes[0], max_depth=max_depth) + ps.register_curve_network("bvh", bvh_nodes, bvh_edges) + + +ps.set_user_callback(foo) + +ps.show() diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 3e5422dec..e91daef66 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -27,6 +27,7 @@ PYBIND11_MODULE(ipctk, m) define_brute_force(m); define_bvh(m); define_hash_grid(m); + define_lbvh(m); define_spatial_hash(m); define_sweep_and_prune(m); define_sweep_and_tiniest_queue(m); diff --git a/python/src/broad_phase/CMakeLists.txt b/python/src/broad_phase/CMakeLists.txt index 2e5cdd237..71ac7f03c 100644 --- a/python/src/broad_phase/CMakeLists.txt +++ b/python/src/broad_phase/CMakeLists.txt @@ -4,6 +4,7 @@ set(SOURCES brute_force.cpp bvh.cpp hash_grid.cpp + lbvh.cpp spatial_hash.cpp sweep_and_prune.cpp sweep_and_tiniest_queue.cpp diff --git a/python/src/broad_phase/bindings.hpp b/python/src/broad_phase/bindings.hpp index 8718500d8..fccc9c315 100644 --- a/python/src/broad_phase/bindings.hpp +++ b/python/src/broad_phase/bindings.hpp @@ -7,6 +7,7 @@ void define_broad_phase(py::module_& m); void define_brute_force(py::module_& m); void define_bvh(py::module_& m); void define_hash_grid(py::module_& m); +void define_lbvh(py::module_& m); void define_spatial_hash(py::module_& m); void define_sweep_and_prune(py::module_& m); void define_sweep_and_tiniest_queue(py::module_& m); diff --git a/python/src/broad_phase/lbvh.cpp b/python/src/broad_phase/lbvh.cpp new file mode 100644 index 000000000..0fe9d77c8 --- /dev/null +++ b/python/src/broad_phase/lbvh.cpp @@ -0,0 +1,23 @@ +#include + +#include + +using namespace ipc; + +void define_lbvh(py::module_& m) +{ + py::class_(m, "LBVH_Node") + .def_readonly("aabb_min", &LBVH::Node::aabb_min) + .def_readonly("aabb_max", &LBVH::Node::aabb_max) + .def_readonly("left", &LBVH::Node::left) + .def_readonly("right", &LBVH::Node::right) + .def_property_readonly("is_leaf", &LBVH::Node::is_leaf) + .def_property_readonly("is_inner", &LBVH::Node::is_inner) + .def_property_readonly("is_valid", &LBVH::Node::is_valid); + + py::class_>(m, "LBVH") + .def(py::init()) + .def_property_readonly("vertex_nodes", &LBVH::vertex_nodes) + .def_property_readonly("edge_nodes", &LBVH::edge_nodes) + .def_property_readonly("face_nodes", &LBVH::face_nodes); +} diff --git a/src/ipc/broad_phase/CMakeLists.txt b/src/ipc/broad_phase/CMakeLists.txt index 8fe5a051d..084b37fab 100644 --- a/src/ipc/broad_phase/CMakeLists.txt +++ b/src/ipc/broad_phase/CMakeLists.txt @@ -10,6 +10,8 @@ set(SOURCES default_broad_phase.hpp hash_grid.cpp hash_grid.hpp + lbvh.cpp + lbvh.hpp spatial_hash.cpp spatial_hash.hpp sweep_and_prune.cpp diff --git a/src/ipc/broad_phase/bvh.cpp b/src/ipc/broad_phase/bvh.cpp index 668333726..b46b4b170 100644 --- a/src/ipc/broad_phase/bvh.cpp +++ b/src/ipc/broad_phase/bvh.cpp @@ -1,6 +1,7 @@ #include "bvh.hpp" #include +#include #include #include @@ -37,6 +38,8 @@ void BVH::init_bvh(const std::vector& boxes, SimpleBVH::BVH& bvh) return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::init_bvh"); + std::vector> vector_boxes(boxes.size()); for (int i = 0; i < boxes.size(); i++) { vector_boxes[i] = { { to_3D(boxes[i].min), to_3D(boxes[i].max) } }; @@ -47,6 +50,7 @@ void BVH::init_bvh(const std::vector& boxes, SimpleBVH::BVH& bvh) void BVH::clear() { + BroadPhase::clear(); vertex_bvh->clear(); edge_bvh->clear(); face_bvh->clear(); @@ -105,6 +109,8 @@ void BVH::detect_vertex_vertex_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::detect_vertex_vertex_candidates"); + detect_candidates< VertexVertexCandidate, /*swap_order=*/false, /*triangular=*/true>( vertex_boxes, *vertex_bvh, can_vertices_collide, candidates); @@ -117,6 +123,8 @@ void BVH::detect_edge_vertex_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::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( @@ -131,6 +139,8 @@ void BVH::detect_edge_edge_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::detect_edge_edge_candidates"); + detect_candidates< EdgeEdgeCandidate, /*swap_order=*/false, /*triangular=*/true>( edge_boxes, *edge_bvh, std::bind(&BVH::can_edges_collide, this, _1, _2), @@ -144,6 +154,8 @@ void BVH::detect_face_vertex_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::detect_face_vertex_candidates"); + // The ratio vertices:faces is 1:2, so we want to iterate over the vertices. detect_candidates( vertex_boxes, *face_bvh, @@ -157,6 +169,8 @@ void BVH::detect_edge_face_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::detect_edge_face_candidates"); + // The ratio edges:faces is 3:2, so we want to iterate over the faces. detect_candidates( face_boxes, *edge_bvh, @@ -170,6 +184,8 @@ void BVH::detect_face_face_candidates( return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::detect_face_face_candidates"); + detect_candidates< FaceFaceCandidate, /*swap_order=*/false, /*triangular=*/true>( face_boxes, *face_bvh, std::bind(&BVH::can_faces_collide, this, _1, _2), diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp new file mode 100644 index 000000000..b4bf85816 --- /dev/null +++ b/src/ipc/broad_phase/lbvh.cpp @@ -0,0 +1,636 @@ +#include "lbvh.hpp" + +#include +#include + +#include +#include +#include +#include + +using namespace std::placeholders; + +namespace ipc { + +namespace { + // Expands a 21-bit integer into 63 bits by inserting 2 zeros after each + // bit. + uint64_t expand_bits_2(uint64_t v) + { + v = (v | v << 32) & 0x1F00000000FFFF; + v = (v | v << 16) & 0x1F0000FF0000FF; + v = (v | v << 8) & 0x100F00F00F00F00F; + v = (v | v << 4) & 0x10C30C30C30C30C3; + v = (v | v << 2) & 0x1249249249249249; + return v; + } + + // Calculates a 63-bit Morton code for the given 3D point located within the + // unit cube [0,1]. + uint64_t morton_3D(double x, double y, double z) + { + constexpr double scale = 1 << 21; + x = std::clamp(x * scale, 0.0, scale - 1); + y = std::clamp(y * scale, 0.0, scale - 1); + z = std::clamp(z * scale, 0.0, scale - 1); + uint64_t xx = expand_bits_2(uint64_t(x)); + uint64_t yy = expand_bits_2(uint64_t(y)); + uint64_t zz = expand_bits_2(uint64_t(z)); + return (xx << 2) | (yy << 1) | zz; + } + + // Expands a 32-bit integer into 64 bits by inserting 1 zero after each bit. + uint64_t expand_bits_1(uint64_t v) + { + v = (v | (v << 16)) & 0x0000FFFF0000FFFF; + v = (v | (v << 8)) & 0x00FF00FF00FF00FF; + v = (v | (v << 4)) & 0x0F0F0F0F0F0F0F0F; + v = (v | (v << 2)) & 0x3333333333333333; + v = (v | (v << 1)) & 0x5555555555555555; + return v; + } + + // Calculates a 63-bit Morton code for the given 2D point located within the + // unit square [0,1]. + uint64_t morton_2D(double x, double y) + { + constexpr double scale = 1 << 21; + x = std::clamp(x * scale, 0.0, scale - 1); + y = std::clamp(y * scale, 0.0, scale - 1); + uint64_t xx = expand_bits_1(uint64_t(x)); + uint64_t yy = expand_bits_1(uint64_t(y)); + return (xx << 1) | yy; + } +} // namespace + +LBVH::LBVH() : BroadPhase() { } + +LBVH::~LBVH() = default; + +void LBVH::build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + BroadPhase::build(edges, faces); // Build edge_boxes and face_boxes + + assert(vertex_boxes.size() > 0); + mesh_aabb = { vertex_boxes[0].min, vertex_boxes[0].max }; + for (const auto& box : vertex_boxes) { + mesh_aabb.min = mesh_aabb.min.min(box.min); + mesh_aabb.max = mesh_aabb.max.max(box.max); + } + + init_bvh(vertex_boxes, vertex_bvh); + init_bvh(edge_boxes, edge_bvh); + init_bvh(face_boxes, face_bvh); +} + +namespace { + + int delta( + const std::vector& sorted_morton_codes, + int i, + uint64_t code_i, + int j) + { + if (j < 0 || j > sorted_morton_codes.size() - 1) { + return -1; + } + uint64_t code_j = sorted_morton_codes[j].morton_code; + if (code_i == code_j) { + // handle duplicate morton codes + int element_idx_i = i; // sorted_morton_codes[i].elementIdx; + int element_idx_j = j; // sorted_morton_codes[j].elementIdx; +// add 32 for common prefix of code_i ^ code_j +#if defined(__GNUC__) || defined(__clang__) + return 32 + __builtin_clz(element_idx_i ^ element_idx_j); +#elif defined(WIN32) + return 32 + __lzcnt(element_idx_i ^ element_idx_j); +#endif + } +#if defined(__GNUC__) || defined(__clang__) + return __builtin_clzll(code_i ^ code_j); +#elif defined(WIN32) + return __lzcnt64(code_i ^ code_j); +#endif + } + + void determine_range( + const std::vector& sorted_morton_codes, + int idx, + int& lower, + int& upper) + { + // determine direction of the range (+1 or -1) + const uint64_t code = sorted_morton_codes[idx].morton_code; + const int delta_l = delta(sorted_morton_codes, idx, code, idx - 1); + const int delta_r = delta(sorted_morton_codes, idx, code, idx + 1); + const int d = (delta_r >= delta_l) ? 1 : -1; + + // compute upper bound for the length of the range + const int delta_min = std::min(delta_l, delta_r); + int l_max = 2; + while (delta(sorted_morton_codes, idx, code, idx + l_max * d) + > delta_min) { + l_max = l_max << 1; + } + + // find the other end using binary search + int l = 0; + for (int t = l_max >> 1; t > 0; t >>= 1) { + if (delta(sorted_morton_codes, idx, code, idx + (l + t) * d) + > delta_min) { + l += t; + } + } + int jdx = idx + l * d; + + // ensure idx < jdx + lower = std::min(idx, jdx); + upper = std::max(idx, jdx); + } + + int find_split( + const std::vector& sorted_morton_codes, + int first, + int last) + { + uint64_t first_code = sorted_morton_codes[first].morton_code; + + // Calculate the number of highest bits that are the same + // for all objects, using the count-leading-zeros intrinsic. + int common_prefix = delta(sorted_morton_codes, first, first_code, last); + + // Use binary search to find where the next bit differs. + // Specifically, we are looking for the highest object that + // shares more than common_prefix bits with the first one. + int split = first; // initial guess + int stride = last - first; + do { + stride = (stride + 1) >> 1; // exponential decrease + int new_split = split + stride; // proposed new position + if (new_split < last) { + int split_prefix = + delta(sorted_morton_codes, first, first_code, new_split); + if (split_prefix > common_prefix) { + split = new_split; // accept proposal + } + } + } while (stride > 1); + + return split; + } +} // namespace + +void LBVH::init_bvh( + const std::vector& boxes, std::vector& lbvh) const +{ + if (boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::init_bvh"); + + std::vector morton_codes(boxes.size()); + { + IPC_TOOLKIT_PROFILE_BLOCK("compute_morton_codes"); + const ArrayMax3d mesh_width = mesh_aabb.max - mesh_aabb.min; + tbb::parallel_for( + tbb::blocked_range(0, boxes.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); i++) { + const auto& box = boxes[i]; + + const ArrayMax3d center = 0.5 * (box.min + box.max); + const ArrayMax3d mapped_center = + (center - mesh_aabb.min) / mesh_width; + + if (mapped_center.size() == 2) { + morton_codes[i].morton_code = + morton_2D(mapped_center.x(), mapped_center.y()); + } else { + morton_codes[i].morton_code = morton_3D( + mapped_center.x(), mapped_center.y(), + mapped_center.z()); + } + morton_codes[i].box_id = i; + } + }); + } + + { + IPC_TOOLKIT_PROFILE_BLOCK("sort_morton_codes"); + tbb::parallel_sort( + morton_codes.begin(), morton_codes.end(), + [](const MortonCodeElement& a, const MortonCodeElement& b) { + return a.morton_code < b.morton_code; + }); + } + + const int LEAF_OFFSET = int(boxes.size()) - 1; + + lbvh.resize(2 * boxes.size() - 1); + std::vector construction_infos(2 * boxes.size() - 1); + { + IPC_TOOLKIT_PROFILE_BLOCK("build_hierarchy"); + tbb::parallel_for( + tbb::blocked_range(0, boxes.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); i++) { + + assert(i < boxes.size()); + { + const auto& box = boxes[morton_codes[i].box_id]; + lbvh[LEAF_OFFSET + i].left = Node::INVALID_POINTER; + lbvh[LEAF_OFFSET + i].right = Node::INVALID_POINTER; + lbvh[LEAF_OFFSET + i].aabb_min = box.min; + lbvh[LEAF_OFFSET + i].aabb_max = box.max; + lbvh[LEAF_OFFSET + i].vertex_ids = box.vertex_ids; + lbvh[LEAF_OFFSET + i].primitive_id = + morton_codes[i].box_id; + } + + if (i < boxes.size() - 1) { + // Find out which range of objects the node corresponds + // to. (This is where the magic happens!) + + int first, last; + determine_range(morton_codes, int(i), first, last); + + // Determine where to split the range + int split = find_split(morton_codes, first, last); + + // Select child_a + int child_a = -1; + if (split == first) { + // pointer to leaf node + child_a = LEAF_OFFSET + split; + } else { + child_a = split; // pointer to internal node + } + + // Select child_b + int child_b = -1; + if (split + 1 == last) { + child_b = + LEAF_OFFSET + split + 1; // pointer to leaf node + } else { + child_b = split + 1; // pointer to internal node + } + + // Record parent-child relationships + if constexpr (Node::ABSOLUTE_POINTERS) { + lbvh[i].left = child_a; + lbvh[i].right = child_b; + } else { + lbvh[i].left = child_a - int(i); + lbvh[i].right = child_b - int(i); + } + construction_infos[child_a].parent = int(i); + construction_infos[child_b].parent = int(i); + } + + // node 0 is the root + if (i == 0) { + construction_infos[0].parent = 0; + } + } + }); + } + + { + IPC_TOOLKIT_PROFILE_BLOCK("populate_boxes"); + tbb::parallel_for( + tbb::blocked_range(0, boxes.size()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); i++) { + int node_idx = construction_infos[LEAF_OFFSET + i].parent; + while (true) { + auto& info = construction_infos[node_idx]; + if (info.visitation_count++ == 0) { + // this is the first thread that arrived at this + // node -> finished + break; + } + // this is the second thread that arrived at this node, + // both children are computed -> compute aabb union and + // continue + const Node& child_b = + lbvh[Node::POINTER(node_idx, lbvh[node_idx].right)]; + const Node& child_a = + lbvh[Node::POINTER(node_idx, lbvh[node_idx].left)]; + + lbvh[node_idx].aabb_min = + child_a.aabb_min.min(child_b.aabb_min); + lbvh[node_idx].aabb_max = + child_a.aabb_max.max(child_b.aabb_max); + + if (node_idx == 0) { + break; // root node + } + node_idx = info.parent; + } + } + }); + } +} + +void LBVH::clear() +{ + BroadPhase::clear(); + vertex_bvh.clear(); + edge_bvh.clear(); + face_bvh.clear(); +} + +namespace { + inline bool share_a_vertex( + const std::array& a, const std::array& b) + { + return a[0] == b[0] || a[0] == b[1] || a[0] == b[2] + || (a[1] >= 0 && (a[1] == b[0] || a[1] == b[1] || a[1] == b[2])) + || (a[2] >= 0 && (a[2] == b[0] || a[2] == b[1] || a[2] == b[2])); + } + + template + inline void attempt_add_candidate( + const LBVH::Node& query, + const LBVH::Node& node, + const std::function& can_collide, + std::vector& candidates) + { + if (share_a_vertex(query.vertex_ids, node.vertex_ids)) { + return; + } + + int i = query.primitive_id, j = node.primitive_id; + if constexpr (swap_order) { + std::swap(i, j); + } + + if constexpr (triangular) { + if (i >= j) { + return; + } + } + + if (!can_collide(i, j)) { + return; + } + + candidates.emplace_back(i, j); + } + + template + void traverse_lbvh( + const LBVH::Node& query, + const std::vector& lbvh, + const std::function& can_collide, + std::vector& candidates) + { + // Use a fixed-size array as a stack to avoid dynamic allocations + constexpr int MAX_STACK_SIZE = 64; + int stack[MAX_STACK_SIZE]; + int stack_ptr = 0; + stack[stack_ptr++] = LBVH::Node::INVALID_POINTER; + + int node_idx = 0; // root + do { + const LBVH::Node& node = lbvh[node_idx]; + +#if defined(__GNUC__) || defined(__clang__) + // Prefetch child nodes to reduce cache misses + __builtin_prefetch(&lbvh[node.left], 0, 1); + __builtin_prefetch(&lbvh[node.right], 0, 1); +#endif + + 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); + + // Query overlaps a leaf node => report collision. + if (intersects_l && child_l.is_leaf()) { + attempt_add_candidate( + query, child_l, can_collide, candidates); + } + if (intersects_r && child_r.is_leaf()) { + attempt_add_candidate( + query, child_r, can_collide, candidates); + } + + // Query overlaps an internal node => traverse. + bool traverse_l = (intersects_l && !child_l.is_leaf()); + bool traverse_r = (intersects_r && !child_r.is_leaf()); + + if (!traverse_l && !traverse_r) { + node_idx = stack[--stack_ptr]; + } else { + node_idx = traverse_l ? node.left : node.right; + if (traverse_l && traverse_r) { + // Postpone traversal of the right child + stack[stack_ptr++] = node.right; + } + } + } while (node_idx != LBVH::Node::INVALID_POINTER); // Same as root + } + + // Traverses the target BVH independently for each leaf node in the source + // BVH. For each source leaf, performs a stack-based traversal of the target + // BVH, collecting candidate pairs that pass the can_collide predicate. + // Results are stored in thread-local storage for later merging. + template + void independent_traversal( + const std::vector& source, + const std::vector& target, + const std::function& can_collide, + tbb::enumerable_thread_specific>& storage) + { + // Calculate the offset to the first leaf node in the source BVH. + const size_t SOURCE_LEAF_OFFSET = source.size() / 2; + const size_t N_SOURCE_LEAVES = SOURCE_LEAF_OFFSET + 1; + + tbb::parallel_for( + tbb::blocked_range(size_t(0), N_SOURCE_LEAVES), + [&](const tbb::blocked_range& r) { + auto& local_candidates = storage.local(); + + for (size_t i = r.begin(); i < r.end(); ++i) { + const auto& query_node = source[SOURCE_LEAF_OFFSET + i]; + traverse_lbvh( + query_node, target, can_collide, local_candidates); + } + }); + } + + // Parallel traversal of two BVHs using TBB task_group. + // Recursively explores all pairs of nodes whose bounding boxes intersect, + // adding candidate pairs when both nodes are leaves. + template + void traverse_lbvh( + const std::vector& source, + const std::vector& target, + int source_idx, + int target_idx, + const std::function& can_collide, + tbb::task_group& g, + tbb::enumerable_thread_specific>& storage) + { + // 1. Check for bounding box intersection + if (!source[source_idx].intersects(target[target_idx])) { + return; + } + + // 2. Handle leaf nodes (base case) + if (source[source_idx].is_leaf() && target[target_idx].is_leaf()) { + attempt_add_candidate( + source[source_idx], target[target_idx], can_collide, + storage.local()); + return; + } + + // 3. Handle mixed or internal nodes (parallel recursion) + + // TBB's task_group provides an easy way to offload tasks. + const auto dispatch = [&](int source_idx, int target_idx) { + g.run([&, source_idx, target_idx] { + traverse_lbvh( + source, target, source_idx, target_idx, can_collide, g, + storage); + }); + }; + + if (source[source_idx].is_leaf()) { + dispatch(source_idx, target[target_idx].left); + dispatch(source_idx, target[target_idx].right); + } else if (target[target_idx].is_leaf()) { + dispatch(source[source_idx].left, target_idx); + dispatch(source[source_idx].right, target_idx); + } else { + // Both internal nodes, test all four combinations. + dispatch(source[source_idx].left, target[target_idx].left); + dispatch(source[source_idx].left, target[target_idx].right); + dispatch(source[source_idx].right, target[target_idx].left); + dispatch(source[source_idx].right, target[target_idx].right); + } + } + + template + void simultaneous_traversal( + const std::vector& source, + const std::vector& target, + const std::function& can_collide, + tbb::enumerable_thread_specific>& storage) + { + tbb::task_group g; // TBB task group to manage parallel work + + g.run_and_wait([&] { + traverse_lbvh( + source, target, /*source_root*/ 0, /*target_root*/ 0, + can_collide, g, storage); + }); + } +} // namespace + +template +void LBVH::detect_candidates( + const std::vector& source, + const std::vector& target, + const std::function& can_collide, + std::vector& candidates) +{ + tbb::enumerable_thread_specific> storage; + + independent_traversal( + source, target, can_collide, storage); + // simultaneous_traversal( + // source, target, can_collide, storage); + + merge_thread_local_vectors(storage, candidates); +} + +void LBVH::detect_vertex_vertex_candidates( + std::vector& candidates) const +{ + if (vertex_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_vertex_candidates"); + + detect_candidates(vertex_bvh, can_vertices_collide, candidates); +} + +void LBVH::detect_edge_vertex_candidates( + std::vector& candidates) const +{ + if (edge_boxes.size() == 0 || vertex_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("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, + std::bind(&LBVH::can_edge_vertex_collide, this, _1, _2), candidates); +} + +void LBVH::detect_edge_edge_candidates( + std::vector& candidates) const +{ + if (edge_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_edge_candidates"); + + detect_candidates( + edge_bvh, std::bind(&LBVH::can_edges_collide, this, _1, _2), + candidates); +} + +void LBVH::detect_face_vertex_candidates( + std::vector& candidates) const +{ + if (face_boxes.size() == 0 || vertex_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("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, + std::bind(&LBVH::can_face_vertex_collide, this, _1, _2), candidates); +} + +void LBVH::detect_edge_face_candidates( + std::vector& candidates) const +{ + if (edge_boxes.size() == 0 || face_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("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, + std::bind(&LBVH::can_edge_face_collide, this, _1, _2), candidates); +} + +void LBVH::detect_face_face_candidates( + std::vector& candidates) const +{ + if (face_boxes.size() == 0) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_face_face_candidates"); + detect_candidates( + face_bvh, std::bind(&LBVH::can_faces_collide, this, _1, _2), + candidates); +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp new file mode 100644 index 000000000..1b275fb21 --- /dev/null +++ b/src/ipc/broad_phase/lbvh.hpp @@ -0,0 +1,183 @@ +#pragma once + +#include + +#include + +namespace ipc { + +/// @brief Linear Bounding Volume Hierarchy (LBVH) broad phase collision detection. +class LBVH : public BroadPhase { +public: + static constexpr index_t INVALID_ID = 0xFFFFFFFF; + + struct Node { + static constexpr int32_t INVALID_POINTER = 0x0; // do not change + + // true to use absolute pointers (left/right child pointer is the + // absolute index of the child in the buffer/array) or false for + // relative pointers (left/right child pointer is the relative pointer + // from the parent index to the child index in the buffer, i.e. absolute + // child pointer = absolute parent pointer + relative child pointer) + static constexpr bool ABSOLUTE_POINTERS = true; + + // helper function to handle relative pointers on + // CPU side, i.e. convert them to absolute + // pointers for array indexing + static constexpr uint32_t POINTER(uint32_t index, uint32_t pointer) + { + return ABSOLUTE_POINTERS ? pointer : index + pointer; + } + + /// @brief The min corner of the AABB + ArrayMax3d aabb_min; + /// @brief The max corner of the AABB + ArrayMax3d aabb_max; + /// @brief The vertex ids (v0, v1, v2) + std::array vertex_ids; + /// @brief Pointer to the left child or INVALID_POINTER in case of leaf + int left; + /// @brief Pointer to the right child or INVALID_POINTER in case of leaf + int right; + /// @brief The primitive id (INVALID_ID <=> inner node) + index_t primitive_id; + + bool is_leaf() const + { + assert(is_valid()); + return left == INVALID_POINTER && right == INVALID_POINTER; + } + + bool is_inner() const + { + return left != INVALID_POINTER && right != INVALID_POINTER; + } + + bool is_valid() const + { + return !((left == INVALID_POINTER) ^ (right == INVALID_POINTER)); + } + + bool intersects(const Node& other) const + { + return (aabb_min <= other.aabb_max).all() + && (other.aabb_min <= aabb_max).all(); + } + }; + + struct MortonCodeElement { + uint64_t morton_code; ///< Key for sorting + size_t box_id; ///< Pointer into boxes buffer + }; + + struct ConstructionInfo { + /// @brief Parent to the parent + int parent; + /// @brief Number of threads that arrived + std::atomic visitation_count = 0; + }; + +public: + LBVH(); + ~LBVH(); + + /// @brief Get the name of the broad phase method. + /// @return The name of the broad phase method. + std::string name() const override { return "LBVH"; } + + using BroadPhase::build; + + /// @brief Clear any built data. + void clear() override; + + /// @brief Find the candidate vertex-vertex collisions. + /// @param[out] candidates The candidate vertex-vertex collisions. + void detect_vertex_vertex_candidates( + std::vector& candidates) const override; + + /// @brief Find the candidate edge-vertex collisions. + /// @param[out] candidates The candidate edge-vertex collisions. + void detect_edge_vertex_candidates( + std::vector& candidates) const override; + + /// @brief Find the candidate edge-edge collisions. + /// @param[out] candidates The candidate edge-edge collisions. + void detect_edge_edge_candidates( + std::vector& candidates) const override; + + /// @brief Find the candidate face-vertex collisions. + /// @param[out] candidates The candidate face-vertex collisions. + void detect_face_vertex_candidates( + std::vector& candidates) const override; + + /// @brief Find the candidate edge-face intersections. + /// @param[out] candidates The candidate edge-face intersections. + void detect_edge_face_candidates( + std::vector& candidates) const override; + + /// @brief Find the candidate face-face collisions. + /// @param[out] candidates The candidate face-face collisions. + void detect_face_face_candidates( + std::vector& candidates) const override; + + const std::vector& vertex_nodes() const { return vertex_bvh; } + const std::vector& edge_nodes() const { return edge_bvh; } + const std::vector& face_nodes() const { return face_bvh; } + +protected: + /// @brief Build the broad phase for collision detection. + /// @note Assumes the vertex_boxes have been built. + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) override; + + /// @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 std::vector& boxes, std::vector& lbvh) 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] can_collide Function to determine if two primitives can collide given their ids. + /// @param[out] candidates The candidate collisions. + template < + typename Candidate, + bool swap_order = false, + bool triangular = false> + static void detect_candidates( + const std::vector& source, + const std::vector& target, + const std::function& can_collide, + std::vector& candidates); + + template + static void detect_candidates( + const std::vector& source_and_target, + const std::function& can_collide, + std::vector& candidates) + { + detect_candidates( + source_and_target, source_and_target, can_collide, candidates); + } + + /// @brief BVH containing the vertices. + std::vector vertex_bvh; + /// @brief BVH containing the edges. + std::vector edge_bvh; + /// @brief BVH containing the faces. + std::vector face_bvh; + /// @brief The axis-aligned bounding box of the entire mesh. + struct { + ArrayMax3d min; + ArrayMax3d max; + } mesh_aabb; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index 61fb05ad6..9cf1bb020 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -16,6 +16,7 @@ #cmakedefine IPC_TOOLKIT_WITH_ROBIN_MAP #cmakedefine IPC_TOOLKIT_WITH_ABSEIL #cmakedefine IPC_TOOLKIT_WITH_FILIB +#cmakedefine IPC_TOOLKIT_WITH_PROFILER namespace ipc { diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index ce09c59a0..ba3f918de 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -11,6 +11,8 @@ set(SOURCES logger.cpp logger.hpp merge_thread_local.hpp + profiler.cpp + profiler.hpp save_obj.cpp save_obj.hpp unordered_map_and_set.cpp diff --git a/src/ipc/utils/profiler.cpp b/src/ipc/utils/profiler.cpp new file mode 100644 index 000000000..986250e5f --- /dev/null +++ b/src/ipc/utils/profiler.cpp @@ -0,0 +1,68 @@ +#include "profiler.hpp" + +#ifdef IPC_TOOLKIT_WITH_PROFILER + +#include +#include + +namespace ipc { + +Profiler& profiler() +{ + static Profiler instance; + return instance; +} + +void Profiler::write_csv(const std::string& filename) const +{ + std::ofstream file(filename); + if (!file.is_open()) { + logger().error("Failed to open file: {}", filename); + return; + } + write_csv(file); + file.close(); +} + +void Profiler::write_csv(std::ostream& os) const +{ + os << "Parent,Name,Time (ms),Count\n"; + + if (m_data.empty()) { + os << std::flush; + return; + } + + // Print the profiler data in CSV format using a breadth-first traversal + const nlohmann::json::json_pointer root; + std::queue queue; + queue.push(root); + + while (!queue.empty()) { + const auto ptr = queue.front(); + const auto parent = ptr.parent_pointer(); + queue.pop(); + + assert(m_data.contains(ptr)); + const auto& data = ptr == root ? m_data : m_data.at(ptr); + if (ptr != root) { + os << fmt::format( + "{},{},{:.6g},{:d}\n", parent == root ? "" : parent.back(), + ptr.back(), data.at("time_ms").get(), + data.at("count").get()); + } + + // Traverse child scopes + for (const auto& [key, val] : data.items()) { + if (val.is_object()) { + queue.push(ptr / key); + } + } + } + + os << std::flush; +} + +} // namespace ipc + +#endif \ No newline at end of file diff --git a/src/ipc/utils/profiler.hpp b/src/ipc/utils/profiler.hpp new file mode 100644 index 000000000..48a819b13 --- /dev/null +++ b/src/ipc/utils/profiler.hpp @@ -0,0 +1,136 @@ +#pragma once + +#include + +#ifdef IPC_TOOLKIT_WITH_PROFILER + +// clang-format off +#include +#include +// clang-format on + +#include + +#include +#include + +// Helper macro to stringify/paste after expansion +#define IPC_TOOLKIT_PROFILE_BLOCK_CONCAT_IMPL(a, b) a##b +#define IPC_TOOLKIT_PROFILE_BLOCK_CONCAT(a, b) \ + IPC_TOOLKIT_PROFILE_BLOCK_CONCAT_IMPL(a, b) + +#define IPC_TOOLKIT_PROFILE_BLOCK(...) \ + ipc::ProfilePoint IPC_TOOLKIT_PROFILE_BLOCK_CONCAT( \ + __ipc_profile_point_, __COUNTER__)(__VA_ARGS__) + +namespace ipc { + +class Profiler { +public: + Profiler() = default; + + // ~Profiler() { print(); } + + void clear() { m_data.clear(); } + + void start(const std::string& name) + { + current_scope.push_back(name); + if (!m_data.contains(current_scope)) { + m_data[current_scope] = { + { "time_ms", 0 }, + { "count", 0 }, + }; + } + } + + void stop(const double time_ms) + { + const static std::string log_fmt_text = fmt::format( + "[{}] {{}} {{:.6f}} ms", + fmt::format(fmt::fg(fmt::terminal_color::magenta), "timing")); + + logger().trace( + fmt::runtime(log_fmt_text), current_scope.to_string(), time_ms); + + assert(m_data.contains(current_scope)); + assert(m_data.at(current_scope).contains("time_ms")); + assert(m_data.at(current_scope).contains("count")); + m_data[current_scope]["time_ms"] = + m_data[current_scope]["time_ms"].get() + time_ms; + m_data[current_scope]["count"] = + m_data[current_scope]["count"].get() + 1; + current_scope.pop_back(); + } + + void reset() + { + m_data.clear(); + current_scope = nlohmann::json::json_pointer(); // root + } + + void print() const + { + logger().info( + "[{}] profiler: {}", + fmt::format(fmt::fg(fmt::terminal_color::magenta), "timing"), + m_data.dump(2)); + } + + void write_csv(const std::string& filename) const; + void print_csv() const { write_csv(std::cout); } + void write_csv(std::ostream& os) const; + + const nlohmann::json& data() const { return m_data; } + nlohmann::json& data() { return m_data; } + +protected: + nlohmann::json m_data; + nlohmann::json::json_pointer current_scope; +}; + +Profiler& profiler(); + +class ChronoTimer { +public: + void start() { m_start = std::chrono::high_resolution_clock::now(); } + void stop() { m_end = std::chrono::high_resolution_clock::now(); } + double getElapsedTimeInMilliSec() const + { + return std::chrono::duration(m_end - m_start) + .count(); + } + +private: + std::chrono::high_resolution_clock::time_point m_start, m_end; +}; + +template class ProfilePoint { +public: + ProfilePoint(const std::string& name) : ProfilePoint(profiler(), name) { } + + ProfilePoint(Profiler& p_profiler, const std::string& name) + : m_profiler(p_profiler) + { + m_profiler.start(name); + timer.start(); + } + + ~ProfilePoint() + { + timer.stop(); + m_profiler.stop(timer.getElapsedTimeInMilliSec()); + } + +protected: + Profiler& m_profiler; + Timer timer; +}; + +} // namespace ipc + +#else + +#define IPC_TOOLKIT_PROFILE_BLOCK(...) + +#endif \ No newline at end of file diff --git a/tests/src/tests/broad_phase/CMakeLists.txt b/tests/src/tests/broad_phase/CMakeLists.txt index 602b7947f..1806f2a49 100644 --- a/tests/src/tests/broad_phase/CMakeLists.txt +++ b/tests/src/tests/broad_phase/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_aabb.cpp test_broad_phase.cpp + test_lbvh.cpp test_spatial_hash.cpp test_stq.cpp test_voxel_size_heuristic.cpp diff --git a/tests/src/tests/broad_phase/test_lbvh.cpp b/tests/src/tests/broad_phase/test_lbvh.cpp new file mode 100644 index 000000000..8ddd33bd5 --- /dev/null +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -0,0 +1,255 @@ +#include + +#include +#include + +#include +#include +// #include +#include + +#include + +#include + +#include + +using namespace ipc; + +namespace { + +bool is_aabb_union(LBVH::Node parent, LBVH::Node childA, LBVH::Node childB) +{ + AABB children; + children.min = childA.aabb_min.min(childB.aabb_min); + children.max = childA.aabb_max.max(childB.aabb_max); + constexpr float EPS = 1e-4f; + return (abs(parent.aabb_max - children.max) < EPS).all() + && (abs(parent.aabb_min - children.min) < EPS).all(); +} + +void traverse_lbvh( + const std::vector& lbvh_nodes, + const uint32_t index, + std::vector& visited) +{ + const LBVH::Node& node = lbvh_nodes[index]; + CHECK(node.is_valid()); + + if (node.left == LBVH::Node::INVALID_POINTER) { + // leaf + CHECK(!visited[index]); + visited[index] = true; + } else { + // inner node + CHECK(!visited[index]); + visited[index] = true; + + uint32_t left_child_index = LBVH::Node::POINTER(index, node.left); + uint32_t right_child_index = LBVH::Node::POINTER(index, node.right); + + // verify aabbs + LBVH::Node childA = lbvh_nodes[left_child_index]; + LBVH::Node childB = lbvh_nodes[right_child_index]; + + { + CAPTURE( + index, left_child_index, right_child_index, + node.aabb_min.transpose(), childA.aabb_min.transpose(), + childB.aabb_min.transpose(), node.aabb_max.transpose(), + childA.aabb_max.transpose(), childB.aabb_max.transpose()); + CHECK(is_aabb_union(node, childA, childB)); + } + + // continue traversal + traverse_lbvh(lbvh_nodes, left_child_index, visited); + traverse_lbvh(lbvh_nodes, right_child_index, visited); + } +} + +void check_valid_lbvh_nodes(const std::vector& lbvh_nodes) +{ + std::vector visited(lbvh_nodes.size(), false); + traverse_lbvh(lbvh_nodes, 0, visited); + REQUIRE( + std::all_of(visited.begin(), visited.end(), [](bool v) { return v; })); +} +} // namespace + +TEST_CASE("LBVH::build", "[broad_phase][lbvh]") +{ + constexpr double inflation_radius = 1e-3; + + const std::shared_ptr lbvh = std::make_shared(); + + SECTION("Static") + { + const std::string mesh = GENERATE("cube.ply", "bunny.ply"); + + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + REQUIRE(tests::load_mesh(mesh, vertices, edges, faces)); + + lbvh->build(vertices, edges, faces, inflation_radius); + } + SECTION("Dynamic") + { + const std::string mesh_t0 = "cloth_ball92.ply"; + const std::string mesh_t1 = "cloth_ball93.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)); + + lbvh->build(vertices_t0, vertices_t1, edges, faces, inflation_radius); + } + + std::ofstream dot_file("/Users/zachary/Downloads/lbvh.dot"); + dot_file << "digraph G {\nnode [shape = circle;];" << std::endl; + + for (int i = 0; i < lbvh->vertex_nodes().size(); ++i) { + dot_file << "N" << i << " [label = \"" + << (lbvh->vertex_nodes()[i].is_inner() ? "I" : "L") + << (lbvh->vertex_nodes()[i].is_inner() + ? i + : lbvh->vertex_nodes()[i].primitive_id) + << "\"];" << std::endl; + } + + for (int i = 0; i < lbvh->vertex_nodes().size(); ++i) { + const auto& node = lbvh->vertex_nodes()[i]; + if (node.is_inner()) { + dot_file << "N" << i << " -> N" << node.left << ";" << std::endl; + dot_file << "N" << i << " -> N" << node.right << ";" << std::endl; + } + } + + dot_file << "}" << std::endl; + dot_file.close(); + + // -- TODO: Check the morton codes ---------------------------------------- + // -- TODO: Check the morton codes are sorted ----------------------------- + + // -- Check the LBVH nodes are all reachable and contain their children --- + + check_valid_lbvh_nodes(lbvh->vertex_nodes()); + // check_valid_lbvh_nodes(lbvh->edge_nodes()); + // check_valid_lbvh_nodes(lbvh->face_nodes()); + + // -- Check clear() works ------------------------------------------------- + lbvh->clear(); + + // CHECK(lbvh->vertex_boxes().empty()); + // CHECK(lbvh->edge_boxes().empty()); + // CHECK(lbvh->face_boxes().empty()); + + CHECK(lbvh->vertex_nodes().empty()); + CHECK(lbvh->edge_nodes().empty()); + CHECK(lbvh->face_nodes().empty()); +} + +TEST_CASE("LBVH::detect_*_candidates", "[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"; + } + // 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); + + const std::shared_ptr bvh = std::make_shared(); + bvh->build(vertices_t0, vertices_t1, edges, faces, inflation_radius); + + // detect_vertex_vertex_candidates + { + std::vector vv_candidates; + lbvh->detect_vertex_vertex_candidates(vv_candidates); + + std::vector expected_vv_candidates; + bvh->detect_vertex_vertex_candidates(expected_vv_candidates); + + CHECK(vv_candidates.size() == expected_vv_candidates.size()); + // TODO: Check the candidates are the same + } + + { + std::vector ev_candidates; + lbvh->detect_edge_vertex_candidates(ev_candidates); + + std::vector expected_ev_candidates; + bvh->detect_edge_vertex_candidates(expected_ev_candidates); + + CHECK(ev_candidates.size() == expected_ev_candidates.size()); + // TODO: Check the candidates are the same + } + + { + std::vector ee_candidates; + lbvh->detect_edge_edge_candidates(ee_candidates); + + std::vector expected_ee_candidates; + bvh->detect_edge_edge_candidates(expected_ee_candidates); + + CHECK(ee_candidates.size() == expected_ee_candidates.size()); + // TODO: Check the candidates are the same + } + + { + std::vector fv_candidates; + lbvh->detect_face_vertex_candidates(fv_candidates); + + std::vector expected_fv_candidates; + bvh->detect_face_vertex_candidates(expected_fv_candidates); + + CHECK(fv_candidates.size() == expected_fv_candidates.size()); + // TODO: Check the candidates are the same + } + + { + std::vector ef_candidates; + lbvh->detect_edge_face_candidates(ef_candidates); + + std::vector expected_ef_candidates; + bvh->detect_edge_face_candidates(expected_ef_candidates); + + CHECK(ef_candidates.size() == expected_ef_candidates.size()); + // TODO: Check the candidates are the same + } + + { + std::vector ff_candidates; + lbvh->detect_face_face_candidates(ff_candidates); + + std::vector expected_ff_candidates; + bvh->detect_face_face_candidates(expected_ff_candidates); + + CHECK(ff_candidates.size() == expected_ff_candidates.size()); + // TODO: Check the candidates are the same + } + +#ifdef IPC_TOOLKIT_WITH_PROFILER + ipc::profiler().print(); + ipc::profiler().clear(); +#endif +} diff --git a/tests/src/tests/main.cpp b/tests/src/tests/main.cpp index 5d9c0a594..19e734c78 100644 --- a/tests/src/tests/main.cpp +++ b/tests/src/tests/main.cpp @@ -43,7 +43,7 @@ int main(int argc, char** argv) using namespace Catch::Clara; auto cli = session.cli(); - int log_level = spdlog::level::err; + int log_level = spdlog::level::warn; cli |= Opt([&log_level](int const d) { return parse_log_level(d, log_level); }, "log_level")["--log"]["--logger-level"]( diff --git a/tests/src/tests/utils.cpp b/tests/src/tests/utils.cpp index 4bcdfee66..35a2398fe 100644 --- a/tests/src/tests/utils.cpp +++ b/tests/src/tests/utils.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #ifdef IPC_TOOLKIT_WITH_CUDA #include @@ -24,10 +25,11 @@ std::vector> broad_phases() { return { { std::make_shared(), - std::make_shared(), - std::make_shared(), + // std::make_shared(), + // std::make_shared(), std::make_shared(), - std::make_shared(), + std::make_shared(), + // std::make_shared(), #ifdef IPC_TOOLKIT_WITH_CUDA std::make_shared(), #endif