Parallel CPU LBVH Implementation

.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
 ...

.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

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 PRIVATE nlohmann_json::nlohmann_json)
+endif()
+
 # Extra warnings (link last for highest priority)
 include(ipc_toolkit_warnings)
 target_link_libraries(ipc_toolkit PRIVATE ipc::toolkit::warnings)

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 ""

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 #
     ###########

python/examples/lbvh.py

@@ -0,0 +1,76 @@
+from find_ipctk import ipctk
+import meshio
+import polyscope as ps
+from polyscope import imgui
+import numpy as np
+
+import pathlib
+
+mesh = meshio.read(pathlib.Path(__file__).parents[2] / "tests/data/puffer-ball/20.ply") # noqa
+
+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()

python/examples/lbvh_profile.csv

@@ -0,0 +1,12 @@
+Id,Parent,Name,Time (ms),Count
+7,,LBVH::detect_edge_edge_candidates,331.688,6
+8,,LBVH::detect_edge_face_candidates,209.872,6
+9,,LBVH::detect_edge_vertex_candidates,139.502,6
+10,,LBVH::detect_face_face_candidates,100.436,6
+11,,LBVH::detect_face_vertex_candidates,131.918,6
+12,,LBVH::detect_vertex_vertex_candidates,17.2223,6
+13,,LBVH::init_bvh,21.2826,18
+14,13,build_hierarchy,8.12017,18
+15,13,compute_morton_codes,1.76938,18
+16,13,populate_boxes,2.26558,18
+17,13,sort_morton_codes,5.25754,18

python/examples/profiler.py

@@ -0,0 +1,40 @@
+import pandas as pd
+import plotly.express as px
+import pathlib
+
+def plot(csv_path):
+    df = pd.read_csv(csv_path, 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="<b>%{label}</b><br>Time (ms): %{value}<br>Call Count: %{customdata[0]}<br>Percentage of Parent: %{percentParent:.2%}",
+        customdata=df[["Count"]].values,
+        # tiling=dict(
+        #     orientation='v'
+        # )
+    )
+    fig.update_layout(
+        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
+
+plot(pathlib.Path(__file__).parent / "lbvh_profile.csv")

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);

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

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);

python/src/broad_phase/lbvh.cpp

@@ -0,0 +1,23 @@
+#include <common.hpp>
+
+#include <ipc/broad_phase/lbvh.hpp>
+
+using namespace ipc;
+
+void define_lbvh(py::module_& m)
+{
+    py::class_<LBVH::Node>(m, "LBVH_Node")
+        .def_readonly("aabb_min", &LBVH::Node::aabb_min)
+        .def_readonly("aabb_max", &LBVH::Node::aabb_max)
+        .def_readonly("left", &LBVH::Node::left)
+        .def_readonly("right", &LBVH::Node::right)
+        .def_property_readonly("is_leaf", &LBVH::Node::is_leaf)
+        .def_property_readonly("is_inner", &LBVH::Node::is_inner)
+        .def_property_readonly("is_valid", &LBVH::Node::is_valid);
+
+    py::class_<LBVH, BroadPhase, std::shared_ptr<LBVH>>(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);
+}

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.

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

src/ipc/broad_phase/aabb.cpp

@@ -49,20 +49,21 @@ void AABB::conservative_inflation(
     Eigen::Ref<ArrayMax3d> 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();
+    // Nudge the bounds outward to ensure conservativity.
 
-    std::fesetround(FE_DOWNWARD);
-    min -= inflation_radius;
+    min = min.unaryExpr([inflation_radius](double v) {
+        return std::nextafter(
+            v - inflation_radius, -std::numeric_limits<double>::infinity());
+    });
 
-    std::fesetround(FE_UPWARD);
-    max += inflation_radius;
-
-    std::fesetround(current_round);
+    max = max.unaryExpr([inflation_radius](double v) {
+        return std::nextafter(
+            v + inflation_radius, std::numeric_limits<double>::infinity());
+    });
 }
 
 void build_vertex_boxes(