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/.github/workflows/python.yml b/.github/workflows/python.yml index 84ad270ae..94fe9871e 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -27,7 +27,7 @@ jobs: fail-fast: false matrix: os: [ubuntu-latest, macos-latest, windows-latest] - python-version: ${{ github.event_name == 'pull_request' && fromJSON('["3.13"]') || fromJSON('["3.9", "3.10", "3.11", "3.12", "3.13"]') }} + python-version: ${{ github.event_name == 'pull_request' && fromJSON('["3.14"]') || fromJSON('["3.10", "3.11", "3.12", "3.13", "3.14"]') }} include: - os: ubuntu-latest name: Linux diff --git a/CMakeLists.txt b/CMakeLists.txt index 68f6a71e1..19208d9e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ option(IPC_TOOLKIT_WITH_ROBIN_MAP "Use Tessil's robin-map rather tha 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" OFF) # Advanced options option(IPC_TOOLKIT_WITH_SIMD "Enable SIMD" OFF) @@ -230,6 +231,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 PUBLIC 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/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake index 4d5d6baa4..9c53bc956 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake @@ -30,7 +30,7 @@ else() SOURCE_DIR ${IPC_TOOLKIT_TESTS_DATA_DIR} GIT_REPOSITORY https://github.com/ipc-sim/ipc-toolkit-tests-data.git - GIT_TAG 7ca6db695adcc00d3d6d978767dfc0d81722a515 + GIT_TAG c7eba549d9a80d15569a013c473f0aff104ac44a CONFIGURE_COMMAND "" BUILD_COMMAND "" diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index b95cb04c6..3b50f2abc 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -100,6 +100,9 @@ else() -Wno-sign-compare + -Wno-gnu-anonymous-struct + -Wno-nested-anon-types + ########### # GCC 6.1 # ########### diff --git a/cmake/recipes/evouga_ccd.cmake b/cmake/recipes/evouga_ccd.cmake index dbdf03eb8..130bf3204 100644 --- a/cmake/recipes/evouga_ccd.cmake +++ b/cmake/recipes/evouga_ccd.cmake @@ -1,4 +1,4 @@ -# Etienne Vouga's CCD Library (https://github.com/evouga/collisiondetection.git) +# Etienne Vouga's CCD Library (https://github.com/evouga/collisiondetection) # License: ??? if(TARGET evouga::ccd) return() diff --git a/cmake/recipes/filib.cmake b/cmake/recipes/filib.cmake index c0fa879bd..fa3cf51cd 100644 --- a/cmake/recipes/filib.cmake +++ b/cmake/recipes/filib.cmake @@ -1,4 +1,4 @@ -# filib (https://github.com/zfergus/filib.git) +# filib (https://github.com/zfergus/filib) # License: LGPL-2.1 if(TARGET filib::filib) return() diff --git a/cmake/recipes/pybind11_json.cmake b/cmake/recipes/pybind11_json.cmake new file mode 100644 index 000000000..508b534c1 --- /dev/null +++ b/cmake/recipes/pybind11_json.cmake @@ -0,0 +1,26 @@ +# pybind11_json (https://github.com/pybind/pybind11_json) +# License: MIT +if(TARGET pybind11::json) + return() +endif() + +message(STATUS "Third-party: creating target 'pybind11::json'") + +include(CPM) +CPMAddPackage( + URI "gh:pybind/pybind11_json#0.2.15" + DOWNLOAD_ONLY YES +) + +add_library(pybind11_json INTERFACE) +add_library(pybind11::json ALIAS pybind11_json) + +target_include_directories(pybind11_json INTERFACE + "$" +) + +include(pybind11) +target_link_libraries(pybind11_json INTERFACE pybind11::pybind11) + +include(json) +target_link_libraries(pybind11_json INTERFACE nlohmann_json::nlohmann_json) \ No newline at end of file diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index 7e09a61a6..70a8c731b 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -83,4 +83,7 @@ digraph "IPC Toolkit Dependencies" { "node5" -> "node13" [color = "#8FB976";]; "node13" -> "node0" [color = "#BE6562";]; "node13" -> "node9" [color = "#BE6562";]; + // ipc_toolkit -> nlohmann_json + "node14" [label = "nlohmann_json\n(nlohmann_json::nlohmann_json)";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; + "node5" -> "node14" [color = "#8FB976";]; } \ No newline at end of file diff --git a/docs/source/_static/graphviz/dependencies.svg b/docs/source/_static/graphviz/dependencies.svg index 3713dcb99..2d65143ce 100644 --- a/docs/source/_static/graphviz/dependencies.svg +++ b/docs/source/_static/graphviz/dependencies.svg @@ -1,11 +1,11 @@ - - + IPC Toolkit Dependencies @@ -16,48 +16,48 @@ legendNode0 - -Static Library + +Static Library legendNode1 - -Shared Library + +Shared Library legendNode0->legendNode1 - - -Public + + +Public legendNode2 - -Interface Library + +Interface Library legendNode1->legendNode2 - - -Private + + +Private legendNode2->legendNode0 - - -Interface + + +Interface node5 - -ipc_toolkit -(ipc::toolkit) + +ipc_toolkit +(ipc::toolkit) @@ -69,7 +69,7 @@ node5->node0 - + @@ -82,8 +82,8 @@ node5->node1 - - + + @@ -95,7 +95,7 @@ node5->node2 - + @@ -108,7 +108,7 @@ node5->node3 - + @@ -121,8 +121,8 @@ node5->node6 - - + + @@ -134,8 +134,8 @@ node5->node7 - - + + @@ -147,7 +147,7 @@ node5->node8 - + @@ -160,8 +160,8 @@ node5->node9 - - + + @@ -173,7 +173,7 @@ node5->node10 - + @@ -186,7 +186,7 @@ node5->node11 - + @@ -199,8 +199,8 @@ node5->node12 - - + + @@ -212,8 +212,21 @@ node5->node13 - - + + + + + +node14 + +nlohmann_json +(nlohmann_json::nlohmann_json) + + + +node5->node14 + + diff --git a/docs/source/about/dependencies.rst b/docs/source/about/dependencies.rst index 62ca0b781..224761e0b 100644 --- a/docs/source/about/dependencies.rst +++ b/docs/source/about/dependencies.rst @@ -91,6 +91,18 @@ Additionally, IPC Toolkit may optionally use the following libraries: - `github.com/zfergus/filib `_ - |:white_check_mark:| - ``IPC_TOOLKIT_WITH_FILIB`` + * - nlohmann/json + - JSON parsing for profiler and tests + - MIT + - `github.com/nlohmann/json `_ + - |:white_large_square:| + - ``IPC_TOOLKIT_WITH_PROFILER`` + * - pybind11_json + - JSON support for pybind11 (used in profiler) + - MIT + - `github.com/pybind/pybind11_json `_ + - |:white_large_square:| + - ``IPC_TOOLKIT_BUILD_PYTHON`` and ``IPC_TOOLKIT_WITH_PROFILER`` * - rational-cpp - Rational arithmetic used for exact intersection checks (requires `GMP `_ to be installed at a system level) - MIT @@ -136,7 +148,4 @@ Unit Test Dependencies - `github.com/catchorg/Catch2 `_ * - finite-diff - Finite-difference comparisons - - `github.com/zfergus/finite-diff `_ - * - Nlohmann JSON - - Loading test data from JSON files - - `github.com/nlohmann/json `_ \ No newline at end of file + - `github.com/zfergus/finite-diff `_ \ No newline at end of file diff --git a/docs/source/cpp-api/broad_phase.rst b/docs/source/cpp-api/broad_phase.rst index 1c4962bda..766f08116 100644 --- a/docs/source/cpp-api/broad_phase.rst +++ b/docs/source/cpp-api/broad_phase.rst @@ -31,6 +31,12 @@ BVH .. doxygenclass:: ipc::BVH :allow-dot-graphs: +LBVH +--- + +.. doxygenclass:: ipc::LBVH + :allow-dot-graphs: + Sweep and Prune ----------------------- diff --git a/docs/source/python-api/broad_phase.rst b/docs/source/python-api/broad_phase.rst index d395856c8..807387e2b 100644 --- a/docs/source/python-api/broad_phase.rst +++ b/docs/source/python-api/broad_phase.rst @@ -36,6 +36,13 @@ BVH .. autoclasstoc:: +LBVH +---- + +.. autoclass:: ipctk.LBVH + + .. autoclasstoc:: + Sweep and Prune --------------- diff --git a/docs/source/references.bib b/docs/source/references.bib index 892c6c948..4d2245bf0 100644 --- a/docs/source/references.bib +++ b/docs/source/references.bib @@ -128,3 +128,33 @@ @article{Huang2025GCP number = 4, note = {\url{https://doi.org/10.1145/3731142}} } +@inproceedings{Karras2012HPG, + author = {Karras, Tero}, + title = {Maximizing Parallelism in the Construction of BVHs, Octrees, and k-d Trees}, + booktitle = {Proceedings of the Fourth Eurographics Conference on High-Performance Graphics}, + series = {HPG '12}, + year = {2012}, + pages = {33--37}, + publisher = {Eurographics Association}, + address = {Goslar, Germany}, + note = {\url{https://doi.org/10.2312/EGGH/HPG12/033-037}} +} +@phdthesis{Baraff1992PhD, + author = {Baraff, David}, + title = {Dynamic Simulation of Non-Penetrating Rigid Bodies}, + school = {Cornell University}, + address = {Ithaca, NY, USA}, + year = {1992}, + note = {Technical Report TR 92-1275} +} +@inproceedings{Cohen1995ICOLLIDE, + author = {Cohen, Jonathan D. and Lin, Ming C. and Manocha, Dinesh and Ponamgi, Madhav}, + title = {I-COLLIDE: An Interactive and Exact Collision Detection System for Large-Scale Environments}, + booktitle = {Proceedings of the 1995 Symposium on Interactive 3D Graphics}, + series = {I3D '95}, + year = {1995}, + pages = {189--196}, + publisher = {ACM}, + address = {New York, NY, USA}, + note = {\url{https://doi.org/10.1145/199404.199437}} +} \ No newline at end of file diff --git a/docs/source/tutorials/getting_started.rst b/docs/source/tutorials/getting_started.rst index 1b30d11c2..90a41b8dd 100644 --- a/docs/source/tutorials/getting_started.rst +++ b/docs/source/tutorials/getting_started.rst @@ -531,8 +531,7 @@ The ``Candidates`` class represents the culled set of candidate pairs and is bui candidates.build( mesh, vertices_t0, vertices_t1, broad_phase=ipctk.HashGrid()) -Possible values for ``broad_phase`` are: ``BruteForce`` (parallel brute force culling), ``HashGrid`` (default), ``SpatialHash`` (implementation from the original IPC codebase), -``BVH`` (`SimpleBVH `_), ``SweepAndPrune`` (method of :cite:t:`Belgrod2023Time`), or ``SweepAndTiniestQueue`` (requires CUDA). +Possible values for ``broad_phase`` are: ``BruteForce`` (parallel brute force culling), ``HashGrid`` (default), ``SpatialHash`` (implementation from the original IPC codebase), ``BVH`` (`SimpleBVH `_), ``LBVH`` (CPU implementation of :cite:t:`Karras2012HPG` using TBB), ``SweepAndPrune`` (a.k.a. Sort-and-Sweep from :cite:t:`Baraff1992PhD`), or ``SweepAndTiniestQueue`` (method of :cite:t:`Belgrod2023Time`; requires CUDA). Narrow-Phase ^^^^^^^^^^^^ diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c867a548e..ca73632f7 100755 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,6 +30,11 @@ if (FILIB_BUILD_SHARED_LIB AND WIN32) ) endif() +if (IPC_TOOLKIT_WITH_PROFILER) + include(pybind11_json) + target_link_libraries(ipctk PRIVATE pybind11::json) +endif() + # Extra warnings # target_link_libraries(ipctk PRIVATE IPCToolkit::warnings) diff --git a/python/examples/lbvh.py b/python/examples/lbvh.py new file mode 100644 index 000000000..9fe4efed2 --- /dev/null +++ b/python/examples/lbvh.py @@ -0,0 +1,85 @@ +from find_ipctk import ipctk +import meshio +import polyscope as ps +from polyscope import imgui +import numpy as np + +import pathlib + +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument( + "--mesh", + type=pathlib.Path, + default=(pathlib.Path(__file__).parents[2] / "tests/data/puffer-ball/20.ply")) +args = parser.parse_args() + +mesh = meshio.read(args.mesh) + +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( + "mesh", + mesh.points, + mesh.cells_dict["triangle"] +) + +nodes = lbvh.face_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/examples/profiler.py b/python/examples/profiler.py new file mode 100644 index 000000000..6d44b169b --- /dev/null +++ b/python/examples/profiler.py @@ -0,0 +1,81 @@ +import pandas as pd +import plotly.express as px +import pathlib +from io import StringIO + +from find_ipctk import ipctk + +def plot_profiler(title=None): + df = pd.read_csv(StringIO(ipctk.profiler().csv), na_filter=False) + # df["Time (s)"] = df["Time (ms)"] / 1000.0 + # df["Parent"] = df["Parent"].astype(str) + # df["Id"] = df["Id"].astype(str) + + fig = px.sunburst( + df, + ids="Id", + names="Name", + parents="Parent", + values="Time (ms)", + branchvalues="total", + color="Parent", + ) + fig.update_traces( + hovertemplate="%{label}
Time (ms): %{value}
Call Count: %{customdata[0]}
Percentage of Parent: %{percentParent:.2%}", + customdata=df[["Count"]].values, + # tiling=dict( + # orientation='v' + # ) + ) + fig.update_layout( + title=title or "Profiler Results", + title_x=0.5, + title_y=0.95, + margin=dict(t=0, l=0, r=0, b=0), + width=800, + height=800, + template="plotly_white", + ) + # fig.write_image(f"icicle_{pathlib.Path(csv_path).stem}.png", scale=2) + # fig.write_image(f"sunburst_{pathlib.Path(csv_path).stem}.png", scale=2) + fig.show() + return fig + +if __name__ == "__main__": + # plot(pathlib.Path(__file__).parent / "lbvh_profile.csv") + + import meshio + import argparse + import numpy as np + + parser = argparse.ArgumentParser() + parser.add_argument( + "--mesh", + type=pathlib.Path, + default=(pathlib.Path(__file__).parents[2] / "tests/data/puffer-ball/20.ply")) + parser.add_argument( + "--method", + type=str, + default="lvbh") + args = parser.parse_args() + + mesh = meshio.read(args.mesh) + faces = mesh.cells_dict["triangle"] + + edges = ipctk.edges(faces) + # indices = np.lexsort((edges[:, 1], edges[:, 0])) + # edges = edges[indices] + + # indices = np.lexsort((faces[:, 2], faces[:, 1], faces[:, 0])) + # faces = faces[indices] + + if args.method.lower() == "bvh": + bp = ipctk.BVH() + elif args.method.lower() == "lbvh": + bp = ipctk.LBVH() + + bp.build(mesh.points, edges, faces) + + candidates = bp.detect_collision_candidates() + + plot_profiler(title=args.mesh.parent.name + "/" + args.mesh.name) diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index af926b675..597b69656 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); @@ -119,6 +120,7 @@ PYBIND11_MODULE(ipctk, m) // utils define_logger(m); + define_profiler(m); define_thread_limiter(m); define_vertex_to_min_edge(m); define_world_bbox_diagonal_length(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/aabb.cpp b/python/src/broad_phase/aabb.cpp index b0d170468..e1d9d0617 100644 --- a/python/src/broad_phase/aabb.cpp +++ b/python/src/broad_phase/aabb.cpp @@ -1,6 +1,7 @@ #include #include +#include using namespace ipc; @@ -60,6 +61,19 @@ void define_aabb(py::module_& m) If the two AABBs intersect. )ipc_Qu8mg5v7", "other"_a) + .def( + "__repr__", + [](const AABB& aabb) { + return fmt::format( + "AABB(min={}, max={})", aabb.min.transpose(), + aabb.max.transpose()); + }) + .def( + "__str__", + [](const AABB& aabb) { + return fmt::format( + "{}×{}", aabb.min.transpose(), aabb.max.transpose()); + }) .def_static( "conservative_inflation", [](ArrayMax3d min, ArrayMax3d max, const double inflation_radius) { @@ -78,7 +92,7 @@ void define_aabb(py::module_& m) "build_vertex_boxes", [](Eigen::ConstRef vertices, const double inflation_radius = 0) { - std::vector vertex_boxes; + AABBs vertex_boxes; build_vertex_boxes(vertices, vertex_boxes, inflation_radius); return vertex_boxes; }, @@ -99,7 +113,7 @@ void define_aabb(py::module_& m) [](Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, const double inflation_radius = 0) { - std::vector vertex_boxes; + AABBs vertex_boxes; build_vertex_boxes( vertices_t0, vertices_t1, vertex_boxes, inflation_radius); return vertex_boxes; @@ -119,9 +133,8 @@ void define_aabb(py::module_& m) m.def( "build_edge_boxes", - [](const std::vector& vertex_boxes, - Eigen::ConstRef edges) { - std::vector edge_boxes; + [](const AABBs& vertex_boxes, Eigen::ConstRef edges) { + AABBs edge_boxes; build_edge_boxes(vertex_boxes, edges, edge_boxes); return edge_boxes; }, @@ -139,9 +152,8 @@ void define_aabb(py::module_& m) m.def( "build_face_boxes", - [](const std::vector& vertex_boxes, - Eigen::ConstRef faces) { - std::vector face_boxes; + [](const AABBs& vertex_boxes, Eigen::ConstRef faces) { + AABBs face_boxes; build_face_boxes(vertex_boxes, faces, face_boxes); return face_boxes; }, 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/broad_phase.cpp b/python/src/broad_phase/broad_phase.cpp index c8ab31784..812599b4b 100644 --- a/python/src/broad_phase/broad_phase.cpp +++ b/python/src/broad_phase/broad_phase.cpp @@ -247,18 +247,12 @@ void define_broad_phase(py::module_& m) )ipc_Qu8mg5v7") .def( "detect_collision_candidates", - [](const BroadPhase& self, int dim) { + [](const BroadPhase& self) { Candidates candidates; - self.detect_collision_candidates(dim, candidates); + self.detect_collision_candidates(candidates); return candidates; }, - R"ipc_Qu8mg5v7( - Detect all collision candidates needed for a given dimensional simulation. - - Parameters: - dim: The dimension of the simulation (i.e., 2 or 3). - )ipc_Qu8mg5v7", - "dim"_a) + "Detect all collision candidates needed for a given dimensional simulation.") .def_readwrite( "can_vertices_collide", &BroadPhase::can_vertices_collide, "Function for determining if two vertices can collide."); diff --git a/python/src/broad_phase/lbvh.cpp b/python/src/broad_phase/lbvh.cpp new file mode 100644 index 000000000..7f2097cfd --- /dev/null +++ b/python/src/broad_phase/lbvh.cpp @@ -0,0 +1,60 @@ +#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_property_readonly( + "left", + [](const LBVH::Node& node) { + if (node.is_inner()) { + return node.left; + } else { + throw py::attribute_error( + "Leaf nodes do not have 'left' child."); + } + }) + .def_property_readonly( + "right", + [](const LBVH::Node& node) { + if (node.is_inner()) { + return node.right; + } else { + throw py::attribute_error( + "Leaf nodes do not have 'right' child."); + } + }) + .def_property_readonly( + "primitive_id", + [](const LBVH::Node& node) { + if (node.is_leaf()) { + return node.primitive_id; + } else { + throw py::attribute_error( + "Inner nodes do not have a 'primitive_id'"); + } + }) + .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) + .def("intersects", &LBVH::Node::intersects) + .def("__str__", [](const LBVH::Node& node) { + return fmt::format( + "LBVH::Node(aabb_min={}, aabb_max={}, {})", + node.aabb_min.transpose(), node.aabb_max.transpose(), + node.is_inner() + ? fmt::format("left={}, right={}", node.left, node.right) + : fmt::format("primitive_id={}", node.primitive_id)); + }); + + 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/python/src/utils/CMakeLists.txt b/python/src/utils/CMakeLists.txt index 3de7d7512..7a17ed16c 100644 --- a/python/src/utils/CMakeLists.txt +++ b/python/src/utils/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES eigen_ext.cpp logger.cpp + profiler.cpp thread_limiter.cpp vertex_to_min_edge.cpp world_bbox_diagonal_length.cpp diff --git a/python/src/utils/bindings.hpp b/python/src/utils/bindings.hpp index 9e3b26741..368962c93 100644 --- a/python/src/utils/bindings.hpp +++ b/python/src/utils/bindings.hpp @@ -4,6 +4,7 @@ void define_eigen_ext(py::module_& m); void define_logger(py::module_& m); +void define_profiler(py::module_& m); void define_thread_limiter(py::module_& m); void define_vertex_to_min_edge(py::module_& m); void define_world_bbox_diagonal_length(py::module_& m); \ No newline at end of file diff --git a/python/src/utils/profiler.cpp b/python/src/utils/profiler.cpp new file mode 100644 index 000000000..7e191027a --- /dev/null +++ b/python/src/utils/profiler.cpp @@ -0,0 +1,41 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_PROFILER +#include +#endif + +using namespace ipc; + +void define_profiler(py::module_& m) +{ +#ifdef IPC_TOOLKIT_WITH_PROFILER + py::class_(m, "Profiler") + .def("clear", &Profiler::clear, "Clear all profiling data") + .def("reset", &Profiler::reset, "Reset the profiler data and scopes") + .def("print", &Profiler::print, "Print the profiler data to the logger") + .def_property_readonly( + "csv", + [](const Profiler& profiler) { + std::ostringstream os; + profiler.write_csv(os); + return os.str(); + }, + "Get the profiler data in CSV format as a string") + .def( + "__str__", + [](const Profiler& profiler) { + std::ostringstream os; + profiler.write_csv(os); + return os.str(); + }) + .def_property_readonly( + "data", py::overload_cast<>(&Profiler::data, py::const_), + "Access the profiling data as a JSON object"); + + m.def( + "profiler", profiler, "Get the global profiler instance", + py::return_value_policy::reference); +#endif +} diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index 983af4cea..e6e3a30c9 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -321,50 +321,56 @@ class TwoStageBarrier : public Barrier { public: TwoStageBarrier() = default; - /// @brief Two-stage activation barrier. - /// - /// \f\[ - /// b(d) = \begin{cases} - /// -\frac{\hat{d}^2}{4} \left(\ln\left(\frac{2d}{\hat{d}}\right) - - /// \tfrac{1}{2}\right) & d < \frac{\hat{d}}{2}\\ - /// \tfrac{1}{2} (\hat{d} - d)^2 & d < \hat{d}\\ - /// 0 & d \ge \hat{d} - /// \end{cases} - /// \f\] - /// - /// @param d The distance. - /// @param dhat Activation distance of the barrier. - /// @return The value of the barrier function at d. + /** + * @brief Two-stage activation barrier. + * + * \f\[ + * b(d) = \begin{cases} + * -\frac{\hat{d}^2}{4} \left(\ln\left(\frac{2d}{\hat{d}}\right) - + * \tfrac{1}{2}\right) & d < \frac{\hat{d}}{2}\\ + * \tfrac{1}{2} (\hat{d} - d)^2 & d < \hat{d}\\ + * 0 & d \ge \hat{d} + * \end{cases} + * \f\] + * + * @param d The distance. + * @param dhat Activation distance of the barrier. + * @return The value of the barrier function at d. + */ double operator()(const double d, const double dhat) const override; - /// @brief Derivative of the barrier function. - /// - /// \f\[ - /// b'(d) = \begin{cases} - // -\frac{\hat{d}}{4d} & d < \frac{\hat{d}}{2}\\ - // d - \hat{d} & d < \hat{d}\\ - // 0 & d \ge \hat{d} - // \end{cases} - /// \f\] - /// - /// @param d The distance. - /// @param dhat Activation distance of the barrier. - /// @return The derivative of the barrier wrt d. + /** + * @brief Derivative of the barrier function. + * + * \f\[ + * b'(d) = \begin{cases} + * -\frac{\hat{d}}{4d} & d < \frac{\hat{d}}{2}\\ + * d - \hat{d} & d < \hat{d}\\ + * 0 & d \ge \hat{d} + * \end{cases} + * \f\] + * + * @param d The distance. + * @param dhat Activation distance of the barrier. + * @return The derivative of the barrier wrt d. + */ double first_derivative(const double d, const double dhat) const override; - /// @brief Second derivative of the barrier function. - /// - /// \f\[ - /// b''(d) = \begin{cases} - /// \frac{\hat{d}}{4d^2} & d < \frac{\hat{d}}{2}\\ - /// 1 & d < \hat{d}\\ - /// 0 & d \ge \hat{d} - /// \end{cases} - /// \f\] - /// - /// @param d The distance. - /// @param dhat Activation distance of the barrier. - /// @return The second derivative of the barrier wrt d. + /** + * @brief Second derivative of the barrier function. + * + * \f\[ + * b''(d) = \begin{cases} + * \frac{\hat{d}}{4d^2} & d < \frac{\hat{d}}{2}\\ + * 1 & d < \hat{d}\\ + * 0 & d \ge \hat{d} + * \end{cases} + * \f\] + * + * @param d The distance. + * @param dhat Activation distance of the barrier. + * @return The second derivative of the barrier wrt d. + */ double second_derivative(const double d, const double dhat) const override; /// @brief Get the units of the barrier function. diff --git a/src/ipc/broad_phase/CMakeLists.txt b/src/ipc/broad_phase/CMakeLists.txt index 49cdb1bc4..c536957f5 100644 --- a/src/ipc/broad_phase/CMakeLists.txt +++ b/src/ipc/broad_phase/CMakeLists.txt @@ -12,6 +12,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/aabb.cpp b/src/ipc/broad_phase/aabb.cpp index 9da17a595..cbfc5f6db 100644 --- a/src/ipc/broad_phase/aabb.cpp +++ b/src/ipc/broad_phase/aabb.cpp @@ -1,5 +1,7 @@ #include "aabb.hpp" +#include + #include #include @@ -7,14 +9,15 @@ namespace ipc { +// NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) AABB::AABB(Eigen::ConstRef _min, Eigen::ConstRef _max) - : min(_min) - , max(_max) + : min(Eigen::Array3d::Zero()) + , max(Eigen::Array3d::Zero()) { - assert(min.size() == max.size()); - assert((min <= max).all()); - // half_extent = (max() - min()) / 2; - // center = min() + half_extent(); + assert(_min.size() == _max.size()); + assert((_min <= _max).all()); + min.head(_min.size()) = _min; + max.head(_max.size()) = _max; } AABB AABB::from_point( @@ -27,20 +30,7 @@ AABB AABB::from_point( bool AABB::intersects(const AABB& other) const { - assert(this->min.size() == other.max.size()); - assert(this->max.size() == other.min.size()); - - // NOTE: This is a faster check (https://gamedev.stackexchange.com/a/587), - // but it is inexact and can result in false negatives. - // return (std::abs(a.center.x() - b.center.x()) - // <= (a.half_extent.x() + b.half_extent.x())) - // && (std::abs(a.center.y() - b.center.y()) - // <= (a.half_extent.y() + b.half_extent.y())) - // && (a.min.size() == 2 - // || std::abs(a.center.z() - b.center.z()) - // <= (a.half_extent.z() + b.half_extent.z())); - - // This on the otherhand, is exact because there is no rounding. + // This is exact because there is no rounding. return (this->min <= other.max).all() && (other.min <= this->max).all(); } @@ -49,27 +39,30 @@ void AABB::conservative_inflation( Eigen::Ref max, const double inflation_radius) { -#pragma STDC FENV_ACCESS ON assert(min.size() == max.size()); assert((min <= max).all()); assert(inflation_radius >= 0); - const int current_round = std::fegetround(); - - std::fesetround(FE_DOWNWARD); - min -= inflation_radius; + // Nudge the bounds outward to ensure conservativity. - std::fesetround(FE_UPWARD); - max += inflation_radius; + min = min.unaryExpr([inflation_radius](double v) { + return std::nextafter( + v - inflation_radius, -std::numeric_limits::infinity()); + }); - std::fesetround(current_round); + max = max.unaryExpr([inflation_radius](double v) { + return std::nextafter( + v + inflation_radius, std::numeric_limits::infinity()); + }); } void build_vertex_boxes( Eigen::ConstRef vertices, - std::vector& vertex_boxes, + AABBs& vertex_boxes, const double inflation_radius) { + IPC_TOOLKIT_PROFILE_BLOCK("build_vertex_boxes"); + vertex_boxes.resize(vertices.rows()); tbb::parallel_for( @@ -86,9 +79,11 @@ void build_vertex_boxes( void build_vertex_boxes( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, - std::vector& vertex_boxes, + AABBs& vertex_boxes, const double inflation_radius) { + IPC_TOOLKIT_PROFILE_BLOCK("build_vertex_boxes"); + vertex_boxes.resize(vertices_t0.rows()); tbb::parallel_for( @@ -103,39 +98,48 @@ void build_vertex_boxes( } void build_edge_boxes( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - std::vector& edge_boxes) + AABBs& edge_boxes) { - edge_boxes.resize(edges.rows()); + IPC_TOOLKIT_PROFILE_BLOCK("build_edge_boxes"); + + if (edge_boxes.size() != edges.rows()) { + IPC_TOOLKIT_PROFILE_BLOCK("resize_edge_boxes"); + edge_boxes.resize(edges.rows()); + } tbb::parallel_for( tbb::blocked_range(0, edges.rows()), [&](const tbb::blocked_range& r) { for (size_t i = r.begin(); i < r.end(); i++) { - edge_boxes[i] = - AABB(vertex_boxes[edges(i, 0)], vertex_boxes[edges(i, 1)]); - edge_boxes[i].vertex_ids = { { edges(i, 0), edges(i, 1), -1 } }; + const int e0 = edges(i, 0), e1 = edges(i, 1); + edge_boxes[i] = AABB(vertex_boxes[e0], vertex_boxes[e1]); + edge_boxes[i].vertex_ids = { { e0, e1, -1 } }; } }); } void build_face_boxes( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef faces, - std::vector& face_boxes) + AABBs& face_boxes) { - face_boxes.resize(faces.rows()); + IPC_TOOLKIT_PROFILE_BLOCK("build_face_boxes"); + + if (face_boxes.size() != faces.rows()) { + IPC_TOOLKIT_PROFILE_BLOCK("resize_face_boxes"); + face_boxes.resize(faces.rows()); + } tbb::parallel_for( tbb::blocked_range(0, faces.rows()), [&](const tbb::blocked_range& r) { for (size_t i = r.begin(); i < r.end(); i++) { - face_boxes[i] = AABB( - vertex_boxes[faces(i, 0)], vertex_boxes[faces(i, 1)], - vertex_boxes[faces(i, 2)]); - face_boxes[i].vertex_ids = { { faces(i, 0), faces(i, 1), - faces(i, 2) } }; + const int f0 = faces(i, 0), f1 = faces(i, 1), f2 = faces(i, 2); + face_boxes[i] = + AABB(vertex_boxes[f0], vertex_boxes[f1], vertex_boxes[f2]); + face_boxes[i].vertex_ids = { { f0, f1, f2 } }; } }); } diff --git a/src/ipc/broad_phase/aabb.hpp b/src/ipc/broad_phase/aabb.hpp index 19ca1ec41..d7d53d3f8 100644 --- a/src/ipc/broad_phase/aabb.hpp +++ b/src/ipc/broad_phase/aabb.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -8,17 +9,21 @@ namespace ipc { /// @brief Axis aligned bounding-box of some type -class AABB { +// NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) +class alignas(64) AABB { public: + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) AABB() = default; AABB(Eigen::ConstRef min, Eigen::ConstRef max); + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) AABB(const AABB& aabb1, const AABB& aabb2) : AABB(aabb1.min.min(aabb2.min), aabb1.max.max(aabb2.max)) { } + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) AABB(const AABB& aabb1, const AABB& aabb2, const AABB& aabb3) : AABB( aabb1.min.min(aabb2.min).min(aabb3.min), @@ -61,20 +66,23 @@ class AABB { public: /// @brief Minimum corner of the AABB. - ArrayMax3d min; + Eigen::Array3d min; /// @brief Maximum corner of the AABB. - ArrayMax3d max; + Eigen::Array3d max; /// @brief Vertex IDs attached to the AABB. - std::array vertex_ids = { { -1, -1, -1 } }; + std::array vertex_ids; }; +/// @brief Vector of AABBs with default initialization allocator. +using AABBs = std::vector>; + /// @brief Build one AABB per vertex position (row of V). /// @param[in] vertices Vertex positions (rowwise). /// @param[out] vertex_boxes Vertex AABBs. /// @param[in] inflation_radius Radius of a sphere around the points which the AABBs enclose. void build_vertex_boxes( Eigen::ConstRef vertices, - std::vector& vertex_boxes, + AABBs& vertex_boxes, const double inflation_radius = 0); /// @brief Build one AABB per vertex position moving linearly from t=0 to t=1. @@ -85,7 +93,7 @@ void build_vertex_boxes( void build_vertex_boxes( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, - std::vector& vertex_boxes, + AABBs& vertex_boxes, const double inflation_radius = 0); /// @brief Build one AABB per edge. @@ -93,17 +101,17 @@ void build_vertex_boxes( /// @param edges Edges (rowwise). /// @param edge_boxes Edge AABBs. void build_edge_boxes( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - std::vector& edge_boxes); + AABBs& edge_boxes); /// @brief Build one AABB per face. /// @param vertex_boxes Vertex AABBs. /// @param faces Faces (rowwise). /// @param face_boxes Face AABBs. void build_face_boxes( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef faces, - std::vector& face_boxes); + AABBs& face_boxes); } // namespace ipc diff --git a/src/ipc/broad_phase/broad_phase.cpp b/src/ipc/broad_phase/broad_phase.cpp index c9605c3f7..114dfbc8f 100644 --- a/src/ipc/broad_phase/broad_phase.cpp +++ b/src/ipc/broad_phase/broad_phase.cpp @@ -2,6 +2,10 @@ #include #include +#include + +#include +#include namespace ipc { @@ -11,7 +15,9 @@ void BroadPhase::build( Eigen::ConstRef faces, const double inflation_radius) { + IPC_TOOLKIT_PROFILE_BLOCK("BroadPhase::build(static)"); clear(); + dim = static_cast(vertices.cols()); build_vertex_boxes(vertices, vertex_boxes, inflation_radius); build(edges, faces); } @@ -23,21 +29,29 @@ void BroadPhase::build( Eigen::ConstRef faces, const double inflation_radius) { + IPC_TOOLKIT_PROFILE_BLOCK("BroadPhase::build(dynamic)"); + assert(vertices_t0.rows() == vertices_t1.rows()); + assert(vertices_t0.cols() == vertices_t1.cols()); clear(); + dim = static_cast(vertices_t0.cols()); build_vertex_boxes( vertices_t0, vertices_t1, vertex_boxes, inflation_radius); build(edges, faces); } void BroadPhase::build( - const std::vector& _vertex_boxes, + const AABBs& _vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) + Eigen::ConstRef faces, + const uint8_t _dim) { + IPC_TOOLKIT_PROFILE_BLOCK("BroadPhase::build(boxes)"); + clear(); assert(&(this->vertex_boxes) != &_vertex_boxes); this->vertex_boxes = _vertex_boxes; + this->dim = _dim; build(edges, faces); } @@ -46,6 +60,7 @@ void BroadPhase::build( Eigen::ConstRef edges, Eigen::ConstRef faces) { + IPC_TOOLKIT_PROFILE_BLOCK("BroadPhase::build(edges_faces)"); assert(!vertex_boxes.empty()); assert(edges.size() == 0 || edges.cols() == 2); assert(faces.size() == 0 || faces.cols() == 3); @@ -58,12 +73,13 @@ void BroadPhase::clear() vertex_boxes.clear(); edge_boxes.clear(); face_boxes.clear(); + dim = 0; // reset dimension } -void BroadPhase::detect_collision_candidates( - int dim, Candidates& candidates) const +void BroadPhase::detect_collision_candidates(Candidates& candidates) const { candidates.clear(); + assert(dim == 2 || dim == 3); if (dim == 2) { // This is not needed for 3D detect_edge_vertex_candidates(candidates.ev_candidates); @@ -74,10 +90,45 @@ void BroadPhase::detect_collision_candidates( } } +void BroadPhase::compute_mesh_aabb( + Eigen::Ref mesh_min, + Eigen::Ref mesh_max) const +{ + IPC_TOOLKIT_PROFILE_BLOCK("BroadPhase::compute_mesh_aabb"); + assert(!vertex_boxes.empty()); + + struct MeshDomain { + Eigen::Array3d min; + Eigen::Array3d max; + }; + + MeshDomain domain { + Eigen::Array3d::Constant(std::numeric_limits::max()), + Eigen::Array3d::Constant(std::numeric_limits::lowest()) + }; + + domain = tbb::parallel_reduce( + tbb::blocked_range(0, vertex_boxes.size()), domain, + [&](const tbb::blocked_range& r, MeshDomain local) { + for (size_t i = r.begin(); i != r.end(); ++i) { + local.min = local.min.min(vertex_boxes[i].min); + local.max = local.max.max(vertex_boxes[i].max); + } + return local; + }, + [](const MeshDomain& a, const MeshDomain& b) { + return MeshDomain { a.min.min(b.min), a.max.max(b.max) }; + }); + + mesh_min = domain.min; + mesh_max = domain.max; +} + // ============================================================================ bool BroadPhase::can_edge_vertex_collide(size_t ei, size_t vi) const { + assert(ei < edge_boxes.size()); const auto& [e0i, e1i, _] = edge_boxes[ei].vertex_ids; return vi != e0i && vi != e1i @@ -86,7 +137,9 @@ bool BroadPhase::can_edge_vertex_collide(size_t ei, size_t vi) const bool BroadPhase::can_edges_collide(size_t eai, size_t ebi) const { + assert(eai < edge_boxes.size()); const auto& [ea0i, ea1i, _] = edge_boxes[eai].vertex_ids; + assert(ebi < edge_boxes.size()); const auto& [eb0i, eb1i, __] = edge_boxes[ebi].vertex_ids; const bool share_endpoint = @@ -100,6 +153,7 @@ bool BroadPhase::can_edges_collide(size_t eai, size_t ebi) const bool BroadPhase::can_face_vertex_collide(size_t fi, size_t vi) const { + assert(fi < face_boxes.size()); const auto& [f0i, f1i, f2i] = face_boxes[fi].vertex_ids; return vi != f0i && vi != f1i && vi != f2i @@ -109,7 +163,9 @@ bool BroadPhase::can_face_vertex_collide(size_t fi, size_t vi) const bool BroadPhase::can_edge_face_collide(size_t ei, size_t fi) const { + assert(ei < edge_boxes.size()); const auto& [e0i, e1i, _] = edge_boxes[ei].vertex_ids; + assert(fi < face_boxes.size()); const auto& [f0i, f1i, f2i] = face_boxes[fi].vertex_ids; const bool share_endpoint = e0i == f0i || e0i == f1i || e0i == f2i @@ -124,7 +180,9 @@ bool BroadPhase::can_edge_face_collide(size_t ei, size_t fi) const bool BroadPhase::can_faces_collide(size_t fai, size_t fbi) const { + assert(fai < face_boxes.size()); const auto& [fa0i, fa1i, fa2i] = face_boxes[fai].vertex_ids; + assert(fbi < face_boxes.size()); const auto& [fb0i, fb1i, fb2i] = face_boxes[fbi].vertex_ids; const bool share_endpoint = fa0i == fb0i || fa0i == fb1i || fa0i == fb2i diff --git a/src/ipc/broad_phase/broad_phase.hpp b/src/ipc/broad_phase/broad_phase.hpp index b3f3f967c..3a13cbadb 100644 --- a/src/ipc/broad_phase/broad_phase.hpp +++ b/src/ipc/broad_phase/broad_phase.hpp @@ -52,18 +52,19 @@ class BroadPhase { /// @param vertex_boxes AABBs for the vertices. /// @param edges Collision mesh edges /// @param faces Collision mesh faces + /// @param dim Dimension of the simulation (2D or 3D) virtual void build( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces); + Eigen::ConstRef faces, + const uint8_t dim); /// @brief Clear any built data. virtual void clear(); /// @brief Detect all collision candidates needed for a given dimensional simulation. - /// @param dim The dimension of the simulation (i.e., 2 or 3). /// @param candidates The detected collision candidates. - void detect_collision_candidates(int dim, Candidates& candidates) const; + void detect_collision_candidates(Candidates& candidates) const; /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions. @@ -108,6 +109,13 @@ class BroadPhase { Eigen::ConstRef edges, Eigen::ConstRef faces); + /// @brief Compute the axis-aligned bounding box (AABB) of the mesh. + /// @param[out] mesh_min Minimum corner of the mesh AABB. + /// @param[out] mesh_max Maximum corner of the mesh AABB. + void compute_mesh_aabb( + Eigen::Ref mesh_min, + Eigen::Ref mesh_max) const; + virtual bool can_edge_vertex_collide(size_t ei, size_t vi) const; virtual bool can_edges_collide(size_t eai, size_t ebi) const; virtual bool can_face_vertex_collide(size_t fi, size_t vi) const; @@ -120,9 +128,15 @@ class BroadPhase { return true; } - std::vector vertex_boxes; - std::vector edge_boxes; - std::vector face_boxes; + /// @brief AABBs for the vertices. + AABBs vertex_boxes; + /// @brief AABBs for the edges. + AABBs edge_boxes; + /// @brief AABBs for the faces. + AABBs face_boxes; + + /// @brief Dimension of the simulation for which the broad phase was built. + uint8_t dim = 0; }; } // namespace ipc diff --git a/src/ipc/broad_phase/brute_force.cpp b/src/ipc/broad_phase/brute_force.cpp index d48e9271e..166380c55 100644 --- a/src/ipc/broad_phase/brute_force.cpp +++ b/src/ipc/broad_phase/brute_force.cpp @@ -14,8 +14,8 @@ namespace ipc { template void BruteForce::detect_candidates( - const std::vector& boxes0, - const std::vector& boxes1, + const AABBs& boxes0, + const AABBs& boxes1, const std::function& can_collide, std::vector& candidates) const { diff --git a/src/ipc/broad_phase/brute_force.hpp b/src/ipc/broad_phase/brute_force.hpp index 56a0e4413..429fb10c0 100644 --- a/src/ipc/broad_phase/brute_force.hpp +++ b/src/ipc/broad_phase/brute_force.hpp @@ -53,8 +53,8 @@ class BruteForce : public BroadPhase { /// @param[out] candidates The candidate collisions. template void detect_candidates( - const std::vector& boxes0, - const std::vector& boxes1, + const AABBs& boxes0, + const AABBs& boxes1, const std::function& can_collide, std::vector& candidates) const; }; diff --git a/src/ipc/broad_phase/bvh.cpp b/src/ipc/broad_phase/bvh.cpp index ca9111bc0..72cb552bf 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 @@ -25,21 +26,25 @@ void BVH::build( Eigen::ConstRef edges, Eigen::ConstRef faces) { + IPC_TOOLKIT_PROFILE_BLOCK("BVH::build"); BroadPhase::build(edges, faces); // Build edge_boxes and face_boxes init_bvh(vertex_boxes, *vertex_bvh); init_bvh(edge_boxes, *edge_bvh); init_bvh(face_boxes, *face_bvh); } -void BVH::init_bvh(const std::vector& boxes, SimpleBVH::BVH& bvh) +void BVH::init_bvh(const AABBs& boxes, SimpleBVH::BVH& bvh) { if (boxes.empty()) { return; } + IPC_TOOLKIT_PROFILE_BLOCK("BVH::init_bvh"); + + // Convert AABBs to the format expected by SimpleBVH 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) } }; + vector_boxes[i] = { { boxes[i].min, boxes[i].max } }; } bvh.init(vector_boxes); @@ -47,6 +52,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(); @@ -54,7 +60,7 @@ void BVH::clear() template void BVH::detect_candidates( - const std::vector& boxes, + const AABBs& boxes, const SimpleBVH::BVH& bvh, const std::function& can_collide, std::vector& candidates) @@ -105,6 +111,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 +125,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 +141,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 +156,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 +171,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 +186,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/bvh.hpp b/src/ipc/broad_phase/bvh.hpp index 4d97d358d..4afa77388 100644 --- a/src/ipc/broad_phase/bvh.hpp +++ b/src/ipc/broad_phase/bvh.hpp @@ -11,7 +11,7 @@ class BVH; namespace ipc { /// @brief Bounding Volume Hierarchy (BVH) broad phase collision detection. -class BVH : public BroadPhase { +class [[deprecated("Use LBVH instead.")]] BVH : public BroadPhase { public: BVH(); ~BVH(); @@ -67,7 +67,7 @@ class BVH : public BroadPhase { /// @brief Initialize a BVH from a set of boxes. /// @param[in] boxes Set of boxes to initialize the BVH with. /// @param[out] bvh The BVH to initialize. - static void init_bvh(const std::vector& boxes, SimpleBVH::BVH& bvh); + static void init_bvh(const AABBs& boxes, SimpleBVH::BVH& bvh); /// @brief Detect candidate collisions between a BVH and a sets of boxes. /// @tparam Candidate Type of candidate collision. @@ -82,7 +82,7 @@ class BVH : public BroadPhase { bool swap_order = false, bool triangular = false> static void detect_candidates( - const std::vector& boxes, + const AABBs& boxes, const SimpleBVH::BVH& bvh, const std::function& can_collide, std::vector& candidates); diff --git a/src/ipc/broad_phase/create_broad_phase.cpp b/src/ipc/broad_phase/create_broad_phase.cpp index 1eecb5ffa..938be137e 100644 --- a/src/ipc/broad_phase/create_broad_phase.cpp +++ b/src/ipc/broad_phase/create_broad_phase.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,8 @@ create_broad_phase(const BroadPhaseMethod& broad_phase_method) return std::make_shared(); case BroadPhaseMethod::BVH: return std::make_shared(); + case BroadPhaseMethod::LBVH: + return std::make_shared(); case BroadPhaseMethod::SWEEP_AND_PRUNE: return std::make_shared(); case BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE: diff --git a/src/ipc/broad_phase/create_broad_phase.hpp b/src/ipc/broad_phase/create_broad_phase.hpp index 066249eb8..e68d34964 100644 --- a/src/ipc/broad_phase/create_broad_phase.hpp +++ b/src/ipc/broad_phase/create_broad_phase.hpp @@ -9,7 +9,8 @@ enum class BroadPhaseMethod : uint8_t { BRUTE_FORCE, HASH_GRID, SPATIAL_HASH, - BVH, + BVH [[deprecated("Use LBVH instead.")]], + LBVH, SWEEP_AND_PRUNE, SWEEP_AND_TINIEST_QUEUE }; diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index a9130bf9d..b4fd0db59 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -23,12 +23,8 @@ void HashGrid::build( { BroadPhase::build(edges, faces); - ArrayMax3d mesh_min = vertex_boxes[0].min; - ArrayMax3d mesh_max = vertex_boxes[0].max; - for (const auto& box : vertex_boxes) { - mesh_min = mesh_min.min(box.min); - mesh_max = mesh_max.max(box.max); - } + Eigen::Array3d mesh_min, mesh_max; + compute_mesh_aabb(mesh_min, mesh_max); const double cell_size = suggest_good_voxel_size(edges.rows() > 0 ? edge_boxes : vertex_boxes); @@ -38,8 +34,8 @@ void HashGrid::build( } void HashGrid::resize( - Eigen::ConstRef domain_min, - Eigen::ConstRef domain_max, + Eigen::ConstRef domain_min, + Eigen::ConstRef domain_max, const double cell_size) { assert(cell_size > 0.0); @@ -53,7 +49,7 @@ void HashGrid::resize( logger().trace( "hash-grid resized with a size of {:d}x{:d}x{:d}", grid_size()[0], - grid_size()[1], grid_size().size() == 3 ? grid_size()[2] : 1); + grid_size()[1], grid_size()[2]); } void HashGrid::insert_boxes() @@ -64,7 +60,7 @@ void HashGrid::insert_boxes() } void HashGrid::insert_boxes( - const std::vector& boxes, std::vector& items) const + const AABBs& boxes, std::vector& items) const { tbb::enumerable_thread_specific> storage; @@ -87,20 +83,22 @@ void HashGrid::insert_boxes( void HashGrid::insert_box( const AABB& aabb, const long id, std::vector& items) const { - ArrayMax3i int_min = ((aabb.min - domain_min()) / cell_size()).cast(); + Eigen::Array3i int_min = + ((aabb.min - domain_min()) / cell_size()).cast(); // We can round down to -1, but not less assert((int_min >= -1).all()); assert((int_min <= grid_size()).all()); int_min = int_min.max(0).min(grid_size() - 1); - ArrayMax3i int_max = ((aabb.max - domain_min()) / cell_size()).cast(); + Eigen::Array3i int_max = + ((aabb.max - domain_min()) / cell_size()).cast(); assert((int_max >= -1).all()); assert((int_max <= grid_size()).all()); int_max = int_max.max(0).min(grid_size() - 1); assert((int_min <= int_max).all()); - int min_z = int_min.size() == 3 ? int_min.z() : 0; - int max_z = int_max.size() == 3 ? int_max.z() : 0; + const int min_z = dim == 3 ? int_min.z() : 0; + const int max_z = dim == 3 ? int_max.z() : 0; for (int x = int_min.x(); x <= int_max.x(); ++x) { for (int y = int_min.y(); y <= int_max.y(); ++y) { for (int z = min_z; z <= max_z; ++z) { @@ -114,8 +112,8 @@ template void HashGrid::detect_candidates( const std::vector& items0, const std::vector& items1, - const std::vector& boxes0, - const std::vector& boxes1, + const AABBs& boxes0, + const AABBs& boxes1, const std::function& can_collide, std::vector& candidates) const { @@ -221,7 +219,7 @@ void HashGrid::detect_candidates( template void HashGrid::detect_candidates( const std::vector& items, - const std::vector& boxes, + const AABBs& boxes, const std::function& can_collide, std::vector& candidates) const { diff --git a/src/ipc/broad_phase/hash_grid.hpp b/src/ipc/broad_phase/hash_grid.hpp index f189efe9a..431ad94b7 100644 --- a/src/ipc/broad_phase/hash_grid.hpp +++ b/src/ipc/broad_phase/hash_grid.hpp @@ -74,9 +74,10 @@ class HashGrid : public BroadPhase { std::vector& candidates) const override; double cell_size() const { return m_cell_size; } - const ArrayMax3i& grid_size() const { return m_grid_size; } - const ArrayMax3d& domain_min() const { return m_domain_min; } - const ArrayMax3d& domain_max() const { return m_domain_max; } + // TODO: Update this + const Eigen::Array3i& grid_size() const { return m_grid_size; } + const Eigen::Array3d& domain_min() const { return m_domain_min; } + const Eigen::Array3d& domain_max() const { return m_domain_max; } protected: /// @brief Build the broad phase for collision detection. @@ -88,14 +89,13 @@ class HashGrid : public BroadPhase { Eigen::ConstRef faces) override; void resize( - Eigen::ConstRef domain_min, - Eigen::ConstRef domain_max, + Eigen::ConstRef domain_min, + Eigen::ConstRef domain_max, double cell_size); void insert_boxes(); - void insert_boxes( - const std::vector& boxes, std::vector& items) const; + void insert_boxes(const AABBs& boxes, std::vector& items) const; /// @brief Add an AABB of the extents to the hash grid. void insert_box( @@ -124,8 +124,8 @@ class HashGrid : public BroadPhase { void detect_candidates( const std::vector& items0, const std::vector& items1, - const std::vector& boxes0, - const std::vector& boxes1, + const AABBs& boxes0, + const AABBs& boxes1, const std::function& can_collide, std::vector& candidates) const; @@ -138,15 +138,15 @@ class HashGrid : public BroadPhase { template void detect_candidates( const std::vector& items, - const std::vector& boxes, + const AABBs& boxes, const std::function& can_collide, std::vector& candidates) const; protected: double m_cell_size = -1; - ArrayMax3i m_grid_size; - ArrayMax3d m_domain_min; - ArrayMax3d m_domain_max; + Eigen::Array3i m_grid_size; + Eigen::Array3d m_domain_min; + Eigen::Array3d m_domain_max; std::vector vertex_items; std::vector edge_items; diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp new file mode 100644 index 000000000..0383ac9c0 --- /dev/null +++ b/src/ipc/broad_phase/lbvh.cpp @@ -0,0 +1,802 @@ +#include "lbvh.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#ifdef __APPLE__ +// We utilize SIMD registers to compare 1 Node against 4 Queries simultaneously. +#include +#endif + +using namespace std::placeholders; + +namespace ipc { + +namespace { + // Helper to safely convert double AABB to float AABB + inline void assign_inflated_aabb(const AABB& box, LBVH::Node& node) + { + // Round Min down + node.aabb_min = box.min.unaryExpr([](double val) { + return std::nextafter( + float(val), -std::numeric_limits::infinity()); + }); + // Round Max up + node.aabb_max = box.max.unaryExpr([](double val) { + return std::nextafter( + float(val), std::numeric_limits::infinity()); + }); + } +} // namespace + +LBVH::LBVH() : BroadPhase() +{ + static_assert( + sizeof(LBVH::Node) == 32, + "LBVH::Node size must be 32 bytes to fit 2 Nodes in a cache line"); +} + +LBVH::~LBVH() = default; + +void LBVH::build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::build"); + + BroadPhase::build(edges, faces); // Build edge_boxes and face_boxes + + if (vertex_boxes.empty()) { + return; + } + + assert(dim == 2 || dim == 3); + + 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); + + // Copy edge and face vertex ids for access during traversal + { + IPC_TOOLKIT_PROFILE_BLOCK("copy_vertex_ids"); + + edge_vertex_ids.resize(edges.rows()); + tbb::parallel_for( + tbb::blocked_range(0, edges.rows()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); ++i) { + edge_vertex_ids[i][0] = static_cast(edges(i, 0)); + edge_vertex_ids[i][1] = static_cast(edges(i, 1)); + } + }); + + face_vertex_ids.resize(faces.rows()); + tbb::parallel_for( + tbb::blocked_range(0, faces.rows()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); ++i) { + face_vertex_ids[i][0] = static_cast(faces(i, 0)); + face_vertex_ids[i][1] = static_cast(faces(i, 1)); + face_vertex_ids[i][2] = static_cast(faces(i, 2)); + } + }); + } + + // Clear parent data to save memory. + // These are redundant after building the BVHs. + vertex_boxes.clear(); + edge_boxes.clear(); + face_boxes.clear(); +} + +namespace { + int delta( + const LBVH::MortonCodeElements& sorted_morton_codes, + int i, + uint64_t code_i, + int j) + { + if (j < 0 || j >= sorted_morton_codes.size()) { + 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 LBVH::MortonCodeElements& 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 LBVH::MortonCodeElements& 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 AABBs& boxes, Nodes& lbvh) const +{ + if (boxes.empty()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::init_bvh"); + + if (lbvh.size() != 2 * boxes.size() - 1) { + IPC_TOOLKIT_PROFILE_BLOCK("resize_bvh"); + lbvh.resize(2 * boxes.size() - 1); + } + + LBVH::MortonCodeElements morton_codes(boxes.size()); + { + IPC_TOOLKIT_PROFILE_BLOCK("compute_morton_codes"); + + const Eigen::Array3d 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 Eigen::Array3d center = 0.5 * (box.min + box.max); + const Eigen::Array3d mapped_center = + (center - mesh_aabb.min) / mesh_width; + + if (dim == 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; + }); + } + + assert(boxes.size() <= std::numeric_limits::max()); + const int LEAF_OFFSET = int(boxes.size()) - 1; + + LBVH::ConstructionInfos construction_infos(lbvh.size()); + { + 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]; + + Node leaf_node; // Create leaf node + assign_inflated_aabb(box, leaf_node); + leaf_node.primitive_id = morton_codes[i].box_id; + leaf_node.is_inner_marker = 0; + lbvh[LEAF_OFFSET + i] = leaf_node; // Store leaf + } + + if (i < LEAF_OFFSET) { + // 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 + lbvh[i].left = child_a; + lbvh[i].right = child_b; + construction_infos[child_a].parent = int(i); + construction_infos[child_b].parent = int(i); + construction_infos[child_a].visitation_count.store( + 0, std::memory_order_relaxed); + construction_infos[child_b].visitation_count.store( + 0, std::memory_order_relaxed); + } + + // node 0 is the root and has no parent to set these values + if (i == 0) { + construction_infos[0].parent = 0; + construction_infos[0].visitation_count.store( + 0, std::memory_order_relaxed); + } + } + }); + } + + { + 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 + assert(lbvh[node_idx].is_inner()); + const Node& child_b = lbvh[lbvh[node_idx].right]; + const Node& child_a = lbvh[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(); + // Clear BVH nodes + vertex_bvh.clear(); + edge_bvh.clear(); + face_bvh.clear(); + + // Clear vertex IDs + edge_vertex_ids.clear(); + face_vertex_ids.clear(); +} + +namespace { + template + inline void attempt_add_candidate( + const LBVH::Node& query, + const LBVH::Node& node, + const std::function& can_collide, + std::vector& candidates) + { + 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 LBVH::Nodes& 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]; + + // Check left and right are valid pointers + assert(node.is_inner()); + +#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) { + assert(stack_ptr > 0); + 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 + assert(stack_ptr < MAX_STACK_SIZE); + stack[stack_ptr++] = node.right; + } + } + } while (node_idx != LBVH::Node::INVALID_POINTER); // Same as root + } + +#ifdef __APPLE__ + // SIMD Traversal + // Traverses 4 queries simultaneously using SIMD. + template + void traverse_lbvh_simd( + const LBVH::Node* queries, + const size_t n_queries, + const LBVH::Nodes& lbvh, + const std::function& can_collide, + std::vector& candidates) + { + assert(n_queries >= 1 && n_queries <= 4); + // Load 4 queries into single registers (Structure of Arrays) + auto make_simd = [&](auto F) -> simd_float4 { + return simd_float4 { + F(0), + n_queries > 1 ? F(1) : 0.0f, + n_queries > 2 ? F(2) : 0.0f, + n_queries > 3 ? F(3) : 0.0f, + }; + }; + + const simd_float4 q_min_x = + make_simd([&](int k) { return queries[k].aabb_min.x(); }); + const simd_float4 q_min_y = + make_simd([&](int k) { return queries[k].aabb_min.y(); }); + const simd_float4 q_min_z = + make_simd([&](int k) { return queries[k].aabb_min.z(); }); + const simd_float4 q_max_x = + make_simd([&](int k) { return queries[k].aabb_max.x(); }); + const simd_float4 q_max_y = + make_simd([&](int k) { return queries[k].aabb_max.y(); }); + const simd_float4 q_max_z = + make_simd([&](int k) { return queries[k].aabb_max.z(); }); + + // 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]; + + // Check left and right are valid pointers + assert(node.is_inner()); + +#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]; + + // 1. Intersect 4 queries at once + // (child_l.min <= query.max) && (query.min <= child_l.max) + const simd_int4 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) + & (q_min_x <= child_l.aabb_max.x()) + & (q_min_y <= child_l.aabb_max.y()) + & (q_min_z <= child_l.aabb_max.z()); + + // 2. Intersect 4 queries at once + // (child_r.min <= query.max) && (query.min <= child_r.max) + const simd_int4 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) + & (q_min_x <= child_r.aabb_max.x()) + & (q_min_y <= child_r.aabb_max.y()) + & (q_min_z <= child_r.aabb_max.z()); + + const bool any_intersects_l = simd_any(intersects_l); + const bool any_intersects_r = simd_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 (intersects_l[k]) { + attempt_add_candidate< + Candidate, swap_order, triangular>( + queries[k], child_l, can_collide, candidates); + } + } + } + if (any_intersects_r && child_r.is_leaf()) { + for (int k = 0; k < n_queries; ++k) { + if (intersects_r[k]) { + attempt_add_candidate< + Candidate, swap_order, triangular>( + queries[k], child_r, can_collide, candidates); + } + } + } + + // Query overlaps an internal node => traverse. + bool traverse_l = (any_intersects_l && !child_l.is_leaf()); + bool traverse_r = (any_intersects_r && !child_r.is_leaf()); + + if (!traverse_l && !traverse_r) { + assert(stack_ptr > 0); + 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 + assert(stack_ptr < MAX_STACK_SIZE); + stack[stack_ptr++] = node.right; + } + } + } while (node_idx != LBVH::Node::INVALID_POINTER); // Same as root + } +#endif + + template < + typename Candidate, + bool swap_order, + bool triangular, + bool use_simd = true> + void independent_traversal( + const LBVH::Nodes& source, + const LBVH::Nodes& target, + const std::function& can_collide, + tbb::enumerable_thread_specific>& storage) + { +#ifdef __APPLE__ // Only support SIMD on Apple platforms for now + constexpr size_t SIMD_SIZE = use_simd ? 4 : 1; + constexpr size_t GRAIN_SIZE = use_simd ? 16 : 1; +#else + constexpr size_t SIMD_SIZE = 1; + constexpr size_t GRAIN_SIZE = 1; +#endif + + // 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; + + const size_t n_tasks = + n_source_leaves / SIMD_SIZE + (n_source_leaves % SIMD_SIZE != 0); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), n_tasks, GRAIN_SIZE), + [&](const tbb::blocked_range& r) { + auto& local_candidates = storage.local(); + const size_t actual_end = // Handle tail case + std::min(SIMD_SIZE * r.end(), n_source_leaves); + for (size_t i = r.begin(); i < r.end(); ++i) { + const size_t idx = SIMD_SIZE * i; +#ifdef __APPLE__ + if constexpr (use_simd) { + assert(actual_end - idx >= 1); + traverse_lbvh_simd( + &source[source_leaf_offset + idx], + std::min(SIMD_SIZE, actual_end - idx), target, + can_collide, local_candidates); + } else { +#endif + traverse_lbvh( + source[source_leaf_offset + idx], target, + can_collide, local_candidates); +#ifdef __APPLE__ + } +#endif + } + }); + } +} // namespace + +template +void LBVH::detect_candidates( + const Nodes& source, + const Nodes& target, + const std::function& can_collide, + std::vector& candidates) +{ + if (source.empty() || target.empty()) { + return; + } + + tbb::enumerable_thread_specific> storage; + + independent_traversal( + source, target, can_collide, storage); + + merge_thread_local_vectors(storage, candidates); +} + +void LBVH::detect_vertex_vertex_candidates( + std::vector& candidates) const +{ + if (!has_vertices()) { + 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 (!has_edges() || !has_vertices()) { + 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 (!has_edges()) { + 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 (!has_faces() || !has_vertices()) { + 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 (!has_edges() || !has_faces()) { + 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 (!has_faces()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_face_face_candidates"); + detect_candidates( + face_bvh, std::bind(&LBVH::can_faces_collide, this, _1, _2), + candidates); +} + +// ============================================================================ + +bool LBVH::can_edge_vertex_collide(size_t ei, size_t vi) const +{ + assert(ei < edge_vertex_ids.size()); + const auto& [e0i, e1i] = edge_vertex_ids[ei]; + + return vi != e0i && vi != e1i + && (can_vertices_collide(vi, e0i) || can_vertices_collide(vi, e1i)); +} + +bool LBVH::can_edges_collide(size_t eai, size_t ebi) const +{ + assert(eai < edge_vertex_ids.size()); + const auto& [ea0i, ea1i] = edge_vertex_ids[eai]; + assert(ebi < edge_vertex_ids.size()); + const auto& [eb0i, eb1i] = edge_vertex_ids[ebi]; + + const bool share_endpoint = + ea0i == eb0i || ea0i == eb1i || ea1i == eb0i || ea1i == eb1i; + + return !share_endpoint + && (can_vertices_collide(ea0i, eb0i) || can_vertices_collide(ea0i, eb1i) + || can_vertices_collide(ea1i, eb0i) + || can_vertices_collide(ea1i, eb1i)); +} + +bool LBVH::can_face_vertex_collide(size_t fi, size_t vi) const +{ + assert(fi < face_vertex_ids.size()); + const auto& [f0i, f1i, f2i] = face_vertex_ids[fi]; + + return vi != f0i && vi != f1i && vi != f2i + && (can_vertices_collide(vi, f0i) || can_vertices_collide(vi, f1i) + || can_vertices_collide(vi, f2i)); +} + +bool LBVH::can_edge_face_collide(size_t ei, size_t fi) const +{ + assert(ei < edge_vertex_ids.size()); + const auto& [e0i, e1i] = edge_vertex_ids[ei]; + assert(fi < face_vertex_ids.size()); + const auto& [f0i, f1i, f2i] = face_vertex_ids[fi]; + + const bool share_endpoint = e0i == f0i || e0i == f1i || e0i == f2i + || e1i == f0i || e1i == f1i || e1i == f2i; + + return !share_endpoint + && (can_vertices_collide(e0i, f0i) || can_vertices_collide(e0i, f1i) + || can_vertices_collide(e0i, f2i) || can_vertices_collide(e1i, f0i) + || can_vertices_collide(e1i, f1i) + || can_vertices_collide(e1i, f2i)); +} + +bool LBVH::can_faces_collide(size_t fai, size_t fbi) const +{ + assert(fai < face_vertex_ids.size()); + const auto& [fa0i, fa1i, fa2i] = face_vertex_ids[fai]; + assert(fbi < face_vertex_ids.size()); + const auto& [fb0i, fb1i, fb2i] = face_vertex_ids[fbi]; + + const bool share_endpoint = fa0i == fb0i || fa0i == fb1i || fa0i == fb2i + || fa1i == fb0i || fa1i == fb1i || fa1i == fb2i || fa2i == fb0i + || fa2i == fb1i || fa2i == fb2i; + + return !share_endpoint + && (can_vertices_collide(fa0i, fb0i) || can_vertices_collide(fa0i, fb1i) + || can_vertices_collide(fa0i, fb2i) + || can_vertices_collide(fa1i, fb0i) + || can_vertices_collide(fa1i, fb1i) + || can_vertices_collide(fa1i, fb2i) + || can_vertices_collide(fa2i, fb0i) + || can_vertices_collide(fa2i, fb1i) + || can_vertices_collide(fa2i, fb2i)); +} + +} // 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..97c27f7fb --- /dev/null +++ b/src/ipc/broad_phase/lbvh.hpp @@ -0,0 +1,226 @@ +#pragma once + +#include +#include + +#include + +namespace ipc { + +/// @brief Linear Bounding Volume Hierarchy (LBVH) broad phase collision detection. +// NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) +class LBVH : public BroadPhase { +public: + static constexpr index_t INVALID_ID = 0xFFFFFFFF; + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) + struct alignas(32) Node { + static constexpr int32_t INVALID_POINTER = 0x0; // do not change + + /// @brief The min corner of the AABB + Eigen::Array3f aabb_min; + /// @brief The max corner of the AABB + Eigen::Array3f aabb_max; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" + + // Union to overlap Leaf data and Internal Node data. + // This compresses the Node size to 32 bytes (2 per cache line), + // reducing cache misses during traversal. + union { + struct { + /// @brief Pointer to the left child or INVALID_POINTER in case of leaf + int32_t left; + /// @brief Pointer to the right child or INVALID_POINTER in case of leaf + int32_t right; + }; + + struct { + /// @brief The primitive id (INVALID_ID <=> inner node) + /// @note We use this to distinguish leaves from internal nodes to safely access the union. + int32_t primitive_id; + + /// @brief Marker to indicate this is an inner node + /// If is_inner == 0 then right = 0 which is INVALID_POINTER + /// If is_inner != 0 then right = actual right pointer + int32_t is_inner_marker; + }; + }; + +#pragma GCC diagnostic pop + + /// @brief Check if this node is an inner node. + bool is_inner() const { return is_inner_marker; } + + /// @brief Check if this node is a leaf node. + bool is_leaf() const { return !is_inner(); } + + /// @brief Check if this node is valid. + bool is_valid() const + { + return is_inner() + ? (left != INVALID_POINTER && right != INVALID_POINTER) + : primitive_id >= 0; + } + + /// @brief Check if this node's AABB intersects with another node's AABB. + 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 + }; + + // Vectors with custom allocator to avoid initialization overhead + using Nodes = std::vector>; + using MortonCodeElements = + std::vector>; + +private: + struct ConstructionInfo { + /// @brief Parent to the parent + int parent; + /// @brief Number of threads that arrived + std::atomic visitation_count; + }; + + using ConstructionInfos = + std::vector>; + +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 Nodes& vertex_nodes() const { return vertex_bvh; } + const Nodes& edge_nodes() const { return edge_bvh; } + const Nodes& 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 AABBs& boxes, Nodes& 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 Nodes& source, + const Nodes& target, + const std::function& can_collide, + std::vector& candidates); + + template + static void detect_candidates( + const Nodes& source_and_target, + const std::function& can_collide, + std::vector& candidates) + { + detect_candidates( + source_and_target, source_and_target, can_collide, candidates); + } + + // ------------------------------------------------------------------------- + // BroadPhase::*_boxes are cleared, so we have to override these methods to + // not depend on them. + bool has_vertices() const { return !vertex_bvh.empty(); } + bool has_edges() const { return !edge_bvh.empty(); } + bool has_faces() const { return !face_bvh.empty(); } + bool can_edge_vertex_collide(size_t ei, size_t vi) const override; + bool can_edges_collide(size_t eai, size_t ebi) const override; + bool can_face_vertex_collide(size_t fi, size_t vi) const override; + bool can_edge_face_collide(size_t ei, size_t fi) const override; + bool can_faces_collide(size_t fai, size_t fbi) const override; + // ------------------------------------------------------------------------- + + /// @brief BVH containing the vertices. + Nodes vertex_bvh; + /// @brief BVH containing the edges. + Nodes edge_bvh; + /// @brief BVH containing the faces. + Nodes face_bvh; + +private: + // Vectors with custom allocator to avoid initialization overhead + using EdgeIndices = std::vector< + std::array, + DefaultInitAllocator>>; + using FaceIndices = std::vector< + std::array, + DefaultInitAllocator>>; + + /// @brief Edge vertices in the original mesh order. + EdgeIndices edge_vertex_ids; + /// @brief Face vertices in the original mesh order. + FaceIndices face_vertex_ids; + + /// @brief The axis-aligned bounding box of the entire mesh. + struct { + Eigen::Array3d min; + Eigen::Array3d max; + } mesh_aabb; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/broad_phase/spatial_hash.cpp b/src/ipc/broad_phase/spatial_hash.cpp index 861421519..0f4ee5cd6 100644 --- a/src/ipc/broad_phase/spatial_hash.cpp +++ b/src/ipc/broad_phase/spatial_hash.cpp @@ -39,7 +39,7 @@ namespace { void fill_primitive_to_voxels( Eigen::ConstRef min_voxel, Eigen::ConstRef max_voxel, - Eigen::ConstRef voxel_count, + Eigen::ConstRef voxel_count, const int voxel_count_0x1, std::vector& primitive_to_voxels) { @@ -70,6 +70,7 @@ void SpatialHash::build( double voxel_size) { clear(); + dim = static_cast(vertices.cols()); build_vertex_boxes(vertices, vertex_boxes, inflation_radius); build(edges, faces, voxel_size); } @@ -83,20 +84,23 @@ void SpatialHash::build( double voxel_size) { clear(); + dim = static_cast(vertices_t0.cols()); build_vertex_boxes( vertices_t0, vertices_t1, vertex_boxes, inflation_radius); build(edges, faces, voxel_size); } void SpatialHash::build( - const std::vector& _vertex_boxes, + const AABBs& _vertex_boxes, Eigen::ConstRef edges, Eigen::ConstRef faces, + const uint8_t _dim, double voxel_size) { // WARNING: Clear will reset vertex_boxes if this assert is triggered assert(&(this->vertex_boxes) != &_vertex_boxes); clear(); + this->dim = _dim; this->vertex_boxes = _vertex_boxes; build(edges, faces, voxel_size); } @@ -108,7 +112,7 @@ void SpatialHash::build( { const size_t num_vertices = vertex_boxes.size(); assert(num_vertices > 0); - dim = vertex_boxes[0].min.size(); + assert(dim == 2 || dim == 3); BroadPhase::build(edges, faces); @@ -117,17 +121,12 @@ void SpatialHash::build( edges.rows() > 0 ? edge_boxes : vertex_boxes); } - left_bottom_corner = vertex_boxes[0].min; - right_top_corner = vertex_boxes[0].max; - for (const auto& box : vertex_boxes) { - left_bottom_corner = left_bottom_corner.min(box.min); - right_top_corner = right_top_corner.max(box.max); - } + compute_mesh_aabb(left_bottom_corner, right_top_corner); one_div_voxel_size = 1.0 / voxel_size; - const ArrayMax3d range = right_top_corner - left_bottom_corner; - voxel_count = (range * one_div_voxel_size).ceil().template cast(); + const Eigen::Array3d range = right_top_corner - left_bottom_corner; + voxel_count = (range * one_div_voxel_size).ceil().cast(); if (voxel_count.minCoeff() <= 0) { // cast overflow due to huge search direction one_div_voxel_size = 1.0 / (range.maxCoeff() * 1.01); @@ -138,14 +137,12 @@ void SpatialHash::build( // ------------------------------------------------------------------------ // precompute vertex min and max voxel axis indices - std::vector vertex_min_voxel_axis_index( - num_vertices, Eigen::Array3i::Zero()); - std::vector vertex_max_voxel_axis_index( - num_vertices, Eigen::Array3i::Zero()); + std::vector vertex_min_voxel_axis_index(num_vertices); + std::vector vertex_max_voxel_axis_index(num_vertices); tbb::parallel_for(size_t(0), num_vertices, [&](size_t vi) { - vertex_min_voxel_axis_index[vi].head(dim) = + vertex_min_voxel_axis_index[vi] = locate_voxel_axis_index(vertex_boxes[vi].min); - vertex_max_voxel_axis_index[vi].head(dim) = + vertex_max_voxel_axis_index[vi] = locate_voxel_axis_index(vertex_boxes[vi].max); }); @@ -198,8 +195,8 @@ namespace { template void detect_candidates( - const std::vector& boxesA, - const std::vector& boxesB, + const AABBs& boxesA, + const AABBs& boxesB, const std::function&)>& query_A_for_Bs, const std::function& can_collide, std::vector& candidates) @@ -243,7 +240,7 @@ namespace { template void detect_candidates( - const std::vector& boxesA, + const AABBs& boxesA, const std::function&)>& query_A_for_As, const std::function& can_collide, std::vector& candidates) @@ -350,37 +347,36 @@ void SpatialHash::detect_face_face_candidates( // ============================================================================ -int SpatialHash::locate_voxel_index(Eigen::ConstRef p) const +int SpatialHash::locate_voxel_index(Eigen::ConstRef p) const { return voxel_axis_index_to_voxel_index(locate_voxel_axis_index(p)); } -ArrayMax3i -SpatialHash::locate_voxel_axis_index(Eigen::ConstRef p) const +Eigen::Array3i +SpatialHash::locate_voxel_axis_index(Eigen::ConstRef p) const { return ((p.array() - left_bottom_corner) * one_div_voxel_size) .floor() - .template cast(); + .cast(); } void SpatialHash::locate_box_voxel_axis_index( - ArrayMax3d min_corner, // input but will be modified - ArrayMax3d max_corner, // input but will be modified - Eigen::Ref min_index, // output - Eigen::Ref max_index, // output + Eigen::Array3d min_corner, // input but will be modified + Eigen::Array3d max_corner, // input but will be modified + Eigen::Ref min_index, // output + Eigen::Ref max_index, // output const double inflation_radius) const { AABB::conservative_inflation(min_corner, max_corner, inflation_radius); - min_index = locate_voxel_axis_index(min_corner).max(ArrayMax3i::Zero(dim)); + min_index = locate_voxel_axis_index(min_corner).max(0); max_index = locate_voxel_axis_index(max_corner).min(voxel_count - 1); } int SpatialHash::voxel_axis_index_to_voxel_index( - Eigen::ConstRef voxel_axis_index) const + Eigen::ConstRef voxel_axis_index) const { return voxel_axis_index_to_voxel_index( - voxel_axis_index[0], voxel_axis_index[1], - voxel_axis_index.size() >= 3 ? voxel_axis_index[2] : 0); + voxel_axis_index[0], voxel_axis_index[1], voxel_axis_index[2]); } int SpatialHash::voxel_axis_index_to_voxel_index(int ix, int iy, int iz) const diff --git a/src/ipc/broad_phase/spatial_hash.hpp b/src/ipc/broad_phase/spatial_hash.hpp index a925db084..9e52dfa94 100644 --- a/src/ipc/broad_phase/spatial_hash.hpp +++ b/src/ipc/broad_phase/spatial_hash.hpp @@ -62,11 +62,12 @@ class SpatialHash : public BroadPhase { /// @param edges Collision mesh edges /// @param faces Collision mesh faces void build( - const std::vector& vertex_boxes, + const AABBs& _vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) override + Eigen::ConstRef faces, + const uint8_t _dim) override { - build(vertex_boxes, edges, faces, /*voxel_size=*/-1); + build(_vertex_boxes, edges, faces, _dim, /*voxel_size=*/-1); } // ------------------------------------------------------------------------ @@ -108,9 +109,10 @@ class SpatialHash : public BroadPhase { /// @param faces Collision mesh faces /// @param voxel_size Size of the voxels used in the spatial hash. void build( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, Eigen::ConstRef faces, + const uint8_t dim, double voxel_size); /// @brief Clear any built data. @@ -155,32 +157,33 @@ class SpatialHash : public BroadPhase { Eigen::ConstRef faces, double voxel_size); - int locate_voxel_index(Eigen::ConstRef p) const; + int locate_voxel_index(Eigen::ConstRef p) const; - ArrayMax3i locate_voxel_axis_index(Eigen::ConstRef p) const; + Eigen::Array3i + locate_voxel_axis_index(Eigen::ConstRef p) const; void locate_box_voxel_axis_index( - ArrayMax3d min_corner, - ArrayMax3d max_corner, - Eigen::Ref min_index, - Eigen::Ref max_index, + Eigen::Array3d min_corner, + Eigen::Array3d max_corner, + Eigen::Ref min_index, + Eigen::Ref max_index, const double inflation_radius = 0) const; int voxel_axis_index_to_voxel_index( - Eigen::ConstRef voxel_axis_index) const; + Eigen::ConstRef voxel_axis_index) const; int voxel_axis_index_to_voxel_index(int ix, int iy, int iz) const; // --- Data members ------------------------------------------------------- /// @brief The left bottom corner of the world bounding box. - ArrayMax3d left_bottom_corner; + Eigen::Array3d left_bottom_corner; /// @brief The right top corner of the world bounding box. - ArrayMax3d right_top_corner; + Eigen::Array3d right_top_corner; /// @brief The number of voxels in each dimension. - ArrayMax3i voxel_count; + Eigen::Array3i voxel_count; /// @note Use the Pimpl idiom to hide unordered_map and unordered_set from the public API. struct Impl; @@ -192,9 +195,6 @@ class SpatialHash : public BroadPhase { /// @brief The number of voxels in the first two dimensions. int voxel_count_0x1 = -1; - - /// @brief The dimension of the space (2D or 3D). - int dim = -1; }; } // namespace ipc diff --git a/src/ipc/broad_phase/sweep_and_prune.cpp b/src/ipc/broad_phase/sweep_and_prune.cpp index d7e186fc4..eeb78cfb0 100644 --- a/src/ipc/broad_phase/sweep_and_prune.cpp +++ b/src/ipc/broad_phase/sweep_and_prune.cpp @@ -30,6 +30,8 @@ void SweepAndPrune::build( clear(); + dim = vertices.cols(); + scalable_ccd::build_vertex_boxes( vertices, boxes->vertices, inflation_radius); scalable_ccd::build_edge_boxes(boxes->vertices, edges, boxes->edges); @@ -48,6 +50,8 @@ void SweepAndPrune::build( clear(); + dim = vertices_t0.cols(); + scalable_ccd::build_vertex_boxes( vertices_t0, vertices_t1, boxes->vertices, inflation_radius); scalable_ccd::build_edge_boxes(boxes->vertices, edges, boxes->edges); @@ -55,27 +59,30 @@ void SweepAndPrune::build( } void SweepAndPrune::build( - const std::vector& vertex_boxes, + const AABBs& p_vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) + Eigen::ConstRef faces, + const uint8_t p_dim) { assert(edges.size() == 0 || edges.cols() == 2); assert(faces.size() == 0 || faces.cols() == 3); clear(); + dim = p_dim; + // Convert from ipc::AABB to scalable_ccd::AABB (additional element_id) - boxes->vertices.resize(vertex_boxes.size()); - for (int i = 0; i < vertex_boxes.size(); ++i) { + boxes->vertices.resize(p_vertex_boxes.size()); + for (int i = 0; i < p_vertex_boxes.size(); ++i) { boxes->vertices[i].min = - vertex_boxes[i].min.cast(); + p_vertex_boxes[i].min.cast(); boxes->vertices[i].max = - vertex_boxes[i].max.cast(); - assert(vertex_boxes[i].vertex_ids[0] >= 0); - assert(vertex_boxes[i].vertex_ids[1] < 0); - assert(vertex_boxes[i].vertex_ids[2] < 0); - boxes->vertices[i].vertex_ids[0] = vertex_boxes[i].vertex_ids[0]; - boxes->vertices[i].vertex_ids[1] = -vertex_boxes[i].vertex_ids[0] - 1; + p_vertex_boxes[i].max.cast(); + assert(p_vertex_boxes[i].vertex_ids[0] >= 0); + assert(p_vertex_boxes[i].vertex_ids[1] < 0); + assert(p_vertex_boxes[i].vertex_ids[2] < 0); + boxes->vertices[i].vertex_ids[0] = p_vertex_boxes[i].vertex_ids[0]; + boxes->vertices[i].vertex_ids[1] = -p_vertex_boxes[i].vertex_ids[0] - 1; boxes->vertices[i].vertex_ids[2] = boxes->vertices[i].vertex_ids[1]; boxes->vertices[i].element_id = i; } diff --git a/src/ipc/broad_phase/sweep_and_prune.hpp b/src/ipc/broad_phase/sweep_and_prune.hpp index f996b7848..37f0abb73 100644 --- a/src/ipc/broad_phase/sweep_and_prune.hpp +++ b/src/ipc/broad_phase/sweep_and_prune.hpp @@ -14,6 +14,8 @@ class SweepAndPrune : public BroadPhase { /// @return The name of the broad phase method. std::string name() const override { return "SweepAndPrune"; } + using BroadPhase::build; + /// @brief Build the broad phase for static collision detection. /// @param vertices Vertex positions /// @param edges Collision mesh edges @@ -43,9 +45,10 @@ class SweepAndPrune : public BroadPhase { /// @param edges Collision mesh edges /// @param faces Collision mesh faces void build( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) override; + Eigen::ConstRef faces, + const uint8_t dim) override; /// @brief Clear any built data. void clear() override; diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.cu b/src/ipc/broad_phase/sweep_and_tiniest_queue.cu index a2cf67bfe..bf68f3d93 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.cu +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.cu @@ -33,6 +33,8 @@ void SweepAndTiniestQueue::build( clear(); + dim = _vertices.cols(); + // Make sure the vertices are 3D const Eigen::MatrixXd vertices = to_X3d(_vertices); @@ -56,6 +58,8 @@ void SweepAndTiniestQueue::build( clear(); + dim = _vertices_t0.cols(); + // Mutable copies of the vertices const Eigen::MatrixXd vertices_t0 = to_X3d(_vertices_t0); const Eigen::MatrixXd vertices_t1 = to_X3d(_vertices_t1); @@ -67,27 +71,28 @@ void SweepAndTiniestQueue::build( } void SweepAndTiniestQueue::build( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) + Eigen::ConstRef faces, + const uint8_t _dim) { assert(edges.size() == 0 || edges.cols() == 2); assert(faces.size() == 0 || faces.cols() == 3); clear(); + dim = _dim; + // Convert from ipc::AABB to scalable_ccd::cuda::AABB boxes->vertices.resize(vertex_boxes.size()); for (int i = 0; i < vertex_boxes.size(); ++i) { boxes->vertices[i].min.x = vertex_boxes[i].min.x(); boxes->vertices[i].min.y = vertex_boxes[i].min.y(); - boxes->vertices[i].min.z = - vertex_boxes[i].min.size() > 2 ? vertex_boxes[i].min.z() : 0; + boxes->vertices[i].min.z = vertex_boxes[i].min.z(); boxes->vertices[i].max.x = vertex_boxes[i].max.x(); boxes->vertices[i].max.y = vertex_boxes[i].max.y(); - boxes->vertices[i].max.z = - vertex_boxes[i].max.size() > 2 ? vertex_boxes[i].max.z() : 0; + boxes->vertices[i].max.z = vertex_boxes[i].max.z(); // If vertex id == -1 it means this slot is not used. // But Scalable CCD does not have this kind of special value so we map diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp index 8f249b812..b137d2609 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp @@ -18,6 +18,8 @@ class SweepAndTiniestQueue : public BroadPhase { /// @return The name of the broad phase method. std::string name() const override { return "SweepAndTiniestQueue"; } + using BroadPhase::build; + /// @brief Build the broad phase for static collision detection. /// @param vertices Vertex positions /// @param edges Collision mesh edges @@ -46,10 +48,12 @@ class SweepAndTiniestQueue : public BroadPhase { /// @param vertex_boxes Precomputed vertex AABBs /// @param edges Collision mesh edges /// @param faces Collision mesh faces + /// @param dim Dimension of the simulation (2D or 3D) void build( - const std::vector& vertex_boxes, + const AABBs& vertex_boxes, Eigen::ConstRef edges, - Eigen::ConstRef faces) override; + Eigen::ConstRef faces, + const uint8_t dim) override; /// @brief Clear any built data. void clear() override; diff --git a/src/ipc/broad_phase/voxel_size_heuristic.cpp b/src/ipc/broad_phase/voxel_size_heuristic.cpp index e507be76d..f7f01e3c1 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.cpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.cpp @@ -77,7 +77,7 @@ double suggest_good_voxel_size( return voxel_size; } -double suggest_good_voxel_size(const std::vector& boxes) +double suggest_good_voxel_size(const AABBs& boxes) { assert(!boxes.empty()); @@ -86,7 +86,7 @@ double suggest_good_voxel_size(const std::vector& boxes) box_sizes(i) = (boxes[i].max - boxes[i].min).maxCoeff(); } - double voxel_size; + double voxel_size = 0; igl::median(box_sizes, voxel_size); if (voxel_size <= 0) { diff --git a/src/ipc/broad_phase/voxel_size_heuristic.hpp b/src/ipc/broad_phase/voxel_size_heuristic.hpp index 5bb3f1016..5a4ee111c 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.hpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.hpp @@ -29,7 +29,7 @@ double suggest_good_voxel_size( /// @brief Suggest a good voxel size for the given mesh. /// @param boxes The axis-aligned bounding boxes of the mesh. /// @return The suggested voxel size. -double suggest_good_voxel_size(const std::vector& boxes); +double suggest_good_voxel_size(const AABBs& boxes); /// @brief Compute the average edge length of a mesh. /// @param vertices_t0 The vertex positions of the mesh at time t0. diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 934b1f7c3..f12028d84 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -56,7 +56,7 @@ void Candidates::build( broad_phase->can_vertices_collide = mesh.can_collide; broad_phase->build(vertices, mesh.edges(), mesh.faces(), inflation_radius); - broad_phase->detect_collision_candidates(dim, *this); + broad_phase->detect_collision_candidates(*this); // Codim. vertices to codim. vertices: if (mesh.num_codim_vertices()) { @@ -133,7 +133,7 @@ void Candidates::build( broad_phase->can_vertices_collide = mesh.can_collide; broad_phase->build( vertices_t0, vertices_t1, mesh.edges(), mesh.faces(), inflation_radius); - broad_phase->detect_collision_candidates(dim, *this); + broad_phase->detect_collision_candidates(*this); // Codim. vertices to codim. vertices: if (mesh.num_codim_vertices()) { diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index ad70e1dd3..6a0db7187 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 #define IPC_TOOLKIT_WITH_TBB // #define IPC_TOOLKIT_DEBUG_AUTODIFF diff --git a/src/ipc/math/CMakeLists.txt b/src/ipc/math/CMakeLists.txt index f94256b8e..b08bec9c8 100644 --- a/src/ipc/math/CMakeLists.txt +++ b/src/ipc/math/CMakeLists.txt @@ -4,6 +4,7 @@ set(SOURCES math.cpp math.hpp math.tpp + morton.hpp ) target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/math/morton.hpp b/src/ipc/math/morton.hpp new file mode 100644 index 000000000..ccc201cda --- /dev/null +++ b/src/ipc/math/morton.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include // for std::clamp +#include // for uint64_t + +namespace ipc { + +/// @brief Expands a 32-bit integer into 64 bits by inserting 1 zero after each bit. +/// @param v The 32-bit integer to expand. +/// @return The expanded 64-bit integer. +inline 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; +} + +/// @brief Expands a 21-bit integer into 63 bits by inserting 2 zeros after each bit. +/// @param v The 21-bit integer to expand. +/// @return The expanded 63-bit integer. +inline 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; +} + +/// @brief Calculates a 64-bit Morton code for the given 2D point located within the unit square [0,1]. +/// @param x The x-coordinate of the point. +/// @param y The y-coordinate of the point. +/// @return The 64-bit Morton code. +inline uint64_t morton_2D(double x, double y) +{ + constexpr double scale = 1ULL << 32; + 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; +} + +/// @brief Calculates a 63-bit Morton code for the given 3D point located within the unit cube [0,1]. +/// @param x The x-coordinate of the point. +/// @param y The y-coordinate of the point. +/// @param z The z-coordinate of the point. +/// @return The 63-bit Morton code. +inline uint64_t morton_3D(double x, double y, double z) +{ + constexpr double scale = 1ULL << 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; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/smooth_collisions_builder.cpp b/src/ipc/smooth_contact/smooth_collisions_builder.cpp index 7dcddde2e..40cc70f51 100644 --- a/src/ipc/smooth_contact/smooth_collisions_builder.cpp +++ b/src/ipc/smooth_contact/smooth_collisions_builder.cpp @@ -12,7 +12,7 @@ namespace ipc { namespace { template void add_collision( - const std::shared_ptr pair, + const std::shared_ptr& pair, unordered_map, std::shared_ptr>& cc_to_id, std::vector>& collisions) @@ -28,7 +28,7 @@ namespace { template void add_collision( - const std::shared_ptr pair, + const std::shared_ptr& pair, std::vector>& collisions) { assert(pair != nullptr); diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index 80786a29b..b6b6d9d63 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -1,10 +1,14 @@ set(SOURCES + autodiff_types.hpp + default_init_allocator.hpp eigen_ext.hpp eigen_ext.tpp local_to_global.hpp 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/default_init_allocator.hpp b/src/ipc/utils/default_init_allocator.hpp new file mode 100644 index 000000000..e00c8201b --- /dev/null +++ b/src/ipc/utils/default_init_allocator.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + +namespace ipc { + +// A wrapper that performs no initialization on construction +template > +class DefaultInitAllocator : public A { + typedef std::allocator_traits a_t; + +public: + // NOLINTNEXTLINE(readability-identifier-naming) + template struct rebind { + using other = + DefaultInitAllocator>; + }; + using A::A; + + // The magic happens here: do nothing when constructing a U + template + void + construct(U* ptr) noexcept(std::is_nothrow_default_constructible::value) + { + ::new (static_cast(ptr)) U; // Default construction + } + + // Override construct to do nothing for trivial types + template void construct(U* ptr, const U& val) + { + a_t::construct(static_cast(*this), ptr, val); + } + template + void construct(U* ptr, Args&&... args) + { + a_t::construct( + static_cast(*this), ptr, std::forward(args)...); + } +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 943f8ff8e..ffdda82eb 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -217,10 +217,10 @@ project_to_psd( /// @brief Convert a 2D or 3D vector to a 3D vector. /// @param v Vector to convert, can be 2D or 3D. /// @return Converted 3D vector. If 2D, the z-component is set to 0. -inline Eigen::Vector3d to_3D(Eigen::ConstRef v) +inline Eigen::Array3d to_3D(Eigen::ConstRef v) { assert(v.size() == 2 || v.size() == 3); - return v.size() == 2 ? Eigen::Vector3d(v.x(), v.y(), 0) : v.head<3>(); + return v.size() == 2 ? Eigen::Array3d(v.x(), v.y(), 0) : v.head<3>(); } // TODO: Change return type to Eigen::MatrixX3f diff --git a/src/ipc/utils/merge_thread_local.hpp b/src/ipc/utils/merge_thread_local.hpp index 8015a00b6..3b5cd7225 100644 --- a/src/ipc/utils/merge_thread_local.hpp +++ b/src/ipc/utils/merge_thread_local.hpp @@ -5,45 +5,131 @@ #pragma once +#include #include #include +#include // std::make_move_iterator +#include #include namespace ipc { +// Assumes `out` is empty at the start. The function may modify the provided +// `vectors` (stealing and clearing per-thread buffers) for performance. template void merge_thread_local_vectors( - const tbb::enumerable_thread_specific>& vectors, + tbb::enumerable_thread_specific>& vectors, std::vector& out) { - // size up the items - size_t size = out.size(); - for (const auto& vector : vectors) { - size += vector.size(); + IPC_TOOLKIT_PROFILE_BLOCK("merge_thread_local_vectors"); + + assert(out.empty()); + + // Since `out` is always empty, compute total from thread-local vectors + // only. + size_t total = 0; + for (auto& v : vectors) { + total += v.size(); } - // serial merge! - out.reserve(size); - for (const auto& vector : vectors) { - out.insert(out.end(), vector.begin(), vector.end()); + if (total == 0) { + return; + } + + // Fast path for trivially-copyable types: allocate once and memcpy each + // thread-local buffer into the contiguous destination. + if constexpr ( + std::is_trivially_copyable_v && std::is_default_constructible_v) { + out.resize(total); + char* dest = reinterpret_cast(out.data()); + for (auto& v : vectors) { + if (v.empty()) { + continue; + } + std::memcpy(dest, v.data(), v.size() * sizeof(T)); + dest += v.size() * sizeof(T); + // release the local buffer to reduce memory usage + std::vector().swap(v); + } + } else { + // For non-trivial types, steal the largest thread-local buffer into + // `out` (cheap swap) and move from the remaining buffers. + std::vector* largest = nullptr; + for (auto& v : vectors) { + if (!largest || v.size() > largest->size()) { + largest = &v; + } + } + + if (largest && !largest->empty()) { + // out is empty, so swapping moves the largest contents into out and + // leaves the thread-local buffer empty (former out). + out.swap(*largest); + } + + out.reserve(total); + + for (auto& v : vectors) { + if (&v != largest && !v.empty()) { + // Move elements to `out` to avoid copies when possible. + out.insert( + out.end(), std::make_move_iterator(v.begin()), + std::make_move_iterator(v.end())); + } + // Ensure capacity is released. + std::vector().swap(v); + } } } +// No trait detection: always use the generic insert-based fallback for +// portability and simplicity. + template void merge_thread_local_unordered_sets( - const tbb::enumerable_thread_specific>& sets, + tbb::enumerable_thread_specific>& sets, unordered_set& out) { - // size up the items - size_t size = out.size(); - for (const auto& set : sets) { - size += set.size(); - } - // serial merge! - out.reserve(size); - for (const auto& set : sets) { - out.insert(set.begin(), set.end()); + IPC_TOOLKIT_PROFILE_BLOCK("merge_thread_local_unordered_sets"); + + // This function assumes `out` is empty at the start and is allowed to + // modify the per-thread `sets` (stealing / clearing buffers) for better + // performance. + assert(out.empty()); + + // Compute total number of elements across thread-local sets. + size_t total = 0; + for (auto& s : sets) { + total += s.size(); + } + if (total == 0) { + return; + } + + // Steal the largest set by swapping it with `out` (cheap). + unordered_set* largest = nullptr; + for (auto& s : sets) { + if (!largest || s.size() > largest->size()) { + largest = &s; + } + } + + if (largest && !largest->empty()) { + out.swap(*largest); + } + + out.reserve(total); + + // Simplified strategy: always insert remaining elements from per-thread + // sets into `out`. After inserting we free the local buffer to reduce + // memory usage. This avoids complex trait detection and keeps behavior + // portable across different unordered_set implementations. + for (auto& s : sets) { + if (&s != largest && !s.empty()) { + out.insert(s.begin(), s.end()); + } + unordered_set().swap(s); } } diff --git a/src/ipc/utils/profiler.cpp b/src/ipc/utils/profiler.cpp new file mode 100644 index 000000000..12bbfaeb6 --- /dev/null +++ b/src/ipc/utils/profiler.cpp @@ -0,0 +1,121 @@ +#include "profiler.hpp" + +#ifdef IPC_TOOLKIT_WITH_PROFILER + +#include +#include + +namespace ipc { + +Profiler::Profiler() { } + +Profiler& profiler() +{ + static Profiler instance; + return instance; +} + +void Profiler::clear() { m_data.clear(); } + +void Profiler::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 Profiler::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 Profiler::reset() +{ + m_data.clear(); + // reset the calling thread's scope + current_scope = nlohmann::json::json_pointer(); // root +} + +void Profiler::print() const +{ + logger().info( + "[{}] profiler: {}", + fmt::format(fmt::fg(fmt::terminal_color::magenta), "timing"), + m_data.dump(2)); +} + +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 << "Id,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; + // parent id, pointer + std::queue> queue; + queue.push(std::make_pair(-1, root)); + int id = -1; + + while (!queue.empty()) { + const auto [parent_id, ptr] = queue.front(); + queue.pop(); + + assert(m_data.contains(ptr)); + const auto& data = ptr == root ? m_data : m_data.at(ptr); + if (ptr != root) { + os << fmt::format( + "{:d},{},{},{:.6g},{:d}\n", id, + parent_id == -1 ? "" : std::to_string(parent_id), 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(std::make_pair(id, ptr / key)); + } + } + + ++id; + } + + 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..2f560247e --- /dev/null +++ b/src/ipc/utils/profiler.hpp @@ -0,0 +1,121 @@ +#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(); + + // ~Profiler() { print(); } + + /// @brief Clear all profiling data. + void clear(); + + /// @brief Start timing a new scope. + /// @param name The name of the scope. + void start(const std::string& name); + + /// @brief Stop timing the current scope and record the elapsed time. + /// @param time_ms The elapsed time in milliseconds. + void stop(const double time_ms); + + /// @brief Reset the profiler data and scopes. + void reset(); + + /// @brief Print the profiler data to the logger. + void print() const; + + /// @brief Write the profiler data to an output stream in CSV format. + void write_csv(std::ostream& os) const; + + /// @brief Write the profiler data to a CSV file. + void write_csv(const std::string& filename) const; + + /// @brief Print the profiler data to standard output in CSV format. + void print_csv() const { write_csv(std::cout); } + + /// @brief Access the profiling data as a JSON object. + const nlohmann::json& data() const { return m_data; } + + /// @brief Access the profiling data as a JSON object. + nlohmann::json& data() { return m_data; } + +protected: + /// @brief The profiling data stored as a JSON object. + nlohmann::json m_data; + + /// @brief The global scope pointer into the JSON 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(); } + + // NOLINTNEXTLINE(readability-identifier-naming) + 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/benchmark_broad_phase.cpp b/tests/src/tests/broad_phase/benchmark_broad_phase.cpp index 1bbbdf61b..49260d6ec 100644 --- a/tests/src/tests/broad_phase/benchmark_broad_phase.cpp +++ b/tests/src/tests/broad_phase/benchmark_broad_phase.cpp @@ -12,7 +12,7 @@ using namespace ipc; -TEST_CASE("Benchmark broad phase", "[!benchmark][broad_phase]") +TEST_CASE("Benchmark broad phase on simple scenes", "[!benchmark][broad_phase]") { Eigen::MatrixXd V0, V1; Eigen::MatrixXi E, F; @@ -91,14 +91,47 @@ TEST_CASE("Benchmark broad phase", "[!benchmark][broad_phase]") } } -TEST_CASE( - "Benchmark broad phase on real data", - "[!benchmark][broad_phase][real_data]") +TEST_CASE("Benchmark broad phase on real scenes", "[!benchmark][broad_phase]") { Eigen::MatrixXd V0, V1; Eigen::MatrixXi E, F; std::string mesh_name_t0, mesh_name_t1; + SECTION("Two cubes") + { + mesh_name_t0 = "two-cubes-far.ply"; + mesh_name_t1 = "two-cubes-intersecting.ply"; + } + SECTION("Cloth-Ball") + { + mesh_name_t0 = "cloth_ball92.ply"; + mesh_name_t1 = "cloth_ball93.ply"; + } + SECTION("Armadillo-Rollers") + { + mesh_name_t0 = "armadillo-rollers/326.ply"; + mesh_name_t1 = "armadillo-rollers/327.ply"; + } + SECTION("Cloth-Funnel") + { + mesh_name_t0 = "cloth-funnel/227.ply"; + mesh_name_t1 = "cloth-funnel/228.ply"; + } + SECTION("N-Body-Simulation") + { + mesh_name_t0 = "n-body-simulation/balls16_18.ply"; + mesh_name_t1 = "n-body-simulation/balls16_19.ply"; + } + SECTION("Rod-Twist") + { + mesh_name_t0 = "rod-twist/3036.ply"; + mesh_name_t1 = "rod-twist/3037.ply"; + } + SECTION("Puffer-Ball") + { + mesh_name_t0 = "puffer-ball/20.ply"; + mesh_name_t1 = "puffer-ball/21.ply"; + } SECTION("Data 0") { mesh_name_t0 = "private/slow-broadphase-ccd/0.ply"; @@ -109,23 +142,13 @@ TEST_CASE( mesh_name_t0 = "private/slow-broadphase-ccd/s0.ply"; mesh_name_t1 = "private/slow-broadphase-ccd/s1.ply"; } - SECTION("Cloth-Ball") - { - mesh_name_t0 = "cloth_ball92.ply"; - mesh_name_t1 = "cloth_ball93.ply"; - } - // SECTION("Squishy-Ball") - // { - // mesh_name_t0 = "private/puffer-ball/20.ply"; - // mesh_name_t1 = "private/puffer-ball/21.ply"; - // } if (!tests::load_mesh(mesh_name_t0, V0, E, F) || !tests::load_mesh(mesh_name_t1, V1, E, F)) { - SKIP("Slow broadphase CCD meshes are private"); + SKIP("Some broadphase CCD meshes are private. Skipping benchmark."); } - double inflation_radius = 1e-2; // GENERATE(take(5, random(0.0, 0.1))); + double inflation_radius = 0; // GENERATE(take(5, random(0.0, 0.1))); CollisionMesh mesh = CollisionMesh::build_from_full_mesh(V0, E, F); // Discard codimensional/internal vertices @@ -134,12 +157,18 @@ TEST_CASE( const auto broad_phases = tests::broad_phases(); + fmt::print( + "Benchmarking broad phase on meshes: {} -> {}\n", mesh_name_t0, + mesh_name_t1); + fmt::print("Number of vertices: {}\n", V0.rows()); + fmt::print("Number of edges: {}\n", E.rows()); + fmt::print("Number of faces: {}\n", F.rows()); for (const auto& broad_phase : broad_phases) { if (broad_phase->name() == "BruteForce") { continue; } - BENCHMARK(fmt::format("BP Real Data ({})", broad_phase->name())) + BENCHMARK(fmt::format("{}", broad_phase->name())) { Candidates candidates; candidates.build(mesh, V0, V1, inflation_radius, broad_phase.get()); diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index 0c0b71ac3..37ab71545 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -274,7 +274,7 @@ TEST_CASE("Broad phase build from boxes", "[broad_phase]") { using namespace ipc; - std::vector boxes(100); + AABBs boxes(100); for (int i = 0; i < boxes.size(); ++i) { boxes[i].min = Eigen::Array3d(i * 0.6, 0, 0); boxes[i].max = Eigen::Array3d(boxes[i].min.x() + 1.0, 0, 0); @@ -284,7 +284,7 @@ TEST_CASE("Broad phase build from boxes", "[broad_phase]") REQUIRE(!boxes[0].intersects(boxes[2])); const auto broad_phase = GENERATE(tests::BroadPhaseGenerator::create()); - broad_phase->build(boxes, Eigen::MatrixXi(), Eigen::MatrixXi()); + broad_phase->build(boxes, Eigen::MatrixXi(), Eigen::MatrixXi(), /*dim=*/3); std::vector candidates; broad_phase->detect_vertex_vertex_candidates(candidates); 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..0625a8aab --- /dev/null +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -0,0 +1,285 @@ +#include + +#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).cast(); + children.max = childA.aabb_max.max(childB.aabb_max).cast(); + constexpr float EPS = 1e-4f; + return (abs(parent.aabb_max.cast() - children.max) < EPS).all() + && (abs(parent.aabb_min.cast() - children.min) < EPS).all(); +} + +void traverse_lbvh( + const LBVH::Nodes& lbvh_nodes, + const uint32_t index, + std::vector& visited) +{ + const LBVH::Node& node = lbvh_nodes[index]; + CHECK(node.is_valid()); + + if (node.is_leaf()) { + // leaf + CHECK(!visited[index]); + visited[index] = true; + } else { + // inner node + CHECK(!visited[index]); + visited[index] = true; + + // verify aabbs + LBVH::Node childA = lbvh_nodes[node.left]; + LBVH::Node childB = lbvh_nodes[node.right]; + + { + CAPTURE( + index, node.left, node.right, 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, node.left, visited); + traverse_lbvh(lbvh_nodes, node.right, visited); + } +} + +void check_valid_lbvh_nodes(const LBVH::Nodes& 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); + } + + // -- 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()); +} + +namespace { + +/// @brief Checks if every candidate in the expected vector is present in the actual vector. +/// @tparam Candidate The type of the candidate (e.g., VertexVertexCandidate, EdgeEdgeCandidate). +/// @param actual The vector of candidates found by the algorithm. +/// @param expected The vector of candidates that are expected to be found. +/// @return true If all candidates in `expected` are found in `actual`. +/// @return false Otherwise. +template +bool contains_all_candidates( + std::vector actual, std::vector expected) +{ + // 1. Sort the actual candidates to prepare for set operations + tbb::parallel_sort(actual.begin(), actual.end()); + // Ensure 'actual' has no duplicates to treat it as a mathematical set + REQUIRE(std::unique(actual.begin(), actual.end()) == actual.end()); + + // 2. Sort the expected candidates + tbb::parallel_sort(expected.begin(), expected.end()); + // Ensure 'expected' has no duplicates to treat it as a mathematical set + REQUIRE(std::unique(expected.begin(), expected.end()) == expected.end()); + + // 3. Check if 'expected' is a subset of 'actual' + // std::includes requires both ranges to be sorted. + // It returns true if every element in the second range is found in the + // first range. + return std::includes( + actual.begin(), actual.end(), expected.begin(), expected.end()); +} + +} // namespace + +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"; + } +#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); + + 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()); + CHECK(contains_all_candidates(vv_candidates, expected_vv_candidates)); + } + + { + 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()); + CHECK(contains_all_candidates(ev_candidates, expected_ev_candidates)); + } + + { + 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()); + CHECK(contains_all_candidates(ee_candidates, expected_ee_candidates)); + } + + { + 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()); + CHECK(contains_all_candidates(fv_candidates, expected_fv_candidates)); + } + + { + 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()); + CHECK(contains_all_candidates(ef_candidates, expected_ef_candidates)); + } + + { + 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()); + CHECK(contains_all_candidates(ff_candidates, expected_ff_candidates)); + } + +#ifdef IPC_TOOLKIT_WITH_PROFILER + ipc::profiler().print(); + ipc::profiler().clear(); +#endif +} diff --git a/tests/src/tests/broad_phase/test_spatial_hash.cpp b/tests/src/tests/broad_phase/test_spatial_hash.cpp index 359bddc83..5064fc159 100644 --- a/tests/src/tests/broad_phase/test_spatial_hash.cpp +++ b/tests/src/tests/broad_phase/test_spatial_hash.cpp @@ -25,7 +25,7 @@ TEST_CASE("Build SpatialHash", "[broad_phase][spatial_hash][build]") sh.build(V0, V1, E, F, inflation_radius); Candidates candidates; - sh.detect_collision_candidates(V0.cols(), candidates); + sh.detect_collision_candidates(candidates); CHECK(candidates.size() == 6'852'873); CHECK(candidates.vv_candidates.size() == 0); diff --git a/tests/src/tests/broad_phase/test_stq.cpp b/tests/src/tests/broad_phase/test_stq.cpp index 5fa85f03c..3dbf9c3a9 100644 --- a/tests/src/tests/broad_phase/test_stq.cpp +++ b/tests/src/tests/broad_phase/test_stq.cpp @@ -36,7 +36,7 @@ TEST_CASE("STQ All Cases", "[broad_phase][stq][cuda]") broad_phase->build(V0, V1, E, F, inflation_radius); Candidates candidates; - broad_phase->detect_collision_candidates(V0.cols(), candidates); + broad_phase->detect_collision_candidates(candidates); CHECK(candidates.size() == 6'852'873); CHECK(candidates.vv_candidates.size() == 0); @@ -69,8 +69,8 @@ TEST_CASE("Puffer-Ball", "[ccd][broad_phase][stq][cuda]") Eigen::MatrixXd V0, V1; Eigen::MatrixXi E, F; - if (!tests::load_mesh("private/puffer-ball/20.ply", V0, E, F) - || !tests::load_mesh("private/puffer-ball/21.ply", V1, E, F)) { + if (!tests::load_mesh("puffer-ball/20.ply", V0, E, F) + || !tests::load_mesh("puffer-ball/21.ply", V1, E, F)) { SKIP("Puffer-ball meshes not available"); } diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index 6fdb32e60..9fca19f64 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -107,7 +107,7 @@ TEST_CASE("Point-line normal hessian", "[pl][normal]") VectorMax3d e1(DIM); Eigen::VectorXd x(3 * DIM); - const int case_i = GENERATE(range(0, 10)); + const int _ = GENERATE(range(0, 10)); p.setRandom(); e0.setRandom(); e1.setRandom(); diff --git a/tests/src/tests/ccd/benchmark_ccd.cpp b/tests/src/tests/ccd/benchmark_ccd.cpp index 663fb8cb0..bec93481a 100644 --- a/tests/src/tests/ccd/benchmark_ccd.cpp +++ b/tests/src/tests/ccd/benchmark_ccd.cpp @@ -5,6 +5,7 @@ #include #include +#include #include using namespace ipc; @@ -30,11 +31,11 @@ TEST_CASE("Benchmark earliest toi", "[!benchmark][ccd][earliest_toi]") mesh_name_t0 = "cloth_ball92.ply"; mesh_name_t1 = "cloth_ball93.ply"; } - // SECTION("Squishy-Ball") - // { - // mesh_name_t0 = "private/puffer-ball/20.ply"; - // mesh_name_t1 = "private/puffer-ball/21.ply"; - // } + SECTION("Puffer-Ball") + { + mesh_name_t0 = "puffer-ball/20.ply"; + mesh_name_t1 = "puffer-ball/21.ply"; + } if (!tests::load_mesh(mesh_name_t0, V0, E, F) || !tests::load_mesh(mesh_name_t1, V1, E, F)) { @@ -48,8 +49,13 @@ TEST_CASE("Benchmark earliest toi", "[!benchmark][ccd][earliest_toi]") TightInclusionCCD ccd(/*tolerance=*/1e-6, /*max_iterations=*/1e7); + LBVH broad_phase; + Candidates candidates; - candidates.build(mesh, V0, V1); + BENCHMARK("Broad-Phase Candidate Generation") + { + candidates.build(mesh, V0, V1, 0, &broad_phase); + }; double toi; BENCHMARK("Earliest ToI Narrow-Phase") diff --git a/tests/src/tests/ccd/test_gpu_ccd.cpp b/tests/src/tests/ccd/test_gpu_ccd.cpp index bd06d03b3..f53e180dd 100644 --- a/tests/src/tests/ccd/test_gpu_ccd.cpp +++ b/tests/src/tests/ccd/test_gpu_ccd.cpp @@ -24,10 +24,10 @@ TEST_CASE("GPU CCD", "[ccd][gpu]") mesh_name_t0 = "cloth_ball92.ply"; mesh_name_t1 = "cloth_ball93.ply"; } - // SECTION("Squishy-Ball") + // SECTION("Puffer-Ball") // { - // mesh_name_t0 = "private/puffer-ball/20.ply"; - // mesh_name_t1 = "private/puffer-ball/21.ply"; + // mesh_name_t0 = "puffer-ball/20.ply"; + // mesh_name_t1 = "puffer-ball/21.ply"; // } if (!tests::load_mesh(mesh_name_t0, V0, E, F) diff --git a/tests/src/tests/utils.cpp b/tests/src/tests/utils.cpp index 4bcdfee66..2e1f8dc4b 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 @@ -27,6 +28,7 @@ std::vector> broad_phases() std::make_shared(), std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), #ifdef IPC_TOOLKIT_WITH_CUDA std::make_shared(),