From db6ca177b38573100a6f3a80fe8e409811c28338 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 20 Apr 2026 15:58:05 -0400 Subject: [PATCH 01/13] Add Planar-DAT (Divide and Truncate) step filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implements `planar_filter_step()` on `TrustRegion` as a direction-aware replacement for the isotropic `filter_step()`. For each active collision pair, a division plane is computed from the closest points and approach speeds; only displacement crossing that plane is truncated, leaving tangential and separating motion unconstrained. An isotropic fallback cap of 0.5·γ_r·r_q covers primitives beyond the query radius. See "Divide and Truncate" [ACM SIGGRAPH 2026]. - src/ipc/ogc/trust_region.hpp: add `planar_filter_step` declaration - src/ipc/ogc/trust_region.cpp: implement FV/EE loops + isotropic fallback - python/src/ogc/trust_region.cpp: Python binding - tests/src/tests/ogc/test_trust_region.cpp: 5 new Planar-DAT test cases - docs/source/tutorials/ogc.rst: Planar-DAT tutorial section - docs/source/references.bib: add Chen2026DivideAndTruncate entry Co-Authored-By: Claude Sonnet 4.6 --- docs/source/references.bib | 6 + docs/source/tutorials/ogc.rst | 140 +++++++++++- python/src/ogc/trust_region.cpp | 27 +++ src/ipc/ogc/trust_region.cpp | 249 ++++++++++++++++++++++ src/ipc/ogc/trust_region.hpp | 28 +++ tests/src/tests/ogc/test_trust_region.cpp | 227 ++++++++++++++++++++ 6 files changed, 676 insertions(+), 1 deletion(-) diff --git a/docs/source/references.bib b/docs/source/references.bib index 8d662e9b6..0c91946df 100644 --- a/docs/source/references.bib +++ b/docs/source/references.bib @@ -108,6 +108,12 @@ @article{Huang2024GIPC pages = {1--18}, doi = {10.1145/3643028} } +@article{Chen2026DivideAndTruncate, + title = {Divide and Truncate: A Penetration and Inversion Free Framework for Coupled Multi-physics Systems}, + author = {Chen, Anka He and Hsu, Jerry and Liu, Ziheng and Macklin, Miles and Yang, Yin and Yuksel, Cem}, + year = 2026, + journal = {ACM SIGGRAPH 2026 Conference Papers}, +} @article{Chen2025Offset, title = {Offset {Geometric} {Contact}}, author = {Chen, Anka He and Hsu, Jerry and Liu, Ziheng and Macklin, Miles and Yang, Yin and Yuksel, Cem}, diff --git a/docs/source/tutorials/ogc.rst b/docs/source/tutorials/ogc.rst index e0cd5cbce..53a423e2e 100644 --- a/docs/source/tutorials/ogc.rst +++ b/docs/source/tutorials/ogc.rst @@ -383,4 +383,142 @@ The ``warm_start_time_step`` function includes an adaptive heuristic. During the 2. If they are outside, it effectively projects them to the boundary (using the scaling method described above). 3. **Crucially**, if the number of vertices hitting the boundary exceeds ``update_threshold`` immediately during this warm start, it triggers a second ``update()`` call. -This prevents the solver from starting with a trust region that is already too restrictive for the bulk of the mesh, potentially saving solver iterations later. \ No newline at end of file +This prevents the solver from starting with a trust region that is already too restrictive for the bulk of the mesh, potentially saving solver iterations later. + +Planar-DAT (Divide and Truncate) +--------------------------------- + +The isotropic ``filter_step`` clips every vertex displacement to a sphere, which restricts motion in *all* directions — even tangential sliding or outward motion away from a contact. In dense or cluttered contact scenarios this causes **artificial damping** and **deadlock**: vertices cannot slide past each other even when no penetration would occur. + +**Planar-DAT** :cite:`Chen2026DivideAndTruncate` replaces the sphere with a *direction-aware division plane* computed per collision pair. Only the displacement component that crosses the plane (i.e., moves toward the opposing primitive) is truncated; tangential and separating motion passes through unconstrained. This typically allows steps 2× larger or more, eliminating damping artifacts. + +How the Division Plane is Computed +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +For each active collision pair the algorithm: + +1. Finds the closest points between the two primitives. +2. Constructs a unit normal :math:`\mathbf{n}` pointing from one primitive toward the other. +3. Distributes the available gap between the two primitives in proportion to their approach speeds, yielding a **division point** :math:`\mathbf{p}` on the segment connecting the closest points. +4. For every vertex of both primitives, finds the ray–plane intersection time :math:`t_i` such that :math:`\mathbf{x}_u + t_i \, \delta\mathbf{x}_u` lies on the plane through :math:`\mathbf{p}` with normal :math:`\mathbf{n}`. If :math:`t_i \in [0, 1/\gamma_r)`, the vertex is truncated to :math:`\gamma_r \, t_i`; otherwise it is left unchanged. + +An isotropic fallback cap of :math:`0.5 \, \gamma_r \, r_q` (where :math:`r_q` is the query radius) is applied to all vertices as a safety net for primitives that were not captured in the active collision set. + +Using ``planar_filter_step`` +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``planar_filter_step`` is a *drop-in replacement* for ``filter_step`` inside the solver loop. It requires the active collision set (from ``update`` or ``update_if_needed``) and the query radius used when that set was built. + +.. md-tab-set:: + + .. md-tab-item:: C++ + + .. code-block:: c++ + + // Inside the solver loop, replace: + // trust_region.filter_step(mesh, x, dx); + // with: + trust_region.planar_filter_step( + mesh, x, dx, collisions, /*query_radius=*/dhat); + + // Optionally tune the relaxation ratio (default 0.9): + // trust_region.planar_filter_step( + // mesh, x, dx, collisions, dhat, /*relaxation_ratio=*/0.9); + + .. md-tab-item:: Python + + .. code-block:: python + + # Inside the solver loop, replace: + # trust_region.filter_step(mesh, x, dx) + # with: + trust_region.planar_filter_step( + mesh, x, dx, collisions, query_radius=dhat) + + # Optionally tune the relaxation ratio (default 0.9): + # trust_region.planar_filter_step( + # mesh, x, dx, collisions, dhat, relaxation_ratio=0.9) + +Full Optimization Loop with Planar-DAT +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. md-tab-set:: + + .. md-tab-item:: C++ + + .. code-block:: c++ + + // 1. Initial setup (same as isotropic OGC) + ipc::NormalCollisions collisions; + collisions.set_collision_set_type(ipc::NormalCollisions::CollisionSetType::OGC); + ipc::ogc::TrustRegion trust_region(dhat); + + // 2. Warm start (Predict & Initialize) + trust_region.warm_start_time_step( + mesh, x, pred_x, collisions, dhat); + + // 3. Solver Loop + for (int i = 0; i < max_iterations; ++i) { + // Update trust region centers/radii if needed + trust_region.update_if_needed(mesh, x, collisions, dhat); + + // Compute search direction (Solver specific) + Eigen::MatrixXd dx = compute_search_direction(x, ...); + + // Filter step using Planar-DAT (direction-aware truncation) + trust_region.planar_filter_step(mesh, x, dx, collisions, dhat); + + // Update positions + x += dx; + + // Check convergence... + } + + .. md-tab-item:: Python + + .. code-block:: python + + # 1. Initial setup (same as isotropic OGC) + collisions = ipctk.NormalCollisions() + collisions.collision_set_type = ipctk.NormalCollisions.CollisionSetType.OGC + trust_region = ipctk.ogc.TrustRegion(dhat) + + # 2. Warm start (Predict & Initialize) + trust_region.warm_start_time_step( + mesh, x, pred_x, collisions, dhat) + + # 3. Solver Loop + for i in range(max_iterations): + # Update trust region centers/radii if needed + trust_region.update_if_needed(mesh, x, collisions, dhat) + + # Compute search direction (Solver specific) + dx = compute_search_direction(x, ...) + + # Filter step using Planar-DAT (direction-aware truncation) + trust_region.planar_filter_step(mesh, x, dx, collisions, dhat) + + # Update positions + x += dx + + # Check convergence... + +.. note:: + ``planar_filter_step`` does **not** set ``should_update_trust_region``. Unlike + the isotropic ``filter_step``, Planar-DAT is not paired with a per-vertex + trust region radius; ``update_if_needed`` should still be called at the top + of each solver iteration to refresh the collision set when needed. + +When to Use Planar-DAT vs. Isotropic +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + ++-----------------------------------+-----------------------------------------------+ +| Use ``filter_step`` (isotropic) | Use ``planar_filter_step`` (Planar-DAT) | ++===================================+===============================================+ +| Simple scenes, sparse contact | Dense contact, many simultaneous constraints | ++-----------------------------------+-----------------------------------------------+ +| Stepping-stone implementation | Coupled multi-physics simulations | ++-----------------------------------+-----------------------------------------------+ +| When implementation simplicity | When artificial damping or deadlock | +| is preferred | is observed with isotropic filtering | ++-----------------------------------+-----------------------------------------------+ \ No newline at end of file diff --git a/python/src/ogc/trust_region.cpp b/python/src/ogc/trust_region.cpp index b034fe038..28eaeba83 100644 --- a/python/src/ogc/trust_region.cpp +++ b/python/src/ogc/trust_region.cpp @@ -81,6 +81,33 @@ void define_trust_region(py::module_& m) Sets should_update_trust_region to true if the trust region should be updated on the next iteration. )ipc_Qu8mg5v7", "mesh"_a, "x"_a, "dx"_a) + .def( + "planar_filter_step", &ogc::TrustRegion::planar_filter_step, + R"ipc_Qu8mg5v7( + Filter the optimization step dx using Planar-DAT (Divide and Truncate). + + For each collision pair, computes a direction-aware division plane + and truncates only the component of displacement toward that plane. + This eliminates the artificial damping and deadlock of the isotropic + ``filter_step`` while retaining the penetration-free guarantee. + + See "Divide and Truncate: A Penetration and Inversion Free Framework + for Coupled Multi-physics Systems" [ACM SIGGRAPH 2026]. + + Parameters: + mesh: The collision mesh. + x: Current vertex positions. + dx: Proposed vertex displacements (modified in-place). + collisions: Active collision pairs (e.g. from ``update()``). + query_radius: Radius used for collision detection; displacements + beyond ``0.5 * relaxation_ratio * query_radius`` are capped + isotropically as a fallback. + relaxation_ratio: Safety margin :math:`\gamma_r \in (0, 1)`; + the displacement is stopped at this fraction of the crossing + time (default 0.9). + )ipc_Qu8mg5v7", + "mesh"_a, "x"_a, "dx"_a, "collisions"_a, "query_radius"_a, + "relaxation_ratio"_a = 0.9) .def_readwrite( "trust_region_centers", &ogc::TrustRegion::trust_region_centers, "Centers of the trust regions for each vertex.") diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index 42d16001a..c0e8a8511 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,5 +1,8 @@ #include "trust_region.hpp" +#include +#include + namespace ipc::ogc { TrustRegion::TrustRegion(double dhat) : trust_region_inflation_radius(2 * dhat) @@ -186,4 +189,250 @@ void TrustRegion::filter_step( } } +namespace { + /// @brief Compute the truncation ratio for a vertex given a division plane. + /// + /// Finds the parameter t_i such that x_u + t_i * dx_u lies on the plane + /// (n, p). If the crossing is in the future and within one step, returns + /// relaxation_ratio * t_i; otherwise returns 1 (no truncation). + /// + /// @param x_u Current position of vertex u. + /// @param dx_u Proposed displacement of vertex u. + /// @param n Division plane normal (unit vector). + /// @param p A point on the division plane. + /// @param relaxation_ratio Safety margin γ_r ∈ (0,1). + /// @return Truncation ratio in (0, 1]. + double planar_truncation_ratio( + const Eigen::Vector3d& x_u, + const Eigen::Vector3d& dx_u, + const Eigen::Vector3d& n, + const Eigen::Vector3d& p, + const double relaxation_ratio) + { + const double denom = dx_u.dot(n); + if (std::abs(denom) < 1e-15) { + return 1.0; // dx parallel to plane → no crossing + } + // Ray-plane intersection: x_u + t_i * dx_u is on plane when + // (x_u + t_i * dx_u - p) · n = 0 → t_i = -(x_u - p) · n / denom + const double t_i = -(x_u - p).dot(n) / denom; + // Only constrain if crossing is in the future and within one step + if (t_i < 0.0 || t_i >= 1.0 / relaxation_ratio) { + return 1.0; + } + return relaxation_ratio * t_i; + } + + /// @brief Compute the closest points between two edges given their distance type. + /// @return Pair (c_e, c_e') of closest points on ea and eb respectively. + std::pair edge_edge_closest_points( + const Eigen::Vector3d& ea0, + const Eigen::Vector3d& ea1, + const Eigen::Vector3d& eb0, + const Eigen::Vector3d& eb1, + const ipc::EdgeEdgeDistanceType dtype) + { + switch (dtype) { + case ipc::EdgeEdgeDistanceType::EA0_EB0: + return { ea0, eb0 }; + case ipc::EdgeEdgeDistanceType::EA0_EB1: + return { ea0, eb1 }; + case ipc::EdgeEdgeDistanceType::EA1_EB0: + return { ea1, eb0 }; + case ipc::EdgeEdgeDistanceType::EA1_EB1: + return { ea1, eb1 }; + case ipc::EdgeEdgeDistanceType::EA_EB0: { + const double alpha = ipc::point_edge_closest_point(eb0, ea0, ea1); + return { (1 - alpha) * ea0 + alpha * ea1, eb0 }; + } + case ipc::EdgeEdgeDistanceType::EA_EB1: { + const double alpha = ipc::point_edge_closest_point(eb1, ea0, ea1); + return { (1 - alpha) * ea0 + alpha * ea1, eb1 }; + } + case ipc::EdgeEdgeDistanceType::EA0_EB: { + const double beta = ipc::point_edge_closest_point(ea0, eb0, eb1); + return { ea0, (1 - beta) * eb0 + beta * eb1 }; + } + case ipc::EdgeEdgeDistanceType::EA1_EB: { + const double beta = ipc::point_edge_closest_point(ea1, eb0, eb1); + return { ea1, (1 - beta) * eb0 + beta * eb1 }; + } + default: { // EA_EB (interior) or AUTO + const Eigen::Vector2d params = + ipc::edge_edge_closest_point(ea0, ea1, eb0, eb1); + return { + (1 - params[0]) * ea0 + params[0] * ea1, + (1 - params[1]) * eb0 + params[1] * eb1, + }; + } + } + } +} // anonymous namespace + +void TrustRegion::planar_filter_step( + const CollisionMesh& mesh, + Eigen::ConstRef x, + Eigen::Ref dx, + const NormalCollisions& collisions, + const double query_radius, + const double relaxation_ratio) const +{ + assert(x.rows() == dx.rows() && x.cols() == dx.cols()); + assert(relaxation_ratio > 0 && relaxation_ratio < 1); + + // Collision mesh vertex positions (may differ from x if hidden DOFs exist) + const bool has_hidden_dofs = x.rows() != mesh.num_vertices(); + const Eigen::MatrixXd vertices = + has_hidden_dofs ? mesh.vertices(x) : x; + + // Map collision-mesh vertex index → full-mesh row index in x/dx + const Eigen::VectorXi& full_id_map = mesh.to_full_vertex_id(); + auto to_full = [&](const index_t coll_id) -> int { + return has_hidden_dofs ? full_id_map(coll_id) : (int)coll_id; + }; + + // Per-vertex truncation ratios (1 = no truncation) + Eigen::VectorXd t_v = Eigen::VectorXd::Ones(x.rows()); + + // ---- Face-Vertex Collisions ---------------------------------------- + // FaceVertexNormalCollision always has known_dtype() == P_T (interior), + // guaranteed by the OGC feasible region filter during build(). + for (const auto& fv : collisions.fv_collisions) { + const auto ids = fv.vertex_ids(mesh.edges(), mesh.faces()); + // ids = [vi, f0i, f1i, f2i] — collision mesh indices + + const Eigen::Vector3d x_v = vertices.row(ids[0]); + const Eigen::Vector3d x_a = vertices.row(ids[1]); + const Eigen::Vector3d x_b = vertices.row(ids[2]); + const Eigen::Vector3d x_c = vertices.row(ids[3]); + + // Closest point on triangle (t0=x_a, t1=x_b, t2=x_c) to vertex x_v. + // Returns barycentric (u, v); reconstruct as (1-u-v)*x_a + u*x_b + v*x_c. + const Eigen::Vector2d uv = + point_triangle_closest_point(x_v, x_a, x_b, x_c); + const Eigen::Vector3d c_vt = + (1 - uv[0] - uv[1]) * x_a + uv[0] * x_b + uv[1] * x_c; + + // Normal pointing from triangle toward vertex + const Eigen::Vector3d diff = x_v - c_vt; + const double dist = diff.norm(); + if (dist < 1e-10) { + continue; // Degenerate (primitives coincident) — skip pair + } + const Eigen::Vector3d n = diff / dist; + + // Full-mesh IDs for indexing into dx + const int fid_v = to_full(ids[0]); + const int fid_a = to_full(ids[1]); + const int fid_b = to_full(ids[2]); + const int fid_c = to_full(ids[3]); + + const Eigen::Vector3d dx_v = dx.row(fid_v); + const Eigen::Vector3d dx_a = dx.row(fid_a); + const Eigen::Vector3d dx_b = dx.row(fid_b); + const Eigen::Vector3d dx_c = dx.row(fid_c); + + // Approach speeds (Eq. 9–10 in DAT paper) + // δ_v: how fast vertex is approaching triangle (negative n-component) + // δ_t: how fast any triangle vertex is approaching vertex (positive n-component) + const double delta_v = std::max(-dx_v.dot(n), 0.0); + const double delta_t = + std::max({ dx_a.dot(n), dx_b.dot(n), dx_c.dot(n), 0.0 }); + + // Adaptive λ: allocate gap proportional to approach speeds (Eq. 11) + const double lambda = (delta_v == 0.0 && delta_t == 0.0) + ? 0.5 + : delta_t / (delta_t + delta_v); + + // Division plane point: interpolated between c_vt and x_v + const Eigen::Vector3d p = c_vt + lambda * diff; + + // Truncate all four vertices using this division plane + t_v[fid_v] = std::min(t_v[fid_v], + planar_truncation_ratio(x_v, dx_v, n, p, relaxation_ratio)); + t_v[fid_a] = std::min(t_v[fid_a], + planar_truncation_ratio(x_a, dx_a, n, p, relaxation_ratio)); + t_v[fid_b] = std::min(t_v[fid_b], + planar_truncation_ratio(x_b, dx_b, n, p, relaxation_ratio)); + t_v[fid_c] = std::min(t_v[fid_c], + planar_truncation_ratio(x_c, dx_c, n, p, relaxation_ratio)); + } + + // ---- Edge-Edge Collisions ------------------------------------------ + for (const auto& ee : collisions.ee_collisions) { + const auto ids = ee.vertex_ids(mesh.edges(), mesh.faces()); + // ids = [ea0i, ea1i, eb0i, eb1i] — collision mesh indices + + const Eigen::Vector3d ea0 = vertices.row(ids[0]); + const Eigen::Vector3d ea1 = vertices.row(ids[1]); + const Eigen::Vector3d eb0 = vertices.row(ids[2]); + const Eigen::Vector3d eb1 = vertices.row(ids[3]); + + // Closest points on each edge (dispatch on stored distance type) + const auto [c_e, c_ep] = + edge_edge_closest_points(ea0, ea1, eb0, eb1, ee.known_dtype()); + + // Normal pointing from ea toward eb + const Eigen::Vector3d diff = c_ep - c_e; + const double dist = diff.norm(); + if (dist < 1e-10) { + continue; // Degenerate — skip pair + } + const Eigen::Vector3d n = diff / dist; + + const int fid_ea0 = to_full(ids[0]); + const int fid_ea1 = to_full(ids[1]); + const int fid_eb0 = to_full(ids[2]); + const int fid_eb1 = to_full(ids[3]); + + const Eigen::Vector3d dx_ea0 = dx.row(fid_ea0); + const Eigen::Vector3d dx_ea1 = dx.row(fid_ea1); + const Eigen::Vector3d dx_eb0 = dx.row(fid_eb0); + const Eigen::Vector3d dx_eb1 = dx.row(fid_eb1); + + // δ_e: max approach of ea toward eb (in +n direction) + // δ_e': max approach of eb toward ea (in −n direction) + const double delta_e = + std::max({ dx_ea0.dot(n), dx_ea1.dot(n), 0.0 }); + const double delta_ep = + std::max({ -dx_eb0.dot(n), -dx_eb1.dot(n), 0.0 }); + + // Adaptive λ (Eq. 12 in DAT paper) + const double lambda = (delta_e == 0.0 && delta_ep == 0.0) + ? 0.5 + : delta_ep / (delta_e + delta_ep); + + // Division plane point: interpolated between c_e and c_ep + const Eigen::Vector3d p = c_e + lambda * diff; + + // Truncate all four edge vertices + t_v[fid_ea0] = std::min(t_v[fid_ea0], + planar_truncation_ratio(ea0, dx_ea0, n, p, relaxation_ratio)); + t_v[fid_ea1] = std::min(t_v[fid_ea1], + planar_truncation_ratio(ea1, dx_ea1, n, p, relaxation_ratio)); + t_v[fid_eb0] = std::min(t_v[fid_eb0], + planar_truncation_ratio(eb0, dx_eb0, n, p, relaxation_ratio)); + t_v[fid_eb1] = std::min(t_v[fid_eb1], + planar_truncation_ratio(eb1, dx_eb1, n, p, relaxation_ratio)); + } + + // ---- Isotropic Fallback -------------------------------------------- + // For primitives beyond the query radius, apply an isotropic cap so + // that no vertex moves more than 0.5 * γ_r * r_q (Sec. 4 in DAT paper). + const double iso_cap = 0.5 * relaxation_ratio * query_radius; + for (int v = 0; v < (int)x.rows(); v++) { + const double dx_norm = dx.row(v).norm(); + if (dx_norm > 0 && t_v[v] * dx_norm > iso_cap) { + t_v[v] = std::min(t_v[v], iso_cap / dx_norm); + } + } + + // ---- Apply Truncations --------------------------------------------- + for (int v = 0; v < (int)x.rows(); v++) { + if (t_v[v] < 1.0) { + dx.row(v) *= t_v[v]; + } + } +} + } // namespace ipc::ogc \ No newline at end of file diff --git a/src/ipc/ogc/trust_region.hpp b/src/ipc/ogc/trust_region.hpp index 4d120f92a..28cb2654f 100644 --- a/src/ipc/ogc/trust_region.hpp +++ b/src/ipc/ogc/trust_region.hpp @@ -97,6 +97,34 @@ struct TrustRegion { const CollisionMesh& mesh, Eigen::ConstRef x, Eigen::Ref dx); + + /// @brief Filter the optimization step dx using Planar-DAT (Divide and Truncate). + /// + /// For each collision pair, computes a direction-aware division plane and + /// truncates only the component of displacement toward that plane. This + /// eliminates the artificial damping and deadlock of the isotropic filter_step + /// while retaining the penetration-free guarantee. + /// + /// @see "Divide and Truncate: A Penetration and Inversion Free Framework + /// for Coupled Multi-physics Systems" [ACM SIGGRAPH 2026] + /// + /// @param mesh The collision mesh. + /// @param x Current vertex positions. + /// @param dx Proposed vertex displacements (modified in-place). + /// @param collisions Active collision pairs (e.g. from update()). + /// @param query_radius Radius used for collision detection; displacements + /// beyond 0.5 * relaxation_ratio * query_radius are capped + /// isotropically as a fallback. + /// @param relaxation_ratio Safety margin \f$\gamma_r \in (0,1)\f$; the + /// displacement is stopped at this fraction of the crossing time + /// (default 0.9). + void planar_filter_step( + const CollisionMesh& mesh, + Eigen::ConstRef x, + Eigen::Ref dx, + const NormalCollisions& collisions, + double query_radius, + double relaxation_ratio = 0.9) const; }; } // namespace ipc::ogc \ No newline at end of file diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 8270d1bfe..6eb73faaa 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -8,6 +8,59 @@ using namespace ipc; +// ============================================================================ +// Helper: build a minimal mesh with one triangle and one floating vertex above +// its interior, close enough to generate an FV collision with the given dhat. +// +// Layout (y-up): +// V[0] = (0, 0, 0) \ +// V[1] = (2, 0, 0) > triangle in y=0 plane +// V[2] = (1, 0, 2) / +// V[3] = (1, gap, 1) <- vertex directly above triangle centroid +// +// Returned mesh has one face [0,1,2] and three boundary edges. +static std::pair +make_fv_mesh(double gap) +{ + Eigen::MatrixXd V(4, 3); + // clang-format off + V << 0, 0, 0, + 2, 0, 0, + 1, 0, 2, + 1, gap, 1; // vertex above centroid + // clang-format on + + Eigen::MatrixXi E(3, 2); + E << 0, 1, /**/ 1, 2, /**/ 0, 2; + + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + + return { CollisionMesh(V, E, F), V }; +} + +// Helper: build a mesh with two perpendicular edges separated by a gap. +// Edge A: (0, 0, 0) → (1, 0, 0) (along x) +// Edge B: (0.5, gap, -0.5) → (0.5, gap, 0.5) (along z, above A's midpoint) +static std::pair +make_ee_mesh(double gap) +{ + Eigen::MatrixXd V(4, 3); + // clang-format off + V << 0.0, 0, 0.0, // ea0 + 1.0, 0, 0.0, // ea1 + 0.5, gap, -0.5, // eb0 + 0.5, gap, 0.5; // eb1 + // clang-format on + + Eigen::MatrixXi E(2, 2); + E << 0, 1, /**/ 2, 3; + + Eigen::MatrixXi F(0, 3); // no faces + + return { CollisionMesh(V, E, F), V }; +} + TEST_CASE("TrustRegion warm_start_time_step sets inflation radius", "[ogc]") { constexpr double dhat = 0.02; // offset distance @@ -134,3 +187,177 @@ TEST_CASE( // Since we restricted one vertex, should_update_trust_region should be set CHECK(tr.should_update_trust_region == true); } + +// ============================================================================ +// Planar-DAT (planar_filter_step) tests +// ============================================================================ + +TEST_CASE("planar_filter_step: separating vertex is not truncated", "[ogc][planar_dat]") +{ + // Vertex moving AWAY from triangle → should not be restricted at all. + const double gap = 0.5; + const double dhat = 0.6; // dhat > gap so the FV pair is active + auto [mesh, V] = make_fv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + const auto vertices = mesh.vertices(V); + collisions.build(mesh, vertices, dhat); + + REQUIRE(!collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Vertex 3 moves upward (away from triangle) + dx.row(3) << 0.0, 0.4, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + + // No truncation: separating motion must be fully preserved + CHECK(dx(3, 0) == Catch::Approx(0.0)); + CHECK(dx(3, 1) == Catch::Approx(0.4)); + CHECK(dx(3, 2) == Catch::Approx(0.0)); +} + +TEST_CASE("planar_filter_step: tangential vertex is not truncated", "[ogc][planar_dat]") +{ + // Vertex sliding sideways (perpendicular to contact normal) should be + // fully preserved; Planar-DAT only restricts motion toward the surface. + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_fv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + const auto vertices = mesh.vertices(V); + collisions.build(mesh, vertices, dhat); + + REQUIRE(!collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Vertex 3 moves sideways (contact normal is (0,1,0), dx is in xz-plane) + dx.row(3) << 0.3, 0.0, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + + // Tangential motion: no truncation (only isotropic cap applies, which is + // 0.5 * 0.9 * dhat = 0.27; 0.3 > 0.27 so it IS capped slightly) + const double iso_cap = 0.5 * 0.9 * dhat; + // The displacement magnitude after truncation should equal iso_cap + // because 0.3 > iso_cap = 0.27 + CHECK(dx.row(3).norm() == Catch::Approx(iso_cap).epsilon(1e-6)); +} + +TEST_CASE("planar_filter_step: approaching vertex is truncated", "[ogc][planar_dat]") +{ + // Vertex moving directly toward the triangle should be stopped before + // penetration. With gap=0.5 and dx_v=(0,-0.6,0), the vertex would + // penetrate if not truncated. + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_fv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + const auto vertices = mesh.vertices(V); + collisions.build(mesh, vertices, dhat); + + REQUIRE(!collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Vertex 3 approaches the triangle (would overshoot if not truncated) + dx.row(3) << 0.0, -0.6, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + + // After truncation the vertex must still be above y=0 (no penetration) + const double final_y = x(3, 1) + dx(3, 1); + CHECK(final_y > 0.0); + // The vertex must have been restricted (dx_y must be less in magnitude than 0.6) + CHECK(std::abs(dx(3, 1)) < 0.6); + // And it must be moving in the correct direction (still downward) + CHECK(dx(3, 1) < 0.0); +} + +TEST_CASE( + "planar_filter_step: preserves more displacement than isotropic for " + "separating pair", + "[ogc][planar_dat]") +{ + // When the vertex moves AWAY, planar_filter_step should preserve the full + // step, while filter_step (isotropic) would cap it to the trust region radius. + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_fv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + tr.update(mesh, V, collisions); + + REQUIRE(!collisions.empty()); + + Eigen::MatrixXd x = V; + + // Isotropic: large outward step gets capped to trust region radius + Eigen::MatrixXd dx_iso = Eigen::MatrixXd::Zero(V.rows(), 3); + dx_iso.row(3) << 0.0, 1.0, 0.0; // large step upward + tr.filter_step(mesh, x, dx_iso); + const double iso_preserved = dx_iso.row(3).norm(); + + // Planar: same step should not be truncated at all (moving away) + Eigen::MatrixXd dx_planar = Eigen::MatrixXd::Zero(V.rows(), 3); + dx_planar.row(3) << 0.0, 1.0, 0.0; + tr.planar_filter_step( + mesh, x, dx_planar, collisions, /*query_radius=*/dhat); + const double planar_preserved = dx_planar.row(3).norm(); + + // Planar-DAT should preserve strictly more (or equal) motion + CHECK(planar_preserved >= iso_preserved - 1e-10); +} + +TEST_CASE( + "planar_filter_step: edge-edge approaching pair is truncated", + "[ogc][planar_dat]") +{ + // Two perpendicular edges approaching each other should both be truncated. + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_ee_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + const auto vertices = mesh.vertices(V); + collisions.build(mesh, vertices, dhat); + + REQUIRE(!collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Edge A moves up, Edge B moves down — both approach each other + dx.row(0) << 0.0, 0.4, 0.0; + dx.row(1) << 0.0, 0.4, 0.0; + dx.row(2) << 0.0, -0.4, 0.0; + dx.row(3) << 0.0, -0.4, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + + // After truncation, no penetration: gap between edges must remain > 0 + const double ea_y = x(0, 1) + dx(0, 1); // both ea vertices move same + const double eb_y = x(2, 1) + dx(2, 1); // both eb vertices move same + CHECK(eb_y - ea_y > 0.0); // eb must stay above ea + // Both edges must have been restricted + CHECK(std::abs(dx(0, 1)) < 0.4); + CHECK(std::abs(dx(2, 1)) < 0.4); +} From 75b47d6dacc6f2804fe3f3d8a4d9cb9696d612c2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 21 Apr 2026 16:27:04 -0400 Subject: [PATCH 02/13] Refactor planar_filter_step to use struct member for relaxation ratio - Remove query_radius and relaxation_ratio parameters from planar_filter_step - Use TrustRegion::relaxed_radius_scaling member to control relaxation ratio - Update Python bindings and documentation to match new API - Move planar_truncation_ratio to private method of TrustRegion - Add compute_trust_region_beta helper for trust region fallback - Update tests to reflect fallback trust region behavior for uncovered vertices - Remove unused includes and minor formatting cleanup --- docs/source/tutorials/ogc.rst | 32 +-- .../collisions/normal/normal_collisions.cpp | 4 +- python/src/ogc/trust_region.cpp | 3 +- src/ipc/broad_phase/lbvh.hpp | 2 - src/ipc/candidates/edge_vertex.cpp | 2 - src/ipc/candidates/face_vertex.cpp | 2 - src/ipc/candidates/vertex_vertex.cpp | 2 - .../collisions/normal/normal_collision.hpp | 2 - src/ipc/ogc/trust_region.cpp | 247 ++++++++++-------- src/ipc/ogc/trust_region.hpp | 27 +- tests/src/tests/ogc/test_trust_region.cpp | 103 +++++--- tests/src/tests/utils.hpp | 4 +- 12 files changed, 243 insertions(+), 187 deletions(-) diff --git a/docs/source/tutorials/ogc.rst b/docs/source/tutorials/ogc.rst index 53a423e2e..486d06339 100644 --- a/docs/source/tutorials/ogc.rst +++ b/docs/source/tutorials/ogc.rst @@ -402,12 +402,12 @@ For each active collision pair the algorithm: 3. Distributes the available gap between the two primitives in proportion to their approach speeds, yielding a **division point** :math:`\mathbf{p}` on the segment connecting the closest points. 4. For every vertex of both primitives, finds the ray–plane intersection time :math:`t_i` such that :math:`\mathbf{x}_u + t_i \, \delta\mathbf{x}_u` lies on the plane through :math:`\mathbf{p}` with normal :math:`\mathbf{n}`. If :math:`t_i \in [0, 1/\gamma_r)`, the vertex is truncated to :math:`\gamma_r \, t_i`; otherwise it is left unchanged. -An isotropic fallback cap of :math:`0.5 \, \gamma_r \, r_q` (where :math:`r_q` is the query radius) is applied to all vertices as a safety net for primitives that were not captured in the active collision set. +For vertices not covered by an active collision pair, an isotropic fallback applies the same trust-region sphere used by ``filter_step`` — centered at ``trust_region_centers`` with radius ``trust_region_inflation_radius`` — so that no uncovered vertex can escape the safe zone. Using ``planar_filter_step`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -``planar_filter_step`` is a *drop-in replacement* for ``filter_step`` inside the solver loop. It requires the active collision set (from ``update`` or ``update_if_needed``) and the query radius used when that set was built. +``planar_filter_step`` is a *drop-in replacement* for ``filter_step`` inside the solver loop. It requires the active collision set (from ``update`` or ``update_if_needed``). The relaxation ratio is controlled via the ``relaxed_radius_scaling`` member of the ``TrustRegion`` object (default 0.9). .. md-tab-set:: @@ -418,12 +418,10 @@ Using ``planar_filter_step`` // Inside the solver loop, replace: // trust_region.filter_step(mesh, x, dx); // with: - trust_region.planar_filter_step( - mesh, x, dx, collisions, /*query_radius=*/dhat); + trust_region.planar_filter_step(mesh, x, dx, collisions); - // Optionally tune the relaxation ratio (default 0.9): - // trust_region.planar_filter_step( - // mesh, x, dx, collisions, dhat, /*relaxation_ratio=*/0.9); + // Optionally tune the relaxation ratio via the struct member: + // trust_region.relaxed_radius_scaling = 0.9; // default .. md-tab-item:: Python @@ -432,12 +430,10 @@ Using ``planar_filter_step`` # Inside the solver loop, replace: # trust_region.filter_step(mesh, x, dx) # with: - trust_region.planar_filter_step( - mesh, x, dx, collisions, query_radius=dhat) + trust_region.planar_filter_step(mesh, x, dx, collisions) - # Optionally tune the relaxation ratio (default 0.9): - # trust_region.planar_filter_step( - # mesh, x, dx, collisions, dhat, relaxation_ratio=0.9) + # Optionally tune the relaxation ratio via the struct member: + # trust_region.relaxed_radius_scaling = 0.9 # default Full Optimization Loop with Planar-DAT ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -466,7 +462,7 @@ Full Optimization Loop with Planar-DAT Eigen::MatrixXd dx = compute_search_direction(x, ...); // Filter step using Planar-DAT (direction-aware truncation) - trust_region.planar_filter_step(mesh, x, dx, collisions, dhat); + trust_region.planar_filter_step(mesh, x, dx, collisions); // Update positions x += dx; @@ -496,7 +492,7 @@ Full Optimization Loop with Planar-DAT dx = compute_search_direction(x, ...) # Filter step using Planar-DAT (direction-aware truncation) - trust_region.planar_filter_step(mesh, x, dx, collisions, dhat) + trust_region.planar_filter_step(mesh, x, dx, collisions) # Update positions x += dx @@ -504,10 +500,10 @@ Full Optimization Loop with Planar-DAT # Check convergence... .. note:: - ``planar_filter_step`` does **not** set ``should_update_trust_region``. Unlike - the isotropic ``filter_step``, Planar-DAT is not paired with a per-vertex - trust region radius; ``update_if_needed`` should still be called at the top - of each solver iteration to refresh the collision set when needed. + Like ``filter_step``, ``planar_filter_step`` also sets + ``should_update_trust_region`` when a critical mass of vertices are + restricted by the trust region. Call ``update_if_needed`` at the top of + each solver iteration to refresh the collision set when needed. When to Use Planar-DAT vs. Isotropic ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/python/src/collisions/normal/normal_collisions.cpp b/python/src/collisions/normal/normal_collisions.cpp index 7ec53a5c0..901959ab1 100644 --- a/python/src/collisions/normal/normal_collisions.cpp +++ b/python/src/collisions/normal/normal_collisions.cpp @@ -6,7 +6,7 @@ using namespace ipc; template -void define_smooth_collision_template(py::module_& m, std::string name) +void define_smooth_collision_template(py::module_& m, const std::string& name) { py::class_(m, name.c_str()) .def("name", &collision_type::name, "Get the type name of collision") @@ -15,7 +15,7 @@ void define_smooth_collision_template(py::module_& m, std::string name) "Get the number of vertices"); } -void define_smooth_collisions(py::module_& m, std::string name) +void define_smooth_collisions(py::module_& m, const std::string& name) { py::class_(m, name.c_str()) .def(py::init()) diff --git a/python/src/ogc/trust_region.cpp b/python/src/ogc/trust_region.cpp index 28eaeba83..fe9d4e2da 100644 --- a/python/src/ogc/trust_region.cpp +++ b/python/src/ogc/trust_region.cpp @@ -106,8 +106,7 @@ void define_trust_region(py::module_& m) the displacement is stopped at this fraction of the crossing time (default 0.9). )ipc_Qu8mg5v7", - "mesh"_a, "x"_a, "dx"_a, "collisions"_a, "query_radius"_a, - "relaxation_ratio"_a = 0.9) + "mesh"_a, "x"_a, "dx"_a, "collisions"_a) .def_readwrite( "trust_region_centers", &ogc::TrustRegion::trust_region_centers, "Centers of the trust regions for each vertex.") diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp index 4075c1612..c2733cf1f 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -3,8 +3,6 @@ #include #include -#include - namespace ipc { /// @brief Linear Bounding Volume Hierarchy (LBVH) broad phase collision detection. diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index 4ea4ef809..5384488d7 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -4,8 +4,6 @@ #include #include -#include - namespace ipc { EdgeVertexCandidate::EdgeVertexCandidate(index_t _edge_id, index_t _vertex_id) diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index 3c26b25c2..2408ed4e4 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -4,8 +4,6 @@ #include #include -#include - namespace ipc { FaceVertexCandidate::FaceVertexCandidate(index_t _face_id, index_t _vertex_id) diff --git a/src/ipc/candidates/vertex_vertex.cpp b/src/ipc/candidates/vertex_vertex.cpp index 1037cc28c..6d411356c 100644 --- a/src/ipc/candidates/vertex_vertex.cpp +++ b/src/ipc/candidates/vertex_vertex.cpp @@ -2,8 +2,6 @@ #include -#include - namespace ipc { VertexVertexCandidate::VertexVertexCandidate( diff --git a/src/ipc/collisions/normal/normal_collision.hpp b/src/ipc/collisions/normal/normal_collision.hpp index 9a61066ca..aa01fa69a 100644 --- a/src/ipc/collisions/normal/normal_collision.hpp +++ b/src/ipc/collisions/normal/normal_collision.hpp @@ -5,8 +5,6 @@ #include -#include - namespace ipc { class NormalCollision : virtual public CollisionStencil { diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index c0e8a8511..a3bc7c700 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,7 +1,7 @@ #include "trust_region.hpp" -#include #include +#include namespace ipc::ogc { @@ -119,6 +119,56 @@ namespace { { return std::abs(a - b) <= tol; } + + /// @brief Compute the trust-region truncation ratio for a single vertex. + /// + /// Returns the largest β ∈ (0, 1] such that ‖xi + β·dxi − ci‖ ≤ ri. + /// If xi + dxi is already inside the trust region, returns 1.0. + /// + /// @param xi Current position of the vertex. + /// @param dxi Proposed displacement of the vertex. + /// @param ci Center of the trust region. + /// @param ri Radius of the trust region. + /// @return Truncation ratio β in (0, 1]. + inline double compute_trust_region_beta( + const VectorMax3d& xi, + const VectorMax3d& dxi, + const VectorMax3d& ci, + double ri) + { + if ((xi + dxi - ci).norm() <= ri) { + return 1.0; // Already inside, no truncation needed + } + + const double a = dxi.squaredNorm(); + if (a == 0) { + return 1.0; // Zero displacement + } + + // Solve ‖x + βΔx - c‖² = r² for β: + // (x + βΔx - c)ᵀ(x + βΔx - c) - r² + // = ‖Δx‖² β² + 2(Δxᵀ(x - c)) β + ‖x - c‖² - r² + // { a } { b } { c } + const double b = 2 * dxi.dot(xi - ci); + const double c = (xi - ci).squaredNorm() - ri * ri; + assert(c <= 0); // Inside the trust region, so c <= 0 + assert(b * b - 4 * a * c >= 0); // Roots must be real + + const double sign_b = (b >= 0) ? 1.0 : -1.0; + const double sqrt_d = sqrt(b * b - 4 * a * c); + + // β = (-b + √(b² - 4ac)) / 2a, using a numerically stable form + double beta; + if (sign_b > 0) { + beta = -2 * c / (b + sqrt_d); + } else { + beta = (-b + sqrt_d) / (2 * a); + } + // β should be the positive root + assert(beta > 0); + + return beta; + } } // namespace void TrustRegion::filter_step( @@ -137,39 +187,9 @@ void TrustRegion::filter_step( // Check that the current position is within the trust region. assert((xi - ci).norm() <= trust_region_radii(i) + 1e-9); - const double alpha = (xi + dxi - ci).norm(); - if (alpha > trust_region_radii(i)) { - // Solve ‖x + βΔx - c‖² = r² for β: - // (x + βΔx - c)ᵀ(x + βΔx - c) - r² - // = ‖Δx‖² β² + 2(Δxᵀ(x - c)) β + ‖x - c‖² - r² - // { a } { b } { c } - - const double a = dxi.squaredNorm(); - assert(a > 0); // Δx should not be zero - - // b can be positive or negative - const double b = 2 * dxi.dot(xi - ci); - - const double c = (xi - ci).squaredNorm() - - trust_region_radii(i) * trust_region_radii(i); - assert(c <= 0); // Inside the trust region, so c <= 0 - - // Roots must be real - assert(b * b - 4 * a * c >= 0); - - const double sign_b = (b >= 0) ? 1.0 : -1.0; - const double sqrt_d = sqrt(b * b - 4 * a * c); - - // β = (-b + √(b² - 4ac)) / 2a, but we use a more stable form below - double beta; - if (sign_b > 0) { - beta = -2 * c / (b + sqrt_d); - } else { - beta = (-b + sqrt_d) / (2 * a); - } - // β should be the positive root - assert(beta > 0); - + const double beta = + compute_trust_region_beta(xi, dxi, ci, trust_region_radii(i)); + if (beta < 1.0) { dx.row(i).array() *= beta; assert(approx( (xi + dx.row(i).transpose() - ci).norm(), @@ -190,38 +210,6 @@ void TrustRegion::filter_step( } namespace { - /// @brief Compute the truncation ratio for a vertex given a division plane. - /// - /// Finds the parameter t_i such that x_u + t_i * dx_u lies on the plane - /// (n, p). If the crossing is in the future and within one step, returns - /// relaxation_ratio * t_i; otherwise returns 1 (no truncation). - /// - /// @param x_u Current position of vertex u. - /// @param dx_u Proposed displacement of vertex u. - /// @param n Division plane normal (unit vector). - /// @param p A point on the division plane. - /// @param relaxation_ratio Safety margin γ_r ∈ (0,1). - /// @return Truncation ratio in (0, 1]. - double planar_truncation_ratio( - const Eigen::Vector3d& x_u, - const Eigen::Vector3d& dx_u, - const Eigen::Vector3d& n, - const Eigen::Vector3d& p, - const double relaxation_ratio) - { - const double denom = dx_u.dot(n); - if (std::abs(denom) < 1e-15) { - return 1.0; // dx parallel to plane → no crossing - } - // Ray-plane intersection: x_u + t_i * dx_u is on plane when - // (x_u + t_i * dx_u - p) · n = 0 → t_i = -(x_u - p) · n / denom - const double t_i = -(x_u - p).dot(n) / denom; - // Only constrain if crossing is in the future and within one step - if (t_i < 0.0 || t_i >= 1.0 / relaxation_ratio) { - return 1.0; - } - return relaxation_ratio * t_i; - } /// @brief Compute the closest points between two edges given their distance type. /// @return Pair (c_e, c_e') of closest points on ea and eb respectively. @@ -243,26 +231,26 @@ namespace { return { ea1, eb1 }; case ipc::EdgeEdgeDistanceType::EA_EB0: { const double alpha = ipc::point_edge_closest_point(eb0, ea0, ea1); - return { (1 - alpha) * ea0 + alpha * ea1, eb0 }; + return { alpha * (ea1 - ea0) + ea0, eb0 }; } case ipc::EdgeEdgeDistanceType::EA_EB1: { const double alpha = ipc::point_edge_closest_point(eb1, ea0, ea1); - return { (1 - alpha) * ea0 + alpha * ea1, eb1 }; + return { alpha * (ea1 - ea0) + ea0, eb1 }; } case ipc::EdgeEdgeDistanceType::EA0_EB: { const double beta = ipc::point_edge_closest_point(ea0, eb0, eb1); - return { ea0, (1 - beta) * eb0 + beta * eb1 }; + return { ea0, beta * (eb1 - eb0) + eb0 }; } case ipc::EdgeEdgeDistanceType::EA1_EB: { const double beta = ipc::point_edge_closest_point(ea1, eb0, eb1); - return { ea1, (1 - beta) * eb0 + beta * eb1 }; + return { ea1, beta * (eb1 - eb0) + eb0 }; } default: { // EA_EB (interior) or AUTO const Eigen::Vector2d params = ipc::edge_edge_closest_point(ea0, ea1, eb0, eb1); return { - (1 - params[0]) * ea0 + params[0] * ea1, - (1 - params[1]) * eb0 + params[1] * eb1, + params[0] * (ea1 - ea0) + ea0, + params[1] * (eb1 - eb0) + eb0, }; } } @@ -273,22 +261,25 @@ void TrustRegion::planar_filter_step( const CollisionMesh& mesh, Eigen::ConstRef x, Eigen::Ref dx, - const NormalCollisions& collisions, - const double query_radius, - const double relaxation_ratio) const + const NormalCollisions& collisions) { assert(x.rows() == dx.rows() && x.cols() == dx.cols()); - assert(relaxation_ratio > 0 && relaxation_ratio < 1); + assert(relaxed_radius_scaling > 0 && relaxed_radius_scaling < 1); // Collision mesh vertex positions (may differ from x if hidden DOFs exist) const bool has_hidden_dofs = x.rows() != mesh.num_vertices(); - const Eigen::MatrixXd vertices = - has_hidden_dofs ? mesh.vertices(x) : x; + Eigen::MatrixXd vertices; + if (has_hidden_dofs) { + vertices = mesh.vertices(x); + } else { + vertices = x; + } // Map collision-mesh vertex index → full-mesh row index in x/dx const Eigen::VectorXi& full_id_map = mesh.to_full_vertex_id(); auto to_full = [&](const index_t coll_id) -> int { - return has_hidden_dofs ? full_id_map(coll_id) : (int)coll_id; + return has_hidden_dofs ? full_id_map(coll_id) + : static_cast(coll_id); }; // Per-vertex truncation ratios (1 = no truncation) @@ -307,11 +298,12 @@ void TrustRegion::planar_filter_step( const Eigen::Vector3d x_c = vertices.row(ids[3]); // Closest point on triangle (t0=x_a, t1=x_b, t2=x_c) to vertex x_v. - // Returns barycentric (u, v); reconstruct as (1-u-v)*x_a + u*x_b + v*x_c. + // Returns barycentric (u, v); reconstruct as (1-u-v)*x_a + u*x_b + + // v*x_c. const Eigen::Vector2d uv = point_triangle_closest_point(x_v, x_a, x_b, x_c); const Eigen::Vector3d c_vt = - (1 - uv[0] - uv[1]) * x_a + uv[0] * x_b + uv[1] * x_c; + x_a + uv[0] * (x_b - x_a) + uv[1] * (x_c - x_a); // Normal pointing from triangle toward vertex const Eigen::Vector3d diff = x_v - c_vt; @@ -334,7 +326,8 @@ void TrustRegion::planar_filter_step( // Approach speeds (Eq. 9–10 in DAT paper) // δ_v: how fast vertex is approaching triangle (negative n-component) - // δ_t: how fast any triangle vertex is approaching vertex (positive n-component) + // δ_t: how fast any triangle vertex is approaching vertex (positive + // n-component) const double delta_v = std::max(-dx_v.dot(n), 0.0); const double delta_t = std::max({ dx_a.dot(n), dx_b.dot(n), dx_c.dot(n), 0.0 }); @@ -348,14 +341,14 @@ void TrustRegion::planar_filter_step( const Eigen::Vector3d p = c_vt + lambda * diff; // Truncate all four vertices using this division plane - t_v[fid_v] = std::min(t_v[fid_v], - planar_truncation_ratio(x_v, dx_v, n, p, relaxation_ratio)); - t_v[fid_a] = std::min(t_v[fid_a], - planar_truncation_ratio(x_a, dx_a, n, p, relaxation_ratio)); - t_v[fid_b] = std::min(t_v[fid_b], - planar_truncation_ratio(x_b, dx_b, n, p, relaxation_ratio)); - t_v[fid_c] = std::min(t_v[fid_c], - planar_truncation_ratio(x_c, dx_c, n, p, relaxation_ratio)); + t_v[fid_v] = + std::min(t_v[fid_v], planar_truncation_ratio(x_v, dx_v, n, p)); + t_v[fid_a] = + std::min(t_v[fid_a], planar_truncation_ratio(x_a, dx_a, n, p)); + t_v[fid_b] = + std::min(t_v[fid_b], planar_truncation_ratio(x_b, dx_b, n, p)); + t_v[fid_c] = + std::min(t_v[fid_c], planar_truncation_ratio(x_c, dx_c, n, p)); } // ---- Edge-Edge Collisions ------------------------------------------ @@ -392,8 +385,7 @@ void TrustRegion::planar_filter_step( // δ_e: max approach of ea toward eb (in +n direction) // δ_e': max approach of eb toward ea (in −n direction) - const double delta_e = - std::max({ dx_ea0.dot(n), dx_ea1.dot(n), 0.0 }); + const double delta_e = std::max({ dx_ea0.dot(n), dx_ea1.dot(n), 0.0 }); const double delta_ep = std::max({ -dx_eb0.dot(n), -dx_eb1.dot(n), 0.0 }); @@ -406,33 +398,70 @@ void TrustRegion::planar_filter_step( const Eigen::Vector3d p = c_e + lambda * diff; // Truncate all four edge vertices - t_v[fid_ea0] = std::min(t_v[fid_ea0], - planar_truncation_ratio(ea0, dx_ea0, n, p, relaxation_ratio)); - t_v[fid_ea1] = std::min(t_v[fid_ea1], - planar_truncation_ratio(ea1, dx_ea1, n, p, relaxation_ratio)); - t_v[fid_eb0] = std::min(t_v[fid_eb0], - planar_truncation_ratio(eb0, dx_eb0, n, p, relaxation_ratio)); - t_v[fid_eb1] = std::min(t_v[fid_eb1], - planar_truncation_ratio(eb1, dx_eb1, n, p, relaxation_ratio)); + t_v[fid_ea0] = + std::min(t_v[fid_ea0], planar_truncation_ratio(ea0, dx_ea0, n, p)); + t_v[fid_ea1] = + std::min(t_v[fid_ea1], planar_truncation_ratio(ea1, dx_ea1, n, p)); + t_v[fid_eb0] = + std::min(t_v[fid_eb0], planar_truncation_ratio(eb0, dx_eb0, n, p)); + t_v[fid_eb1] = + std::min(t_v[fid_eb1], planar_truncation_ratio(eb1, dx_eb1, n, p)); } // ---- Isotropic Fallback -------------------------------------------- - // For primitives beyond the query radius, apply an isotropic cap so - // that no vertex moves more than 0.5 * γ_r * r_q (Sec. 4 in DAT paper). - const double iso_cap = 0.5 * relaxation_ratio * query_radius; - for (int v = 0; v < (int)x.rows(); v++) { - const double dx_norm = dx.row(v).norm(); - if (dx_norm > 0 && t_v[v] * dx_norm > iso_cap) { - t_v[v] = std::min(t_v[v], iso_cap / dx_norm); - } + // For primitives beyond the query radius, fall back to isotropic + // trust-region clamping (same as filter_step) to prevent penetration. + for (int i = 0; i < x.rows(); i++) { + const VectorMax3d xi = x.row(i); + const VectorMax3d dxi = dx.row(i); + const VectorMax3d ci = trust_region_centers.row(i); + // Use trust_region_inflation_radius instead of trust_region_radii(v). + // This is the key difference between Planar-DAT and Isotropic-DAT. + // This allows us to preserve more motion for vertices that are near the + // boundary of the trust region, rather than restricting them to a + // smaller trust region radius. + const double beta = compute_trust_region_beta( + xi, dxi, ci, trust_region_inflation_radius); + t_v[i] = std::min(t_v[i], beta); } // ---- Apply Truncations --------------------------------------------- - for (int v = 0; v < (int)x.rows(); v++) { + int num_updates = 0; + for (int v = 0; v < x.rows(); v++) { if (t_v[v] < 1.0) { dx.row(v) *= t_v[v]; + num_updates++; } } + + // Mirror filter_step: if a critical mass of vertices are restricted, + // flag the trust region for re-centering on the next iteration. + if (num_updates > update_threshold * mesh.num_vertices()) { + logger().trace( + "{:.1f}% of vertices restricted by planar trust region. Updating trust region.", + 100.0 * num_updates / mesh.num_vertices()); + should_update_trust_region = true; + } +} + +double TrustRegion::planar_truncation_ratio( + const Eigen::Vector3d& x_u, + const Eigen::Vector3d& dx_u, + const Eigen::Vector3d& n, + const Eigen::Vector3d& p) const +{ + const double denom = dx_u.dot(n); + if (std::abs(denom) < 1e-15) { + return 1.0; // dx parallel to plane → no crossing + } + // Ray-plane intersection: x_u + t_i * dx_u is on plane when + // (x_u + t_i * dx_u - p) · n = 0 → t_i = -(x_u - p) · n / denom + const double t_i = -(x_u - p).dot(n) / denom; + // Only constrain if crossing is in the future and within one step + if (t_i < 0.0 || t_i >= 1.0 / relaxed_radius_scaling) { + return 1.0; + } + return relaxed_radius_scaling * t_i; } } // namespace ipc::ogc \ No newline at end of file diff --git a/src/ipc/ogc/trust_region.hpp b/src/ipc/ogc/trust_region.hpp index 28cb2654f..78c1cd276 100644 --- a/src/ipc/ogc/trust_region.hpp +++ b/src/ipc/ogc/trust_region.hpp @@ -102,8 +102,8 @@ struct TrustRegion { /// /// For each collision pair, computes a direction-aware division plane and /// truncates only the component of displacement toward that plane. This - /// eliminates the artificial damping and deadlock of the isotropic filter_step - /// while retaining the penetration-free guarantee. + /// eliminates the artificial damping and deadlock of the isotropic + /// filter_step while retaining the penetration-free guarantee. /// /// @see "Divide and Truncate: A Penetration and Inversion Free Framework /// for Coupled Multi-physics Systems" [ACM SIGGRAPH 2026] @@ -122,9 +122,26 @@ struct TrustRegion { const CollisionMesh& mesh, Eigen::ConstRef x, Eigen::Ref dx, - const NormalCollisions& collisions, - double query_radius, - double relaxation_ratio = 0.9) const; + const NormalCollisions& collisions); + +private: + /// @brief Compute the truncation ratio for a vertex given a division plane. + /// + /// Finds the parameter t_i such that x_u + t_i * dx_u lies on the plane + /// (n, p). If the crossing is in the future and within one step, returns + /// relaxation_ratio * t_i; otherwise returns 1 (no truncation). + /// + /// @param x_u Current position of vertex u. + /// @param dx_u Proposed displacement of vertex u. + /// @param n Division plane normal (unit vector). + /// @param p A point on the division plane. + /// @param relaxation_ratio Safety margin γ_r ∈ (0,1). + /// @return Truncation ratio in (0, 1]. + double planar_truncation_ratio( + const Eigen::Vector3d& x_u, + const Eigen::Vector3d& dx_u, + const Eigen::Vector3d& n, + const Eigen::Vector3d& p) const; }; } // namespace ipc::ogc \ No newline at end of file diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 6eb73faaa..b31841ceb 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -19,8 +19,7 @@ using namespace ipc; // V[3] = (1, gap, 1) <- vertex directly above triangle centroid // // Returned mesh has one face [0,1,2] and three boundary edges. -static std::pair -make_fv_mesh(double gap) +static std::pair make_fv_mesh(double gap) { Eigen::MatrixXd V(4, 3); // clang-format off @@ -42,8 +41,7 @@ make_fv_mesh(double gap) // Helper: build a mesh with two perpendicular edges separated by a gap. // Edge A: (0, 0, 0) → (1, 0, 0) (along x) // Edge B: (0.5, gap, -0.5) → (0.5, gap, 0.5) (along z, above A's midpoint) -static std::pair -make_ee_mesh(double gap) +static std::pair make_ee_mesh(double gap) { Eigen::MatrixXd V(4, 3); // clang-format off @@ -192,9 +190,14 @@ TEST_CASE( // Planar-DAT (planar_filter_step) tests // ============================================================================ -TEST_CASE("planar_filter_step: separating vertex is not truncated", "[ogc][planar_dat]") +TEST_CASE( + "planar_filter_step: separating vertex is capped by trust region", + "[ogc][planar_dat]") { - // Vertex moving AWAY from triangle → should not be restricted at all. + // Vertex moving AWAY from the triangle is not restricted by the planar + // constraint, but the trust-region fallback (same as filter_step) still + // applies to prevent penetration with undetected geometry beyond the + // query radius. const double gap = 0.5; const double dhat = 0.6; // dhat > gap so the FV pair is active auto [mesh, V] = make_fv_mesh(gap); @@ -203,28 +206,41 @@ TEST_CASE("planar_filter_step: separating vertex is not truncated", "[ogc][plana ipc::NormalCollisions collisions; collisions.set_collision_set_type( ipc::NormalCollisions::CollisionSetType::OGC); - const auto vertices = mesh.vertices(V); - collisions.build(mesh, vertices, dhat); + tr.update(mesh, V, collisions); REQUIRE(!collisions.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); - // Vertex 3 moves upward (away from triangle) - dx.row(3) << 0.0, 0.4, 0.0; + // Vertex 3 moves upward (away from triangle) with a large step that + // exceeds the trust region inflation radius (2 * dhat = 1.2). + dx.row(3) << 0.0, 2.0, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + tr.planar_filter_step(mesh, x, dx, collisions); - // No truncation: separating motion must be fully preserved + // The planar constraint leaves the step alone (moving away), but the + // isotropic trust-region fallback clamps the displacement to the + // trust region inflation radius. CHECK(dx(3, 0) == Catch::Approx(0.0)); - CHECK(dx(3, 1) == Catch::Approx(0.4)); CHECK(dx(3, 2) == Catch::Approx(0.0)); + // Displacement was reduced and is still in the correct direction + CHECK(dx(3, 1) > 0.0); + CHECK(dx(3, 1) < 2.0); + // Final position must lie on the inflation-radius sphere boundary + const double dist_from_center = + (x.row(3) + dx.row(3) - tr.trust_region_centers.row(3)).norm(); + CHECK( + dist_from_center + == Catch::Approx(tr.trust_region_inflation_radius).epsilon(1e-6)); } -TEST_CASE("planar_filter_step: tangential vertex is not truncated", "[ogc][planar_dat]") +TEST_CASE( + "planar_filter_step: tangential vertex is not truncated by planar DAT", + "[ogc][planar_dat]") { - // Vertex sliding sideways (perpendicular to contact normal) should be - // fully preserved; Planar-DAT only restricts motion toward the surface. + // Vertex sliding sideways (perpendicular to contact normal) should not + // be restricted by Planar-DAT, but the trust-region fallback still + // applies. const double gap = 0.5; const double dhat = 0.6; auto [mesh, V] = make_fv_mesh(gap); @@ -233,27 +249,36 @@ TEST_CASE("planar_filter_step: tangential vertex is not truncated", "[ogc][plana ipc::NormalCollisions collisions; collisions.set_collision_set_type( ipc::NormalCollisions::CollisionSetType::OGC); - const auto vertices = mesh.vertices(V); - collisions.build(mesh, vertices, dhat); + tr.update(mesh, V, collisions); REQUIRE(!collisions.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); // Vertex 3 moves sideways (contact normal is (0,1,0), dx is in xz-plane) - dx.row(3) << 0.3, 0.0, 0.0; + // with a large step that exceeds the trust region inflation radius + // (2 * dhat = 1.2). + dx.row(3) << 2.0, 0.0, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + tr.planar_filter_step(mesh, x, dx, collisions); - // Tangential motion: no truncation (only isotropic cap applies, which is - // 0.5 * 0.9 * dhat = 0.27; 0.3 > 0.27 so it IS capped slightly) - const double iso_cap = 0.5 * 0.9 * dhat; - // The displacement magnitude after truncation should equal iso_cap - // because 0.3 > iso_cap = 0.27 - CHECK(dx.row(3).norm() == Catch::Approx(iso_cap).epsilon(1e-6)); + // Tangential motion: Planar-DAT does not truncate, but the isotropic + // trust-region fallback caps the displacement to the inflation radius. + CHECK(dx(3, 1) == Catch::Approx(0.0)); + CHECK(dx(3, 2) == Catch::Approx(0.0)); + // Displacement was reduced and is still in the correct direction + CHECK(dx(3, 0) > 0.0); + CHECK(dx(3, 0) < 2.0); + // Final position must lie on the inflation-radius sphere boundary + const double dist_from_center = + (x.row(3) + dx.row(3) - tr.trust_region_centers.row(3)).norm(); + CHECK( + dist_from_center + == Catch::Approx(tr.trust_region_inflation_radius).epsilon(1e-6)); } -TEST_CASE("planar_filter_step: approaching vertex is truncated", "[ogc][planar_dat]") +TEST_CASE( + "planar_filter_step: approaching vertex is truncated", "[ogc][planar_dat]") { // Vertex moving directly toward the triangle should be stopped before // penetration. With gap=0.5 and dx_v=(0,-0.6,0), the vertex would @@ -266,8 +291,7 @@ TEST_CASE("planar_filter_step: approaching vertex is truncated", "[ogc][planar_d ipc::NormalCollisions collisions; collisions.set_collision_set_type( ipc::NormalCollisions::CollisionSetType::OGC); - const auto vertices = mesh.vertices(V); - collisions.build(mesh, vertices, dhat); + tr.update(mesh, V, collisions); REQUIRE(!collisions.empty()); @@ -276,12 +300,13 @@ TEST_CASE("planar_filter_step: approaching vertex is truncated", "[ogc][planar_d // Vertex 3 approaches the triangle (would overshoot if not truncated) dx.row(3) << 0.0, -0.6, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + tr.planar_filter_step(mesh, x, dx, collisions); // After truncation the vertex must still be above y=0 (no penetration) const double final_y = x(3, 1) + dx(3, 1); CHECK(final_y > 0.0); - // The vertex must have been restricted (dx_y must be less in magnitude than 0.6) + // The vertex must have been restricted (dx_y must be less in magnitude than + // 0.6) CHECK(std::abs(dx(3, 1)) < 0.6); // And it must be moving in the correct direction (still downward) CHECK(dx(3, 1) < 0.0); @@ -293,7 +318,8 @@ TEST_CASE( "[ogc][planar_dat]") { // When the vertex moves AWAY, planar_filter_step should preserve the full - // step, while filter_step (isotropic) would cap it to the trust region radius. + // step, while filter_step (isotropic) would cap it to the trust region + // radius. const double gap = 0.5; const double dhat = 0.6; auto [mesh, V] = make_fv_mesh(gap); @@ -314,11 +340,11 @@ TEST_CASE( tr.filter_step(mesh, x, dx_iso); const double iso_preserved = dx_iso.row(3).norm(); - // Planar: same step should not be truncated at all (moving away) + // Planar: trust-region fallback applies (same as filter_step), so + // planar should preserve at least as much motion as isotropic. Eigen::MatrixXd dx_planar = Eigen::MatrixXd::Zero(V.rows(), 3); dx_planar.row(3) << 0.0, 1.0, 0.0; - tr.planar_filter_step( - mesh, x, dx_planar, collisions, /*query_radius=*/dhat); + tr.planar_filter_step(mesh, x, dx_planar, collisions); const double planar_preserved = dx_planar.row(3).norm(); // Planar-DAT should preserve strictly more (or equal) motion @@ -338,8 +364,7 @@ TEST_CASE( ipc::NormalCollisions collisions; collisions.set_collision_set_type( ipc::NormalCollisions::CollisionSetType::OGC); - const auto vertices = mesh.vertices(V); - collisions.build(mesh, vertices, dhat); + tr.update(mesh, V, collisions); REQUIRE(!collisions.empty()); @@ -351,12 +376,12 @@ TEST_CASE( dx.row(2) << 0.0, -0.4, 0.0; dx.row(3) << 0.0, -0.4, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions, /*query_radius=*/dhat); + tr.planar_filter_step(mesh, x, dx, collisions); // After truncation, no penetration: gap between edges must remain > 0 const double ea_y = x(0, 1) + dx(0, 1); // both ea vertices move same const double eb_y = x(2, 1) + dx(2, 1); // both eb vertices move same - CHECK(eb_y - ea_y > 0.0); // eb must stay above ea + CHECK(eb_y - ea_y > 0.0); // eb must stay above ea // Both edges must have been restricted CHECK(std::abs(dx(0, 1)) < 0.4); CHECK(std::abs(dx(2, 1)) < 0.4); diff --git a/tests/src/tests/utils.hpp b/tests/src/tests/utils.hpp index 5b126b6b7..70f02c0c9 100644 --- a/tests/src/tests/utils.hpp +++ b/tests/src/tests/utils.hpp @@ -78,8 +78,8 @@ class RotationGenerator // ============================================================================ // Matrix Market file utils -Eigen::MatrixXd loadMarketXd(const std::string& f); -Eigen::MatrixXi loadMarketXi(const std::string& f); +Eigen::MatrixXd loadMarketXd(const std::string& f); // NOLINT +Eigen::MatrixXi loadMarketXi(const std::string& f); // NOLINT // ============================================================================ From 870796e21b66e7b83058d67e738f01aaab752b65 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 11:19:15 -0400 Subject: [PATCH 03/13] Update Chen2026DivideAndTruncate bib entry with correct authors and arXiv info --- docs/source/references.bib | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/docs/source/references.bib b/docs/source/references.bib index 0c91946df..79445e160 100644 --- a/docs/source/references.bib +++ b/docs/source/references.bib @@ -109,10 +109,13 @@ @article{Huang2024GIPC doi = {10.1145/3643028} } @article{Chen2026DivideAndTruncate, - title = {Divide and Truncate: A Penetration and Inversion Free Framework for Coupled Multi-physics Systems}, - author = {Chen, Anka He and Hsu, Jerry and Liu, Ziheng and Macklin, Miles and Yang, Yin and Yuksel, Cem}, - year = 2026, - journal = {ACM SIGGRAPH 2026 Conference Papers}, + title = {Divide and Truncate: A Penetration and Inversion Free Framework for Coupled Multi-physics Systems}, + author = {Anka He Chen and Jerry Hsu and Youssef Ayman and Miles Macklin}, + year = 2026, + eprint = {2604.15513}, + archivePrefix = {arXiv}, + primaryClass = {cs.GR}, + note = {\url{https://arxiv.org/abs/2604.15513}}, } @article{Chen2025Offset, title = {Offset {Geometric} {Contact}}, From cfa632b8686d8e1547f71efebe9e8996fba52b3e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 11:46:54 -0400 Subject: [PATCH 04/13] Clarify trust-region fallback in planar_filter_step docs - Update documentation to distinguish the isotropic fallback in planar_filter_step from the clamp in filter_step - Remove outdated parameter descriptions for query_radius and relaxation_ratio in both C++ and Python docstrings - Improve explanation of safe-zone clamping for uncovered vertices in the OGC tutorial --- docs/source/tutorials/ogc.rst | 2 +- python/src/ogc/trust_region.cpp | 6 ------ src/ipc/ogc/trust_region.hpp | 7 ------- 3 files changed, 1 insertion(+), 14 deletions(-) diff --git a/docs/source/tutorials/ogc.rst b/docs/source/tutorials/ogc.rst index 486d06339..245748557 100644 --- a/docs/source/tutorials/ogc.rst +++ b/docs/source/tutorials/ogc.rst @@ -402,7 +402,7 @@ For each active collision pair the algorithm: 3. Distributes the available gap between the two primitives in proportion to their approach speeds, yielding a **division point** :math:`\mathbf{p}` on the segment connecting the closest points. 4. For every vertex of both primitives, finds the ray–plane intersection time :math:`t_i` such that :math:`\mathbf{x}_u + t_i \, \delta\mathbf{x}_u` lies on the plane through :math:`\mathbf{p}` with normal :math:`\mathbf{n}`. If :math:`t_i \in [0, 1/\gamma_r)`, the vertex is truncated to :math:`\gamma_r \, t_i`; otherwise it is left unchanged. -For vertices not covered by an active collision pair, an isotropic fallback applies the same trust-region sphere used by ``filter_step`` — centered at ``trust_region_centers`` with radius ``trust_region_inflation_radius`` — so that no uncovered vertex can escape the safe zone. +For vertices not covered by an active collision pair, an isotropic fallback clamps motion to a safe-zone sphere centered at ``trust_region_centers`` with radius ``trust_region_inflation_radius``. This fallback is distinct from the isotropic clamp in ``filter_step``, which uses the per-vertex radii ``trust_region_radii(i)``, but it serves the same purpose of ensuring that no uncovered vertex can escape the safe zone. Using ``planar_filter_step`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/python/src/ogc/trust_region.cpp b/python/src/ogc/trust_region.cpp index fe9d4e2da..8308ceb7d 100644 --- a/python/src/ogc/trust_region.cpp +++ b/python/src/ogc/trust_region.cpp @@ -99,12 +99,6 @@ void define_trust_region(py::module_& m) x: Current vertex positions. dx: Proposed vertex displacements (modified in-place). collisions: Active collision pairs (e.g. from ``update()``). - query_radius: Radius used for collision detection; displacements - beyond ``0.5 * relaxation_ratio * query_radius`` are capped - isotropically as a fallback. - relaxation_ratio: Safety margin :math:`\gamma_r \in (0, 1)`; - the displacement is stopped at this fraction of the crossing - time (default 0.9). )ipc_Qu8mg5v7", "mesh"_a, "x"_a, "dx"_a, "collisions"_a) .def_readwrite( diff --git a/src/ipc/ogc/trust_region.hpp b/src/ipc/ogc/trust_region.hpp index 78c1cd276..87b86bb3e 100644 --- a/src/ipc/ogc/trust_region.hpp +++ b/src/ipc/ogc/trust_region.hpp @@ -112,12 +112,6 @@ struct TrustRegion { /// @param x Current vertex positions. /// @param dx Proposed vertex displacements (modified in-place). /// @param collisions Active collision pairs (e.g. from update()). - /// @param query_radius Radius used for collision detection; displacements - /// beyond 0.5 * relaxation_ratio * query_radius are capped - /// isotropically as a fallback. - /// @param relaxation_ratio Safety margin \f$\gamma_r \in (0,1)\f$; the - /// displacement is stopped at this fraction of the crossing time - /// (default 0.9). void planar_filter_step( const CollisionMesh& mesh, Eigen::ConstRef x, @@ -135,7 +129,6 @@ struct TrustRegion { /// @param dx_u Proposed displacement of vertex u. /// @param n Division plane normal (unit vector). /// @param p A point on the division plane. - /// @param relaxation_ratio Safety margin γ_r ∈ (0,1). /// @return Truncation ratio in (0, 1]. double planar_truncation_ratio( const Eigen::Vector3d& x_u, From ccd021b0e5fb2b78dec2481167e33ec3308f6704 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 17:57:47 -0400 Subject: [PATCH 05/13] Add planar_filter_step support for edge-vertex and vertex-vertex collisions - Extend planar_filter_step to handle edge-vertex and vertex-vertex cases - Use VectorMax3d for dimension-agnostic closest-point calculations - Update planar_truncation_ratio to accept Eigen::ConstRef - Add comprehensive tests for edge-vertex and vertex-vertex truncation - Ensure correct handling for 2D and 3D meshes - Improve comments and clarify approach speed calculations --- src/ipc/ogc/trust_region.cpp | 125 ++++++++++-- src/ipc/ogc/trust_region.hpp | 8 +- tests/src/tests/ogc/test_trust_region.cpp | 232 ++++++++++++++++++++-- 3 files changed, 331 insertions(+), 34 deletions(-) diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index a3bc7c700..63b694b5b 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -213,11 +213,11 @@ namespace { /// @brief Compute the closest points between two edges given their distance type. /// @return Pair (c_e, c_e') of closest points on ea and eb respectively. - std::pair edge_edge_closest_points( - const Eigen::Vector3d& ea0, - const Eigen::Vector3d& ea1, - const Eigen::Vector3d& eb0, - const Eigen::Vector3d& eb1, + std::pair edge_edge_closest_points( + const VectorMax3d& ea0, + const VectorMax3d& ea1, + const VectorMax3d& eb0, + const VectorMax3d& eb1, const ipc::EdgeEdgeDistanceType dtype) { switch (dtype) { @@ -245,7 +245,7 @@ namespace { const double beta = ipc::point_edge_closest_point(ea1, eb0, eb1); return { ea1, beta * (eb1 - eb0) + eb0 }; } - default: { // EA_EB (interior) or AUTO + case ipc::EdgeEdgeDistanceType::EA_EB: { const Eigen::Vector2d params = ipc::edge_edge_closest_point(ea0, ea1, eb0, eb1); return { @@ -253,6 +253,9 @@ namespace { params[1] * (eb1 - eb0) + eb0, }; } + default: + assert(false && "unexpected EdgeEdgeDistanceType"); + return { ea0, eb0 }; // unreachable } } } // anonymous namespace @@ -326,9 +329,8 @@ void TrustRegion::planar_filter_step( // Approach speeds (Eq. 9–10 in DAT paper) // δ_v: how fast vertex is approaching triangle (negative n-component) - // δ_t: how fast any triangle vertex is approaching vertex (positive - // n-component) const double delta_v = std::max(-dx_v.dot(n), 0.0); + // δ_t: max approach speed of any triangle vertex toward vertex (Eq. 10) const double delta_t = std::max({ dx_a.dot(n), dx_b.dot(n), dx_c.dot(n), 0.0 }); @@ -383,9 +385,9 @@ void TrustRegion::planar_filter_step( const Eigen::Vector3d dx_eb0 = dx.row(fid_eb0); const Eigen::Vector3d dx_eb1 = dx.row(fid_eb1); - // δ_e: max approach of ea toward eb (in +n direction) - // δ_e': max approach of eb toward ea (in −n direction) + // δ_e: max approach speed of ea toward eb in +n direction (Eq. 12) const double delta_e = std::max({ dx_ea0.dot(n), dx_ea1.dot(n), 0.0 }); + // δ_e': max approach speed of eb toward ea in −n direction (Eq. 12) const double delta_ep = std::max({ -dx_eb0.dot(n), -dx_eb1.dot(n), 0.0 }); @@ -408,6 +410,92 @@ void TrustRegion::planar_filter_step( std::min(t_v[fid_eb1], planar_truncation_ratio(eb1, dx_eb1, n, p)); } + // ---- Edge-Vertex Collisions ---------------------------------------- + // ids = [vi, e0i, e1i, -1] — collision mesh indices + for (const auto& ev : collisions.ev_collisions) { + const auto ids = ev.vertex_ids(mesh.edges(), mesh.faces()); + + const VectorMax3d x_v = vertices.row(ids[0]); + const VectorMax3d x_e0 = vertices.row(ids[1]); + const VectorMax3d x_e1 = vertices.row(ids[2]); + + const double alpha = point_edge_closest_point(x_v, x_e0, x_e1); + assert(-1e-8 <= alpha && alpha <= (1 + 1e-8)); + const VectorMax3d c_e = x_e0 + alpha * (x_e1 - x_e0); + + // Normal pointing from edge toward vertex + const VectorMax3d diff = x_v - c_e; + const double dist = diff.norm(); + if (dist < 1e-10) { + continue; // Degenerate — skip pair + } + const VectorMax3d n = diff / dist; + + const int fid_v = to_full(ids[0]); + const int fid_e0 = to_full(ids[1]); + const int fid_e1 = to_full(ids[2]); + + const VectorMax3d dx_v = dx.row(fid_v); + const VectorMax3d dx_e0 = dx.row(fid_e0); + const VectorMax3d dx_e1 = dx.row(fid_e1); + + // δ_v: vertex approaching edge (moving in −n direction) + const double delta_v = std::max(-dx_v.dot(n), 0.0); + // δ_e: max approach speed of any edge vertex toward vertex (in +n) + const double delta_e = std::max({ dx_e0.dot(n), dx_e1.dot(n), 0.0 }); + + const double lambda = (delta_v == 0.0 && delta_e == 0.0) + ? 0.5 + : delta_e / (delta_e + delta_v); + + const VectorMax3d p = c_e + lambda * diff; + + t_v[fid_v] = + std::min(t_v[fid_v], planar_truncation_ratio(x_v, dx_v, n, p)); + t_v[fid_e0] = + std::min(t_v[fid_e0], planar_truncation_ratio(x_e0, dx_e0, n, p)); + t_v[fid_e1] = + std::min(t_v[fid_e1], planar_truncation_ratio(x_e1, dx_e1, n, p)); + } + + // ---- Vertex-Vertex Collisions -------------------------------------- + // ids = [v0i, v1i, -1, -1] — collision mesh indices + for (const auto& vv : collisions.vv_collisions) { + const auto ids = vv.vertex_ids(mesh.edges(), mesh.faces()); + + const VectorMax3d x_a = vertices.row(ids[0]); + const VectorMax3d x_b = vertices.row(ids[1]); + + // Normal pointing from v0 toward v1 + const VectorMax3d diff = x_b - x_a; + const double dist = diff.norm(); + if (dist < 1e-10) { + continue; // Degenerate — skip pair + } + const VectorMax3d n = diff / dist; + + const int fid_a = to_full(ids[0]); + const int fid_b = to_full(ids[1]); + + const VectorMax3d dx_a = dx.row(fid_a); + const VectorMax3d dx_b = dx.row(fid_b); + + // δ_a: v0 approaching v1 (moving in +n); δ_b: v1 approaching v0 (-n) + const double delta_a = std::max(dx_a.dot(n), 0.0); + const double delta_b = std::max(-dx_b.dot(n), 0.0); + + const double lambda = (delta_a == 0.0 && delta_b == 0.0) + ? 0.5 + : delta_b / (delta_a + delta_b); + + const VectorMax3d p = x_a + lambda * diff; + + t_v[fid_a] = + std::min(t_v[fid_a], planar_truncation_ratio(x_a, dx_a, n, p)); + t_v[fid_b] = + std::min(t_v[fid_b], planar_truncation_ratio(x_b, dx_b, n, p)); + } + // ---- Isotropic Fallback -------------------------------------------- // For primitives beyond the query radius, fall back to isotropic // trust-region clamping (same as filter_step) to prevent penetration. @@ -445,10 +533,10 @@ void TrustRegion::planar_filter_step( } double TrustRegion::planar_truncation_ratio( - const Eigen::Vector3d& x_u, - const Eigen::Vector3d& dx_u, - const Eigen::Vector3d& n, - const Eigen::Vector3d& p) const + Eigen::ConstRef x_u, + Eigen::ConstRef dx_u, + Eigen::ConstRef n, + Eigen::ConstRef p) const { const double denom = dx_u.dot(n); if (std::abs(denom) < 1e-15) { @@ -457,8 +545,13 @@ double TrustRegion::planar_truncation_ratio( // Ray-plane intersection: x_u + t_i * dx_u is on plane when // (x_u + t_i * dx_u - p) · n = 0 → t_i = -(x_u - p) · n / denom const double t_i = -(x_u - p).dot(n) / denom; - // Only constrain if crossing is in the future and within one step - if (t_i < 0.0 || t_i >= 1.0 / relaxed_radius_scaling) { + // Only constrain if the crossing is meaningfully in the future. + // Edge/triangle vertices lie geometrically on the division plane (provable + // from the closest-point construction), so t_i is O(ε_machine) rather + // than exactly 0. Treating any t_i below this threshold as "no crossing" + // avoids zeroing out their displacements when they are retreating. + constexpr double T_EPS = 1e-10; + if (t_i < T_EPS || t_i >= 1.0 / relaxed_radius_scaling) { return 1.0; } return relaxed_radius_scaling * t_i; diff --git a/src/ipc/ogc/trust_region.hpp b/src/ipc/ogc/trust_region.hpp index 87b86bb3e..976e77c0f 100644 --- a/src/ipc/ogc/trust_region.hpp +++ b/src/ipc/ogc/trust_region.hpp @@ -131,10 +131,10 @@ struct TrustRegion { /// @param p A point on the division plane. /// @return Truncation ratio in (0, 1]. double planar_truncation_ratio( - const Eigen::Vector3d& x_u, - const Eigen::Vector3d& dx_u, - const Eigen::Vector3d& n, - const Eigen::Vector3d& p) const; + Eigen::ConstRef x_u, + Eigen::ConstRef dx_u, + Eigen::ConstRef n, + Eigen::ConstRef p) const; }; } // namespace ipc::ogc \ No newline at end of file diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index b31841ceb..9290467fa 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -317,9 +317,14 @@ TEST_CASE( "separating pair", "[ogc][planar_dat]") { - // When the vertex moves AWAY, planar_filter_step should preserve the full - // step, while filter_step (isotropic) would cap it to the trust region - // radius. + // When the vertex moves AWAY with a step that exceeds trust_region_radii + // (tight, per-vertex) but stays within trust_region_inflation_radius + // (loose), planar_filter_step passes the full step while filter_step + // clips it to trust_region_radii. + // + // For gap=0.5, dhat=0.6: inflation_radius = 1.2, + // trust_region_radii(3) ≈ gap * relaxed_radius_scaling / 2 ≈ 0.225. + // A separating step of 0.3 sits between those two values. const double gap = 0.5; const double dhat = 0.6; auto [mesh, V] = make_fv_mesh(gap); @@ -332,23 +337,71 @@ TEST_CASE( REQUIRE(!collisions.empty()); + // Guard: per-vertex radius must be < 0.3 for the test to be meaningful + REQUIRE(tr.trust_region_radii(3) < 0.3); + Eigen::MatrixXd x = V; - // Isotropic: large outward step gets capped to trust region radius + // Planar-DAT: separating step (t_i < 0 — no plane crossing) and within + // inflation radius → full step preserved + Eigen::MatrixXd dx_planar = Eigen::MatrixXd::Zero(V.rows(), 3); + dx_planar.row(3) << 0.0, 0.3, 0.0; + tr.planar_filter_step(mesh, x, dx_planar, collisions); + CHECK(dx_planar(3, 1) == Catch::Approx(0.3)); + + // Isotropic: clips to trust_region_radii(3) < 0.3 Eigen::MatrixXd dx_iso = Eigen::MatrixXd::Zero(V.rows(), 3); - dx_iso.row(3) << 0.0, 1.0, 0.0; // large step upward + dx_iso.row(3) << 0.0, 0.3, 0.0; tr.filter_step(mesh, x, dx_iso); - const double iso_preserved = dx_iso.row(3).norm(); + CHECK(dx_iso(3, 1) < 0.3 - 1e-10); +} - // Planar: trust-region fallback applies (same as filter_step), so - // planar should preserve at least as much motion as isotropic. - Eigen::MatrixXd dx_planar = Eigen::MatrixXd::Zero(V.rows(), 3); - dx_planar.row(3) << 0.0, 1.0, 0.0; - tr.planar_filter_step(mesh, x, dx_planar, collisions); - const double planar_preserved = dx_planar.row(3).norm(); +// Helper: two vertices with no edges/faces, separated by gap along y. +// V[0] = (0, 0, 0) +// V[1] = (0, gap, 0) +static std::pair make_vv_mesh(double gap) +{ + Eigen::MatrixXd V(2, 3); + V << 0.0, 0.0, 0.0, /**/ 0.0, gap, 0.0; + Eigen::MatrixXi E(0, 2); + Eigen::MatrixXi F(0, 3); + return { CollisionMesh(V, E, F), V }; +} + +// Helper: one edge along x with a vertex above its midpoint, gap along y. +// V[0] = (0, 0, 0) edge e0 +// V[1] = (1, 0, 0) edge e1 +// V[2] = (0.5, gap, 0) vertex above midpoint +static std::pair make_ev_mesh(double gap) +{ + Eigen::MatrixXd V(3, 3); + // clang-format off + V << 0.0, 0, 0.0, + 1.0, 0, 0.0, + 0.5, gap, 0.0; + // clang-format on + Eigen::MatrixXi E(1, 2); + E << 0, 1; + Eigen::MatrixXi F(0, 3); + return { CollisionMesh(V, E, F), V }; +} - // Planar-DAT should preserve strictly more (or equal) motion - CHECK(planar_preserved >= iso_preserved - 1e-10); +// Helper: 2-D edge (x-axis) with a vertex above midpoint, using 2D positions. +// V[0] = (0, 0) edge e0 +// V[1] = (1, 0) edge e1 +// V[2] = (0.5, gap) vertex above midpoint +static std::pair make_ev_mesh_2d(double gap) +{ + Eigen::MatrixXd V(3, 2); + // clang-format off + V << 0.0, 0, + 1.0, 0, + 0.5, gap; + // clang-format on + Eigen::MatrixXi E(1, 2); + E << 0, 1; + Eigen::MatrixXi F(0, 3); + return { CollisionMesh(V, E, F), V }; } TEST_CASE( @@ -386,3 +439,154 @@ TEST_CASE( CHECK(std::abs(dx(0, 1)) < 0.4); CHECK(std::abs(dx(2, 1)) < 0.4); } + +TEST_CASE( + "planar_filter_step: vertex-vertex approaching pair is truncated", + "[ogc][planar_dat]") +{ + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_vv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + tr.update(mesh, V, collisions); + + REQUIRE(!collisions.vv_collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // v0 approaches v1 (upward), v1 approaches v0 (downward) + dx.row(0) << 0.0, 0.4, 0.0; + dx.row(1) << 0.0, -0.4, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions); + + // After truncation gap must remain positive + const double final_gap = (x(1, 1) + dx(1, 1)) - (x(0, 1) + dx(0, 1)); + CHECK(final_gap > 0.0); + // Both vertices must have been restricted + CHECK(std::abs(dx(0, 1)) < 0.4); + CHECK(std::abs(dx(1, 1)) < 0.4); +} + +TEST_CASE( + "planar_filter_step: edge-vertex approaching pair is truncated", + "[ogc][planar_dat]") +{ + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_ev_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + tr.update(mesh, V, collisions); + + REQUIRE(!collisions.ev_collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Vertex approaches edge (downward), edge stationary + dx.row(2) << 0.0, -0.6, 0.0; + + tr.planar_filter_step(mesh, x, dx, collisions); + + // After truncation vertex must remain above edge (y > 0) + const double final_y = x(2, 1) + dx(2, 1); + CHECK(final_y > 0.0); + CHECK(std::abs(dx(2, 1)) < 0.6); + CHECK(dx(2, 1) < 0.0); // still moving downward + + // Tangential motion of the vertex is not truncated by the planar + // constraint (only the isotropic fallback applies if needed) + Eigen::MatrixXd dx_tan = Eigen::MatrixXd::Zero(V.rows(), 3); + dx_tan.row(2) << 0.3, 0.0, 0.0; // tangential (within inflation radius) + tr.planar_filter_step(mesh, x, dx_tan, collisions); + CHECK(dx_tan(2, 0) == Catch::Approx(0.3)); // not truncated +} + +TEST_CASE( + "planar_filter_step: approaching vertex, retreating triangle — only " + "vertex is truncated", + "[ogc][planar_dat]") +{ + // This tests the image scenario: P approaches triangle ABC while + // A, B, C move away from P. + // + // With delta_t = 0 (triangle not closing), λ = 0 and the division + // plane is placed at the triangle surface (c_vt). The planar constraint + // truncates only P; A, B, C have t_i < 0 (moving away from the plane) + // so their displacements are left untouched. + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_fv_mesh(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + tr.update(mesh, V, collisions); + + REQUIRE(!collisions.fv_collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + + // Triangle (A=0, B=1, C=2) moves away from P (downward, −y). + // P (vertex 3) approaches the triangle (would overshoot gap=0.5). + const double retreat = 0.3; + const double approach = 0.6; + dx.row(0) << 0.0, -retreat, 0.0; + dx.row(1) << 0.0, -retreat, 0.0; + dx.row(2) << 0.0, -retreat, 0.0; + dx.row(3) << 0.0, -approach, 0.0; // P towards triangle + + tr.planar_filter_step(mesh, x, dx, collisions); + + // A, B, C move away from the division plane (t_i < 0) → no truncation. + CHECK(dx(0, 1) == Catch::Approx(-retreat)); + CHECK(dx(1, 1) == Catch::Approx(-retreat)); + CHECK(dx(2, 1) == Catch::Approx(-retreat)); + + // P must be truncated: less displacement in magnitude, still downward. + CHECK(dx(3, 1) > -approach + 1e-10); + CHECK(dx(3, 1) < 0.0); + + // P must not penetrate the triangle (final P.y > final triangle.y). + const double final_tri_y = x(0, 1) + dx(0, 1); // all triangle verts same + const double final_p_y = x(3, 1) + dx(3, 1); + CHECK(final_p_y > final_tri_y); +} + +TEST_CASE( + "planar_filter_step: works on 2D mesh (edge-vertex)", + "[ogc][planar_dat]") +{ + const double gap = 0.5; + const double dhat = 0.6; + auto [mesh, V] = make_ev_mesh_2d(gap); + + ipc::ogc::TrustRegion tr(dhat); + ipc::NormalCollisions collisions; + collisions.set_collision_set_type( + ipc::NormalCollisions::CollisionSetType::OGC); + tr.update(mesh, V, collisions); + + REQUIRE(!collisions.ev_collisions.empty()); + + Eigen::MatrixXd x = V; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 2); + // Vertex approaches edge (downward) + dx.row(2) << 0.0, -0.6; + + // Must not crash and must truncate the approaching vertex + tr.planar_filter_step(mesh, x, dx, collisions); + + const double final_y = x(2, 1) + dx(2, 1); + CHECK(final_y > 0.0); + CHECK(std::abs(dx(2, 1)) < 0.6); +} From 41ea9807c2227ac7120f1fbc4c98115af9c540c2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 20:26:00 -0400 Subject: [PATCH 06/13] Fix formatting of TEST_CASE macro arguments in planar_filter_step test --- tests/src/tests/ogc/test_trust_region.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 9290467fa..66ddea3c2 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -563,8 +563,7 @@ TEST_CASE( } TEST_CASE( - "planar_filter_step: works on 2D mesh (edge-vertex)", - "[ogc][planar_dat]") + "planar_filter_step: works on 2D mesh (edge-vertex)", "[ogc][planar_dat]") { const double gap = 0.5; const double dhat = 0.6; From 794b57bccdaae766f6dee2e81eddbf59aa163159 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 21:05:46 -0400 Subject: [PATCH 07/13] Remove unicode from test name --- tests/src/tests/ogc/test_trust_region.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 66ddea3c2..391db6a4d 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -510,12 +510,11 @@ TEST_CASE( } TEST_CASE( - "planar_filter_step: approaching vertex, retreating triangle — only " + "planar_filter_step: approaching vertex, retreating triangle - only " "vertex is truncated", "[ogc][planar_dat]") { - // This tests the image scenario: P approaches triangle ABC while - // A, B, C move away from P. + // Tests the scenario: P approaches triangle ABC while A, B, C move away. // // With delta_t = 0 (triangle not closing), λ = 0 and the division // plane is placed at the triangle surface (c_vt). The planar constraint From 95d0b7ccc296ea29c8cdda8e6c36ca5533fc8206 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 25 Apr 2026 20:41:28 -0400 Subject: [PATCH 08/13] Refactor planar_filter_step to use Candidates directly via CollisionStencil interface MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the four type-specific loops (FV/EE/EV/VV over NormalCollisions) with a single unified loop over Candidates using the polymorphic CollisionStencil interface. This eliminates the stale-distance-type bug where NormalCollisions decomposes FV pairs into sub-types at update() time; as vertices move during optimization the stored type goes stale, causing wrong division plane geometry. Using Candidates directly means FaceVertexCandidate::known_dtype() always returns AUTO and compute_distance_vector() recomputes the closest-point type from current positions. The division plane formula is unified to p = c_second + λ·dv for all collision types, consistent with the old per-type formulas and correct when c_first is a single vertex (plane at λ=0 placed at c_second rather than at the vertex itself). The candidates member is stored on TrustRegion and planar_filter_step uses it directly (no longer takes a separate candidates/collisions argument). Co-Authored-By: Claude Sonnet 4.6 --- docs/source/tutorials/ogc.rst | 10 +- python/src/ogc/trust_region.cpp | 13 +- src/ipc/ogc/trust_region.cpp | 307 ++++------------------ src/ipc/ogc/trust_region.hpp | 16 +- tests/src/tests/ogc/test_trust_region.cpp | 28 +- 5 files changed, 87 insertions(+), 287 deletions(-) diff --git a/docs/source/tutorials/ogc.rst b/docs/source/tutorials/ogc.rst index 245748557..995cdfa14 100644 --- a/docs/source/tutorials/ogc.rst +++ b/docs/source/tutorials/ogc.rst @@ -407,7 +407,7 @@ For vertices not covered by an active collision pair, an isotropic fallback clam Using ``planar_filter_step`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -``planar_filter_step`` is a *drop-in replacement* for ``filter_step`` inside the solver loop. It requires the active collision set (from ``update`` or ``update_if_needed``). The relaxation ratio is controlled via the ``relaxed_radius_scaling`` member of the ``TrustRegion`` object (default 0.9). +``planar_filter_step`` is a *drop-in replacement* for ``filter_step`` inside the solver loop. It uses the collision candidates stored in ``trust_region.candidates`` by the last call to ``update`` or ``update_if_needed``. The relaxation ratio is controlled via the ``relaxed_radius_scaling`` member of the ``TrustRegion`` object (default 0.9). .. md-tab-set:: @@ -418,7 +418,7 @@ Using ``planar_filter_step`` // Inside the solver loop, replace: // trust_region.filter_step(mesh, x, dx); // with: - trust_region.planar_filter_step(mesh, x, dx, collisions); + trust_region.planar_filter_step(mesh, x, dx); // Optionally tune the relaxation ratio via the struct member: // trust_region.relaxed_radius_scaling = 0.9; // default @@ -430,7 +430,7 @@ Using ``planar_filter_step`` # Inside the solver loop, replace: # trust_region.filter_step(mesh, x, dx) # with: - trust_region.planar_filter_step(mesh, x, dx, collisions) + trust_region.planar_filter_step(mesh, x, dx) # Optionally tune the relaxation ratio via the struct member: # trust_region.relaxed_radius_scaling = 0.9 # default @@ -462,7 +462,7 @@ Full Optimization Loop with Planar-DAT Eigen::MatrixXd dx = compute_search_direction(x, ...); // Filter step using Planar-DAT (direction-aware truncation) - trust_region.planar_filter_step(mesh, x, dx, collisions); + trust_region.planar_filter_step(mesh, x, dx); // Update positions x += dx; @@ -492,7 +492,7 @@ Full Optimization Loop with Planar-DAT dx = compute_search_direction(x, ...) # Filter step using Planar-DAT (direction-aware truncation) - trust_region.planar_filter_step(mesh, x, dx, collisions) + trust_region.planar_filter_step(mesh, x, dx) # Update positions x += dx diff --git a/python/src/ogc/trust_region.cpp b/python/src/ogc/trust_region.cpp index 8308ceb7d..9b6eb2dbd 100644 --- a/python/src/ogc/trust_region.cpp +++ b/python/src/ogc/trust_region.cpp @@ -86,8 +86,9 @@ void define_trust_region(py::module_& m) R"ipc_Qu8mg5v7( Filter the optimization step dx using Planar-DAT (Divide and Truncate). - For each collision pair, computes a direction-aware division plane - and truncates only the component of displacement toward that plane. + For each collision candidate (stored in ``candidates`` by the last + call to ``update``), computes a direction-aware division plane and + truncates only the component of displacement toward that plane. This eliminates the artificial damping and deadlock of the isotropic ``filter_step`` while retaining the penetration-free guarantee. @@ -98,9 +99,8 @@ void define_trust_region(py::module_& m) mesh: The collision mesh. x: Current vertex positions. dx: Proposed vertex displacements (modified in-place). - collisions: Active collision pairs (e.g. from ``update()``). )ipc_Qu8mg5v7", - "mesh"_a, "x"_a, "dx"_a, "collisions"_a) + "mesh"_a, "x"_a, "dx"_a) .def_readwrite( "trust_region_centers", &ogc::TrustRegion::trust_region_centers, "Centers of the trust regions for each vertex.") @@ -138,5 +138,8 @@ void define_trust_region(py::module_& m) .def_readwrite( "should_update_trust_region", &ogc::TrustRegion::should_update_trust_region, - "If true, the trust region will be updated on the next call to ``update_if_needed``."); + "If true, the trust region will be updated on the next call to ``update_if_needed``.") + .def_readwrite( + "candidates", &ogc::TrustRegion::candidates, + "Collision candidates used for Planar-DAT. Updated by ``update()``."); } diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index 63b694b5b..b833a478b 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,8 +1,5 @@ #include "trust_region.hpp" -#include -#include - namespace ipc::ogc { TrustRegion::TrustRegion(double dhat) : trust_region_inflation_radius(2 * dhat) @@ -81,15 +78,14 @@ void TrustRegion::update( vertices = mesh.vertices(x); } - Candidates trust_region_candidates; - trust_region_candidates.build( + candidates.build( mesh, vertices, trust_region_inflation_radius + min_distance, broad_phase); // Compute the trust region distances for the reduced set of collision // vertices. Eigen::VectorXd reduced_trust_region_radii = - trust_region_candidates.compute_per_vertex_safe_distances( + candidates.compute_per_vertex_safe_distances( mesh, vertices, trust_region_inflation_radius, min_distance); // Use < half the safe distance to account for double sided contact @@ -110,7 +106,7 @@ void TrustRegion::update( // NOTE: Use the trust_region_inflation_radius to ensure that the // collisions include all pairs until this function is called again. collisions.build( - trust_region_candidates, mesh, vertices, trust_region_inflation_radius, + candidates, mesh, vertices, trust_region_inflation_radius, min_distance); } @@ -209,66 +205,16 @@ void TrustRegion::filter_step( } } -namespace { - - /// @brief Compute the closest points between two edges given their distance type. - /// @return Pair (c_e, c_e') of closest points on ea and eb respectively. - std::pair edge_edge_closest_points( - const VectorMax3d& ea0, - const VectorMax3d& ea1, - const VectorMax3d& eb0, - const VectorMax3d& eb1, - const ipc::EdgeEdgeDistanceType dtype) - { - switch (dtype) { - case ipc::EdgeEdgeDistanceType::EA0_EB0: - return { ea0, eb0 }; - case ipc::EdgeEdgeDistanceType::EA0_EB1: - return { ea0, eb1 }; - case ipc::EdgeEdgeDistanceType::EA1_EB0: - return { ea1, eb0 }; - case ipc::EdgeEdgeDistanceType::EA1_EB1: - return { ea1, eb1 }; - case ipc::EdgeEdgeDistanceType::EA_EB0: { - const double alpha = ipc::point_edge_closest_point(eb0, ea0, ea1); - return { alpha * (ea1 - ea0) + ea0, eb0 }; - } - case ipc::EdgeEdgeDistanceType::EA_EB1: { - const double alpha = ipc::point_edge_closest_point(eb1, ea0, ea1); - return { alpha * (ea1 - ea0) + ea0, eb1 }; - } - case ipc::EdgeEdgeDistanceType::EA0_EB: { - const double beta = ipc::point_edge_closest_point(ea0, eb0, eb1); - return { ea0, beta * (eb1 - eb0) + eb0 }; - } - case ipc::EdgeEdgeDistanceType::EA1_EB: { - const double beta = ipc::point_edge_closest_point(ea1, eb0, eb1); - return { ea1, beta * (eb1 - eb0) + eb0 }; - } - case ipc::EdgeEdgeDistanceType::EA_EB: { - const Eigen::Vector2d params = - ipc::edge_edge_closest_point(ea0, ea1, eb0, eb1); - return { - params[0] * (ea1 - ea0) + ea0, - params[1] * (eb1 - eb0) + eb0, - }; - } - default: - assert(false && "unexpected EdgeEdgeDistanceType"); - return { ea0, eb0 }; // unreachable - } - } -} // anonymous namespace - void TrustRegion::planar_filter_step( const CollisionMesh& mesh, Eigen::ConstRef x, - Eigen::Ref dx, - const NormalCollisions& collisions) + Eigen::Ref dx) { assert(x.rows() == dx.rows() && x.cols() == dx.cols()); assert(relaxed_radius_scaling > 0 && relaxed_radius_scaling < 1); + const int d = x.cols(); + // Collision mesh vertex positions (may differ from x if hidden DOFs exist) const bool has_hidden_dofs = x.rows() != mesh.num_vertices(); Eigen::MatrixXd vertices; @@ -288,212 +234,57 @@ void TrustRegion::planar_filter_step( // Per-vertex truncation ratios (1 = no truncation) Eigen::VectorXd t_v = Eigen::VectorXd::Ones(x.rows()); - // ---- Face-Vertex Collisions ---------------------------------------- - // FaceVertexNormalCollision always has known_dtype() == P_T (interior), - // guaranteed by the OGC feasible region filter during build(). - for (const auto& fv : collisions.fv_collisions) { - const auto ids = fv.vertex_ids(mesh.edges(), mesh.faces()); - // ids = [vi, f0i, f1i, f2i] — collision mesh indices - - const Eigen::Vector3d x_v = vertices.row(ids[0]); - const Eigen::Vector3d x_a = vertices.row(ids[1]); - const Eigen::Vector3d x_b = vertices.row(ids[2]); - const Eigen::Vector3d x_c = vertices.row(ids[3]); - - // Closest point on triangle (t0=x_a, t1=x_b, t2=x_c) to vertex x_v. - // Returns barycentric (u, v); reconstruct as (1-u-v)*x_a + u*x_b + - // v*x_c. - const Eigen::Vector2d uv = - point_triangle_closest_point(x_v, x_a, x_b, x_c); - const Eigen::Vector3d c_vt = - x_a + uv[0] * (x_b - x_a) + uv[1] * (x_c - x_a); - - // Normal pointing from triangle toward vertex - const Eigen::Vector3d diff = x_v - c_vt; - const double dist = diff.norm(); - if (dist < 1e-10) { - continue; // Degenerate (primitives coincident) — skip pair - } - const Eigen::Vector3d n = diff / dist; - - // Full-mesh IDs for indexing into dx - const int fid_v = to_full(ids[0]); - const int fid_a = to_full(ids[1]); - const int fid_b = to_full(ids[2]); - const int fid_c = to_full(ids[3]); - - const Eigen::Vector3d dx_v = dx.row(fid_v); - const Eigen::Vector3d dx_a = dx.row(fid_a); - const Eigen::Vector3d dx_b = dx.row(fid_b); - const Eigen::Vector3d dx_c = dx.row(fid_c); - - // Approach speeds (Eq. 9–10 in DAT paper) - // δ_v: how fast vertex is approaching triangle (negative n-component) - const double delta_v = std::max(-dx_v.dot(n), 0.0); - // δ_t: max approach speed of any triangle vertex toward vertex (Eq. 10) - const double delta_t = - std::max({ dx_a.dot(n), dx_b.dot(n), dx_c.dot(n), 0.0 }); - - // Adaptive λ: allocate gap proportional to approach speeds (Eq. 11) - const double lambda = (delta_v == 0.0 && delta_t == 0.0) - ? 0.5 - : delta_t / (delta_t + delta_v); - - // Division plane point: interpolated between c_vt and x_v - const Eigen::Vector3d p = c_vt + lambda * diff; - - // Truncate all four vertices using this division plane - t_v[fid_v] = - std::min(t_v[fid_v], planar_truncation_ratio(x_v, dx_v, n, p)); - t_v[fid_a] = - std::min(t_v[fid_a], planar_truncation_ratio(x_a, dx_a, n, p)); - t_v[fid_b] = - std::min(t_v[fid_b], planar_truncation_ratio(x_b, dx_b, n, p)); - t_v[fid_c] = - std::min(t_v[fid_c], planar_truncation_ratio(x_c, dx_c, n, p)); - } - - // ---- Edge-Edge Collisions ------------------------------------------ - for (const auto& ee : collisions.ee_collisions) { - const auto ids = ee.vertex_ids(mesh.edges(), mesh.faces()); - // ids = [ea0i, ea1i, eb0i, eb1i] — collision mesh indices - - const Eigen::Vector3d ea0 = vertices.row(ids[0]); - const Eigen::Vector3d ea1 = vertices.row(ids[1]); - const Eigen::Vector3d eb0 = vertices.row(ids[2]); - const Eigen::Vector3d eb1 = vertices.row(ids[3]); + for (size_t i = 0; i < candidates.size(); ++i) { + const CollisionStencil& c = candidates[i]; + const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); + const int nv = c.num_vertices(); + const VectorMax12d pos = c.dof(vertices, mesh.edges(), mesh.faces()); - // Closest points on each edge (dispatch on stored distance type) - const auto [c_e, c_ep] = - edge_edge_closest_points(ea0, ea1, eb0, eb1, ee.known_dtype()); - - // Normal pointing from ea toward eb - const Eigen::Vector3d diff = c_ep - c_e; - const double dist = diff.norm(); + // dv = c_first − c_second; positive coeffs → first primitive + VectorMax4d coeffs; + const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); + const double dist = dv.norm(); if (dist < 1e-10) { - continue; // Degenerate — skip pair + continue; } - const Eigen::Vector3d n = diff / dist; - - const int fid_ea0 = to_full(ids[0]); - const int fid_ea1 = to_full(ids[1]); - const int fid_eb0 = to_full(ids[2]); - const int fid_eb1 = to_full(ids[3]); - - const Eigen::Vector3d dx_ea0 = dx.row(fid_ea0); - const Eigen::Vector3d dx_ea1 = dx.row(fid_ea1); - const Eigen::Vector3d dx_eb0 = dx.row(fid_eb0); - const Eigen::Vector3d dx_eb1 = dx.row(fid_eb1); - - // δ_e: max approach speed of ea toward eb in +n direction (Eq. 12) - const double delta_e = std::max({ dx_ea0.dot(n), dx_ea1.dot(n), 0.0 }); - // δ_e': max approach speed of eb toward ea in −n direction (Eq. 12) - const double delta_ep = - std::max({ -dx_eb0.dot(n), -dx_eb1.dot(n), 0.0 }); - - // Adaptive λ (Eq. 12 in DAT paper) - const double lambda = (delta_e == 0.0 && delta_ep == 0.0) - ? 0.5 - : delta_ep / (delta_e + delta_ep); - - // Division plane point: interpolated between c_e and c_ep - const Eigen::Vector3d p = c_e + lambda * diff; - - // Truncate all four edge vertices - t_v[fid_ea0] = - std::min(t_v[fid_ea0], planar_truncation_ratio(ea0, dx_ea0, n, p)); - t_v[fid_ea1] = - std::min(t_v[fid_ea1], planar_truncation_ratio(ea1, dx_ea1, n, p)); - t_v[fid_eb0] = - std::min(t_v[fid_eb0], planar_truncation_ratio(eb0, dx_eb0, n, p)); - t_v[fid_eb1] = - std::min(t_v[fid_eb1], planar_truncation_ratio(eb1, dx_eb1, n, p)); - } - - // ---- Edge-Vertex Collisions ---------------------------------------- - // ids = [vi, e0i, e1i, -1] — collision mesh indices - for (const auto& ev : collisions.ev_collisions) { - const auto ids = ev.vertex_ids(mesh.edges(), mesh.faces()); - - const VectorMax3d x_v = vertices.row(ids[0]); - const VectorMax3d x_e0 = vertices.row(ids[1]); - const VectorMax3d x_e1 = vertices.row(ids[2]); - const double alpha = point_edge_closest_point(x_v, x_e0, x_e1); - assert(-1e-8 <= alpha && alpha <= (1 + 1e-8)); - const VectorMax3d c_e = x_e0 + alpha * (x_e1 - x_e0); - - // Normal pointing from edge toward vertex - const VectorMax3d diff = x_v - c_e; - const double dist = diff.norm(); - if (dist < 1e-10) { - continue; // Degenerate — skip pair + const VectorMax3d n = dv / dist; + + VectorMax3d c_first = VectorMax3d::Zero(d); + VectorMax3d c_second = VectorMax3d::Zero(d); + double delta_first = 0, delta_second = 0; + + for (int j = 0; j < nv; ++j) { + const int fid = to_full(ids[j]); + const VectorMax3d xj = pos.segment(j * d, d); + const VectorMax3d dxj = dx.row(fid); + if (coeffs[j] > 0) { + c_first += coeffs[j] * xj; + delta_first = std::max(delta_first, -dxj.dot(n)); + } else if (coeffs[j] < 0) { + c_second -= coeffs[j] * xj; + delta_second = std::max(delta_second, dxj.dot(n)); + } } - const VectorMax3d n = diff / dist; - - const int fid_v = to_full(ids[0]); - const int fid_e0 = to_full(ids[1]); - const int fid_e1 = to_full(ids[2]); + delta_first = std::max(delta_first, 0.0); + delta_second = std::max(delta_second, 0.0); - const VectorMax3d dx_v = dx.row(fid_v); - const VectorMax3d dx_e0 = dx.row(fid_e0); - const VectorMax3d dx_e1 = dx.row(fid_e1); - - // δ_v: vertex approaching edge (moving in −n direction) - const double delta_v = std::max(-dx_v.dot(n), 0.0); - // δ_e: max approach speed of any edge vertex toward vertex (in +n) - const double delta_e = std::max({ dx_e0.dot(n), dx_e1.dot(n), 0.0 }); - - const double lambda = (delta_v == 0.0 && delta_e == 0.0) + const double lambda = (delta_first == 0 && delta_second == 0) ? 0.5 - : delta_e / (delta_e + delta_v); - - const VectorMax3d p = c_e + lambda * diff; - - t_v[fid_v] = - std::min(t_v[fid_v], planar_truncation_ratio(x_v, dx_v, n, p)); - t_v[fid_e0] = - std::min(t_v[fid_e0], planar_truncation_ratio(x_e0, dx_e0, n, p)); - t_v[fid_e1] = - std::min(t_v[fid_e1], planar_truncation_ratio(x_e1, dx_e1, n, p)); - } - - // ---- Vertex-Vertex Collisions -------------------------------------- - // ids = [v0i, v1i, -1, -1] — collision mesh indices - for (const auto& vv : collisions.vv_collisions) { - const auto ids = vv.vertex_ids(mesh.edges(), mesh.faces()); - - const VectorMax3d x_a = vertices.row(ids[0]); - const VectorMax3d x_b = vertices.row(ids[1]); - - // Normal pointing from v0 toward v1 - const VectorMax3d diff = x_b - x_a; - const double dist = diff.norm(); - if (dist < 1e-10) { - continue; // Degenerate — skip pair + : delta_second / (delta_first + delta_second); + + // Division plane: p = c_second + λ·dv (λ=0 → plane at second prim, + // λ=1 → plane at first prim). + const VectorMax3d p = c_second + lambda * dv; + + for (int j = 0; j < nv; ++j) { + assert(ids[j] >= 0); // All candidates should be valid vertices + const int fid = to_full(ids[j]); + const VectorMax3d xj = pos.segment(j * d, d); + const VectorMax3d dxj = dx.row(fid); + t_v[fid] = + std::min(t_v[fid], planar_truncation_ratio(xj, dxj, n, p)); } - const VectorMax3d n = diff / dist; - - const int fid_a = to_full(ids[0]); - const int fid_b = to_full(ids[1]); - - const VectorMax3d dx_a = dx.row(fid_a); - const VectorMax3d dx_b = dx.row(fid_b); - - // δ_a: v0 approaching v1 (moving in +n); δ_b: v1 approaching v0 (-n) - const double delta_a = std::max(dx_a.dot(n), 0.0); - const double delta_b = std::max(-dx_b.dot(n), 0.0); - - const double lambda = (delta_a == 0.0 && delta_b == 0.0) - ? 0.5 - : delta_b / (delta_a + delta_b); - - const VectorMax3d p = x_a + lambda * diff; - - t_v[fid_a] = - std::min(t_v[fid_a], planar_truncation_ratio(x_a, dx_a, n, p)); - t_v[fid_b] = - std::min(t_v[fid_b], planar_truncation_ratio(x_b, dx_b, n, p)); } // ---- Isotropic Fallback -------------------------------------------- diff --git a/src/ipc/ogc/trust_region.hpp b/src/ipc/ogc/trust_region.hpp index 976e77c0f..259745029 100644 --- a/src/ipc/ogc/trust_region.hpp +++ b/src/ipc/ogc/trust_region.hpp @@ -14,6 +14,10 @@ struct TrustRegion { /// @brief Trust region radii for each vertex. Eigen::VectorXd trust_region_radii; + /// @brief Collision candidates used for Planar-DAT. + /// Updated by update(); passed to planar_filter_step(). + Candidates candidates; + /// @brief Scaling factor for relaxing the trust region radii. /// @note This should be in (0, 1). /// @note This is referred to as \f\(2\gamma_p\f\) in the paper. @@ -100,23 +104,25 @@ struct TrustRegion { /// @brief Filter the optimization step dx using Planar-DAT (Divide and Truncate). /// - /// For each collision pair, computes a direction-aware division plane and - /// truncates only the component of displacement toward that plane. This + /// For each collision candidate, computes a direction-aware division plane + /// and truncates only the component of displacement toward that plane. This /// eliminates the artificial damping and deadlock of the isotropic /// filter_step while retaining the penetration-free guarantee. /// + /// Uses CollisionStencil::compute_distance_vector() to obtain the contact + /// normal and closest-point coefficients in a single polymorphic call, + /// avoiding stale distance-type decompositions from NormalCollisions. + /// /// @see "Divide and Truncate: A Penetration and Inversion Free Framework /// for Coupled Multi-physics Systems" [ACM SIGGRAPH 2026] /// /// @param mesh The collision mesh. /// @param x Current vertex positions. /// @param dx Proposed vertex displacements (modified in-place). - /// @param collisions Active collision pairs (e.g. from update()). void planar_filter_step( const CollisionMesh& mesh, Eigen::ConstRef x, - Eigen::Ref dx, - const NormalCollisions& collisions); + Eigen::Ref dx); private: /// @brief Compute the truncation ratio for a vertex given a division plane. diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 391db6a4d..e23ec8879 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -216,7 +216,7 @@ TEST_CASE( // exceeds the trust region inflation radius (2 * dhat = 1.2). dx.row(3) << 0.0, 2.0, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // The planar constraint leaves the step alone (moving away), but the // isotropic trust-region fallback clamps the displacement to the @@ -260,7 +260,7 @@ TEST_CASE( // (2 * dhat = 1.2). dx.row(3) << 2.0, 0.0, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // Tangential motion: Planar-DAT does not truncate, but the isotropic // trust-region fallback caps the displacement to the inflation radius. @@ -300,7 +300,7 @@ TEST_CASE( // Vertex 3 approaches the triangle (would overshoot if not truncated) dx.row(3) << 0.0, -0.6, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // After truncation the vertex must still be above y=0 (no penetration) const double final_y = x(3, 1) + dx(3, 1); @@ -346,7 +346,7 @@ TEST_CASE( // inflation radius → full step preserved Eigen::MatrixXd dx_planar = Eigen::MatrixXd::Zero(V.rows(), 3); dx_planar.row(3) << 0.0, 0.3, 0.0; - tr.planar_filter_step(mesh, x, dx_planar, collisions); + tr.planar_filter_step(mesh, x, dx_planar); CHECK(dx_planar(3, 1) == Catch::Approx(0.3)); // Isotropic: clips to trust_region_radii(3) < 0.3 @@ -429,7 +429,7 @@ TEST_CASE( dx.row(2) << 0.0, -0.4, 0.0; dx.row(3) << 0.0, -0.4, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // After truncation, no penetration: gap between edges must remain > 0 const double ea_y = x(0, 1) + dx(0, 1); // both ea vertices move same @@ -454,7 +454,7 @@ TEST_CASE( ipc::NormalCollisions::CollisionSetType::OGC); tr.update(mesh, V, collisions); - REQUIRE(!collisions.vv_collisions.empty()); + REQUIRE(!tr.candidates.vv_candidates.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); @@ -462,7 +462,7 @@ TEST_CASE( dx.row(0) << 0.0, 0.4, 0.0; dx.row(1) << 0.0, -0.4, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // After truncation gap must remain positive const double final_gap = (x(1, 1) + dx(1, 1)) - (x(0, 1) + dx(0, 1)); @@ -486,14 +486,14 @@ TEST_CASE( ipc::NormalCollisions::CollisionSetType::OGC); tr.update(mesh, V, collisions); - REQUIRE(!collisions.ev_collisions.empty()); + REQUIRE(!tr.candidates.ev_candidates.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); // Vertex approaches edge (downward), edge stationary dx.row(2) << 0.0, -0.6, 0.0; - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // After truncation vertex must remain above edge (y > 0) const double final_y = x(2, 1) + dx(2, 1); @@ -505,7 +505,7 @@ TEST_CASE( // constraint (only the isotropic fallback applies if needed) Eigen::MatrixXd dx_tan = Eigen::MatrixXd::Zero(V.rows(), 3); dx_tan.row(2) << 0.3, 0.0, 0.0; // tangential (within inflation radius) - tr.planar_filter_step(mesh, x, dx_tan, collisions); + tr.planar_filter_step(mesh, x, dx_tan); CHECK(dx_tan(2, 0) == Catch::Approx(0.3)); // not truncated } @@ -530,7 +530,7 @@ TEST_CASE( ipc::NormalCollisions::CollisionSetType::OGC); tr.update(mesh, V, collisions); - REQUIRE(!collisions.fv_collisions.empty()); + REQUIRE(!tr.candidates.fv_candidates.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); @@ -544,7 +544,7 @@ TEST_CASE( dx.row(2) << 0.0, -retreat, 0.0; dx.row(3) << 0.0, -approach, 0.0; // P towards triangle - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); // A, B, C move away from the division plane (t_i < 0) → no truncation. CHECK(dx(0, 1) == Catch::Approx(-retreat)); @@ -574,7 +574,7 @@ TEST_CASE( ipc::NormalCollisions::CollisionSetType::OGC); tr.update(mesh, V, collisions); - REQUIRE(!collisions.ev_collisions.empty()); + REQUIRE(!tr.candidates.ev_candidates.empty()); Eigen::MatrixXd x = V; Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 2); @@ -582,7 +582,7 @@ TEST_CASE( dx.row(2) << 0.0, -0.6; // Must not crash and must truncate the approaching vertex - tr.planar_filter_step(mesh, x, dx, collisions); + tr.planar_filter_step(mesh, x, dx); const double final_y = x(2, 1) + dx(2, 1); CHECK(final_y > 0.0); From 26bc75d0f1cd1cc9691d0543e8ee4d16578117a2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 25 Apr 2026 22:06:01 -0400 Subject: [PATCH 09/13] Parallelize trust region filters with TBB for improved performance - Replace serial loops in warm_start_time_step, filter_step, and planar_filter_step with tbb::parallel_for for per-vertex and per-candidate operations - Use std::atomic for thread-safe counters and min-reduction - No changes to algorithm logic; improves scalability on large meshes Co-Authored-By: Claude Sonnet 4.6 --- src/ipc/ogc/trust_region.cpp | 253 ++++++++++++++++++++--------------- 1 file changed, 147 insertions(+), 106 deletions(-) diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index b833a478b..86ad09ac7 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,5 +1,10 @@ #include "trust_region.hpp" +#include +#include + +#include + namespace ipc::ogc { TrustRegion::TrustRegion(double dhat) : trust_region_inflation_radius(2 * dhat) @@ -37,20 +42,26 @@ void TrustRegion::warm_start_time_step( update(mesh, x, collisions, min_distance, broad_phase); should_update_trust_region = false; - int num_updates = 0; - for (int i = 0; i < x.rows(); i++) { - const VectorMax3d x_i = x.row(i); - const VectorMax3d pred_x_i = pred_x.row(i); - - if (dx_norm(i) <= trust_region_radii(i)) { - x.row(i) = pred_x_i; // Free to move to the predicted position - } else { - // Move to the boundary of the trust region - x.row(i) = - x_i + (trust_region_radii(i) / dx_norm(i)) * (pred_x_i - x_i); - ++num_updates; - } - } + std::atomic num_updates(0); + tbb::parallel_for( + tbb::blocked_range(0, static_cast(x.rows())), + [&](const tbb::blocked_range& range) { + for (int i = range.begin(); i < range.end(); i++) { + const VectorMax3d x_i = x.row(i); + const VectorMax3d pred_x_i = pred_x.row(i); + + if (dx_norm(i) <= trust_region_radii(i)) { + x.row(i) = pred_x_i; // Free to move to the predicted + // position + } else { + // Move to the boundary of the trust region + x.row(i) = x_i + + (trust_region_radii(i) / dx_norm(i)) + * (pred_x_i - x_i); + ++num_updates; + } + } + }); // If a critical mass of vertices are restricted by the trust region, // we update the trust region to avoid excessive filtering. @@ -174,26 +185,30 @@ void TrustRegion::filter_step( { assert(x.rows() == dx.rows() && x.cols() == dx.cols()); - int num_updates = 0; - for (int i = 0; i < x.rows(); i++) { - const VectorMax3d ci = trust_region_centers.row(i); - const VectorMax3d xi = x.row(i); - const VectorMax3d dxi = dx.row(i); - - // Check that the current position is within the trust region. - assert((xi - ci).norm() <= trust_region_radii(i) + 1e-9); - - const double beta = - compute_trust_region_beta(xi, dxi, ci, trust_region_radii(i)); - if (beta < 1.0) { - dx.row(i).array() *= beta; - assert(approx( - (xi + dx.row(i).transpose() - ci).norm(), - trust_region_radii(i))); - - num_updates++; - } - } + std::atomic num_updates(0); + tbb::parallel_for( + tbb::blocked_range(0, static_cast(x.rows())), + [&](const tbb::blocked_range& range) { + for (int i = range.begin(); i < range.end(); i++) { + const VectorMax3d ci = trust_region_centers.row(i); + const VectorMax3d xi = x.row(i); + const VectorMax3d dxi = dx.row(i); + + // Check that the current position is within the trust region. + assert((xi - ci).norm() <= trust_region_radii(i) + 1e-9); + + const double beta = compute_trust_region_beta( + xi, dxi, ci, trust_region_radii(i)); + if (beta < 1.0) { + dx.row(i).array() *= beta; + assert(approx( + (xi + dx.row(i).transpose() - ci).norm(), + trust_region_radii(i))); + + ++num_updates; + } + } + }); // If a critical mass of vertices are restricted by the trust region, // we update the trust region to avoid excessive filtering. @@ -231,87 +246,113 @@ void TrustRegion::planar_filter_step( : static_cast(coll_id); }; - // Per-vertex truncation ratios (1 = no truncation) - Eigen::VectorXd t_v = Eigen::VectorXd::Ones(x.rows()); - - for (size_t i = 0; i < candidates.size(); ++i) { - const CollisionStencil& c = candidates[i]; - const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); - const int nv = c.num_vertices(); - const VectorMax12d pos = c.dof(vertices, mesh.edges(), mesh.faces()); - - // dv = c_first − c_second; positive coeffs → first primitive - VectorMax4d coeffs; - const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); - const double dist = dv.norm(); - if (dist < 1e-10) { - continue; - } + // Per-vertex truncation ratios (1 = no truncation). + // Use atomics to allow concurrent min-reduction across candidates. + std::vector> t_v_atomic(x.rows()); + for (int i = 0; i < x.rows(); i++) { + t_v_atomic[i].store(1.0, std::memory_order_relaxed); + } - const VectorMax3d n = dv / dist; - - VectorMax3d c_first = VectorMax3d::Zero(d); - VectorMax3d c_second = VectorMax3d::Zero(d); - double delta_first = 0, delta_second = 0; - - for (int j = 0; j < nv; ++j) { - const int fid = to_full(ids[j]); - const VectorMax3d xj = pos.segment(j * d, d); - const VectorMax3d dxj = dx.row(fid); - if (coeffs[j] > 0) { - c_first += coeffs[j] * xj; - delta_first = std::max(delta_first, -dxj.dot(n)); - } else if (coeffs[j] < 0) { - c_second -= coeffs[j] * xj; - delta_second = std::max(delta_second, dxj.dot(n)); + tbb::parallel_for( + tbb::blocked_range(0, candidates.size()), + [&](const tbb::blocked_range& range) { + for (size_t i = range.begin(); i < range.end(); i++) { + const CollisionStencil& c = candidates[i]; + const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); + const int nv = c.num_vertices(); + const VectorMax12d pos = + c.dof(vertices, mesh.edges(), mesh.faces()); + + // dv = c_first − c_second; positive coeffs → first primitive + VectorMax4d coeffs; + const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); + const double dist = dv.norm(); + if (dist < 1e-10) { + continue; + } + + const VectorMax3d n = dv / dist; + + VectorMax3d c_first = VectorMax3d::Zero(d); + VectorMax3d c_second = VectorMax3d::Zero(d); + double delta_first = 0, delta_second = 0; + + for (int j = 0; j < nv; ++j) { + const int fid = to_full(ids[j]); + Eigen::ConstRef xj = pos.segment(j * d, d); + Eigen::ConstRef dxj = dx.row(fid); + if (coeffs[j] > 0) { + c_first += coeffs[j] * xj; + delta_first = std::max(delta_first, -dxj.dot(n)); + } else if (coeffs[j] < 0) { + c_second -= coeffs[j] * xj; + delta_second = std::max(delta_second, dxj.dot(n)); + } + } + delta_first = std::max(delta_first, 0.0); + delta_second = std::max(delta_second, 0.0); + + const double lambda = (delta_first == 0 && delta_second == 0) + ? 0.5 + : delta_second / (delta_first + delta_second); + + // Division plane: p = c_second + λ·dv (λ=0 → plane at + // second prim, λ=1 → plane at first prim). + const VectorMax3d p = c_second + lambda * dv; + + for (int j = 0; j < nv; ++j) { + assert(ids[j] >= 0); // All candidates should be valid + const int fid = to_full(ids[j]); + Eigen::ConstRef xj = pos.segment(j * d, d); + Eigen::ConstRef dxj = dx.row(fid); + const double t_val = planar_truncation_ratio(xj, dxj, n, p); + double old_val = + t_v_atomic[fid].load(std::memory_order_relaxed); + while (t_val < old_val + && !t_v_atomic[fid].compare_exchange_weak( + old_val, t_val, std::memory_order_relaxed)) { } + } } - } - delta_first = std::max(delta_first, 0.0); - delta_second = std::max(delta_second, 0.0); - - const double lambda = (delta_first == 0 && delta_second == 0) - ? 0.5 - : delta_second / (delta_first + delta_second); - - // Division plane: p = c_second + λ·dv (λ=0 → plane at second prim, - // λ=1 → plane at first prim). - const VectorMax3d p = c_second + lambda * dv; - - for (int j = 0; j < nv; ++j) { - assert(ids[j] >= 0); // All candidates should be valid vertices - const int fid = to_full(ids[j]); - const VectorMax3d xj = pos.segment(j * d, d); - const VectorMax3d dxj = dx.row(fid); - t_v[fid] = - std::min(t_v[fid], planar_truncation_ratio(xj, dxj, n, p)); - } + }); + + // Convert atomics to a plain vector for the subsequent serial passes. + Eigen::VectorXd t_v(x.rows()); + for (int i = 0; i < x.rows(); i++) { + t_v[i] = t_v_atomic[i].load(std::memory_order_relaxed); } // ---- Isotropic Fallback -------------------------------------------- // For primitives beyond the query radius, fall back to isotropic // trust-region clamping (same as filter_step) to prevent penetration. - for (int i = 0; i < x.rows(); i++) { - const VectorMax3d xi = x.row(i); - const VectorMax3d dxi = dx.row(i); - const VectorMax3d ci = trust_region_centers.row(i); - // Use trust_region_inflation_radius instead of trust_region_radii(v). - // This is the key difference between Planar-DAT and Isotropic-DAT. - // This allows us to preserve more motion for vertices that are near the - // boundary of the trust region, rather than restricting them to a - // smaller trust region radius. - const double beta = compute_trust_region_beta( - xi, dxi, ci, trust_region_inflation_radius); - t_v[i] = std::min(t_v[i], beta); - } + tbb::parallel_for( + tbb::blocked_range(0, static_cast(x.rows())), + [&](const tbb::blocked_range& range) { + for (int i = range.begin(); i < range.end(); i++) { + // Use trust_region_inflation_radius instead of + // trust_region_radii(v). This is the key difference between + // Planar-DAT and Isotropic-DAT. This allows us to preserve + // more motion for vertices that are near the boundary of the + // trust region, rather than restricting them to a smaller + // trust region radius. + const double beta = compute_trust_region_beta( + x.row(i), dx.row(i), trust_region_centers.row(i), + trust_region_inflation_radius); + t_v[i] = std::min(t_v[i], beta); + } + }); // ---- Apply Truncations --------------------------------------------- - int num_updates = 0; - for (int v = 0; v < x.rows(); v++) { - if (t_v[v] < 1.0) { - dx.row(v) *= t_v[v]; - num_updates++; - } - } + std::atomic num_updates(0); + tbb::parallel_for( + tbb::blocked_range(0, static_cast(x.rows())), + [&](const tbb::blocked_range& range) { + for (int v = range.begin(); v < range.end(); v++) { + if (t_v[v] < 1.0) { + dx.row(v) *= t_v[v]; + ++num_updates; + } + } + }); // Mirror filter_step: if a critical mass of vertices are restricted, // flag the trust region for re-centering on the next iteration. From 9305c88dfad27d2eac917e723724ea5833ae4234 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 29 Apr 2026 16:34:04 -0400 Subject: [PATCH 10/13] Refactor tbb::parallel_for to use range-based overloads - Replace tbb::blocked_range-based parallel_for calls with simpler range-based overloads throughout the codebase - Remove unnecessary #include from affected files - Improves code clarity and reduces boilerplate in parallel loops --- src/ipc/broad_phase/aabb.cpp | 58 ++--- src/ipc/candidates/candidates.cpp | 103 ++++---- src/ipc/collision_mesh.cpp | 17 +- src/ipc/ogc/trust_region.cpp | 233 ++++++++---------- src/ipc/potentials/normal_potential.cpp | 47 ++-- src/ipc/potentials/potential.cpp | 50 ++-- src/ipc/potentials/tangential_potential.cpp | 192 ++++++--------- src/ipc/smooth_contact/smooth_collisions.cpp | 34 +-- .../smooth_contact_potential.cpp | 59 ++--- 9 files changed, 322 insertions(+), 471 deletions(-) diff --git a/src/ipc/broad_phase/aabb.cpp b/src/ipc/broad_phase/aabb.cpp index 8a52f75dc..4716988a4 100644 --- a/src/ipc/broad_phase/aabb.cpp +++ b/src/ipc/broad_phase/aabb.cpp @@ -2,7 +2,6 @@ #include -#include #include #include @@ -64,15 +63,10 @@ void build_vertex_boxes( vertex_boxes.resize(vertices.rows()); - tbb::parallel_for( - tbb::blocked_range(0, vertices.rows()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - vertex_boxes[i] = - AABB::from_point(vertices.row(i), inflation_radius); - vertex_boxes[i].vertex_ids = { { index_t(i), -1, -1 } }; - } - }); + tbb::parallel_for(0, index_t(vertices.rows()), [&](index_t i) { + vertex_boxes[i] = AABB::from_point(vertices.row(i), inflation_radius); + vertex_boxes[i].vertex_ids = { { i, -1, -1 } }; + }); } void build_vertex_boxes( @@ -85,15 +79,11 @@ void build_vertex_boxes( vertex_boxes.resize(vertices_t0.rows()); - tbb::parallel_for( - tbb::blocked_range(0, vertices_t0.rows()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - vertex_boxes[i] = AABB::from_point( - vertices_t0.row(i), vertices_t1.row(i), inflation_radius); - vertex_boxes[i].vertex_ids = { { index_t(i), -1, -1 } }; - } - }); + tbb::parallel_for(0, index_t(vertices_t0.rows()), [&](index_t i) { + vertex_boxes[i] = AABB::from_point( + vertices_t0.row(i), vertices_t1.row(i), inflation_radius); + vertex_boxes[i].vertex_ids = { { i, -1, -1 } }; + }); } void build_edge_boxes( @@ -108,15 +98,11 @@ void build_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++) { - 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 } }; - } - }); + tbb::parallel_for(0, index_t(edges.rows()), [&](index_t i) { + 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( @@ -131,16 +117,12 @@ void build_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++) { - 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 } }; - } - }); + tbb::parallel_for(0, index_t(faces.rows()), [&](index_t i) { + 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 } }; + }); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 8f23e102a..060edea16 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -262,29 +262,25 @@ double Candidates::compute_collision_free_stepsize( std::atomic earliest_toi(1.0); - tbb::parallel_for( - tbb::blocked_range(0, size()), - [&](tbb::blocked_range r) { - for (size_t i = r.begin(); i < r.end(); i++) { - double tmax = earliest_toi.load(std::memory_order_relaxed); - - const CollisionStencil& candidate = (*this)[i]; - - double toi = std::numeric_limits::infinity(); // output - const bool are_colliding = candidate.ccd( - candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), - candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // - toi, min_distance, tmax, narrow_phase_ccd); - - if (are_colliding) { - // Update the earliest time of impact (TOI) atomically - double prev = earliest_toi.load(std::memory_order_relaxed); - while (toi < prev - && !earliest_toi.compare_exchange_weak( - prev, toi, std::memory_order_relaxed)) { } - } - } - }); + tbb::parallel_for(size_t(0), size(), [&](size_t i) { + double tmax = earliest_toi.load(std::memory_order_relaxed); + + const CollisionStencil& candidate = (*this)[i]; + + double toi = std::numeric_limits::infinity(); // output + const bool are_colliding = candidate.ccd( + candidate.dof(vertices_t0, mesh.edges(), mesh.faces()), + candidate.dof(vertices_t1, mesh.edges(), mesh.faces()), // + toi, min_distance, tmax, narrow_phase_ccd); + + if (are_colliding) { + // Update the earliest time of impact (TOI) atomically + double prev = earliest_toi.load(std::memory_order_relaxed); + while (toi < prev + && !earliest_toi.compare_exchange_weak( + prev, toi, std::memory_order_relaxed)) { } + } + }); double result = earliest_toi.load(std::memory_order_relaxed); assert(result >= 0 && result <= 1.0); @@ -310,19 +306,14 @@ double Candidates::compute_noncandidate_conservative_stepsize( is_vertex_a_candidates[i].store(false, std::memory_order_relaxed); } - tbb::parallel_for( - tbb::blocked_range(0, size()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); ++i) { - for (const index_t vid : (*this)[i].vertex_ids(E, F)) { - if (vid < 0) { - break; - } - is_vertex_a_candidates[vid].store( - true, std::memory_order_relaxed); - } + tbb::parallel_for(size_t(0), size(), [&](size_t i) { + for (const index_t vid : (*this)[i].vertex_ids(E, F)) { + if (vid < 0) { + break; } - }); + is_vertex_a_candidates[vid].store(true, std::memory_order_relaxed); + } + }); double max_displacement = tbb::parallel_reduce( tbb::blocked_range(0, displacements.rows()), 0.0, @@ -382,31 +373,25 @@ Eigen::VectorXd Candidates::compute_per_vertex_safe_distances( inflation_radius - min_distance, std::memory_order_relaxed); } - tbb::parallel_for( - tbb::blocked_range(0, size()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - const CollisionStencil& candidate = (*this)[i]; - - const double d = sqrt(candidate.compute_distance( - vertices, mesh.edges(), mesh.faces())) - - min_distance; - - // Compute the distance for each vertex in the candidate - for (const index_t vid : - candidate.vertex_ids(mesh.edges(), mesh.faces())) { - if (vid < 0) { - break; // No more vertices in this candidate - } - // Update the minimum distance atomically - double old_val = - min_distances[vid].load(std::memory_order_relaxed); - while (d < old_val - && !min_distances[vid].compare_exchange_weak( - old_val, d, std::memory_order_relaxed)) { } - } + tbb::parallel_for(size_t(0), size(), [&](size_t i) { + const CollisionStencil& candidate = (*this)[i]; + + const double d = sqrt(candidate.compute_distance( + vertices, mesh.edges(), mesh.faces())) + - min_distance; + + // Compute the distance for each vertex in the candidate + for (auto vid : candidate.vertex_ids(mesh.edges(), mesh.faces())) { + if (vid < 0) { + break; // No more vertices in this candidate } - }); + // Update the minimum distance atomically + double old_val = min_distances[vid].load(std::memory_order_relaxed); + while (d < old_val + && !min_distances[vid].compare_exchange_weak( + old_val, d, std::memory_order_relaxed)) { } + } + }); // Convert atomic distances to a vector Eigen::VectorXd result(mesh.num_vertices()); diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 217ad0c5b..23c1a56c6 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -6,7 +6,6 @@ #include #include -#include #include #include @@ -235,16 +234,12 @@ namespace { void remove_duplicates(std::vector>& v) { - tbb::parallel_for( - tbb::blocked_range(0, v.size()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - std::sort(v[i].begin(), v[i].end()); - auto last = std::unique(v[i].begin(), v[i].end()); - v[i].erase(last, v[i].end()); - v[i].shrink_to_fit(); - } - }); + tbb::parallel_for(size_t(0), v.size(), [&](size_t i) { + std::sort(v[i].begin(), v[i].end()); + auto last = std::unique(v[i].begin(), v[i].end()); + v[i].erase(last, v[i].end()); + v[i].shrink_to_fit(); + }); } } // namespace diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index 86ad09ac7..56402412b 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,6 +1,5 @@ #include "trust_region.hpp" -#include #include #include @@ -43,25 +42,19 @@ void TrustRegion::warm_start_time_step( should_update_trust_region = false; std::atomic num_updates(0); - tbb::parallel_for( - tbb::blocked_range(0, static_cast(x.rows())), - [&](const tbb::blocked_range& range) { - for (int i = range.begin(); i < range.end(); i++) { - const VectorMax3d x_i = x.row(i); - const VectorMax3d pred_x_i = pred_x.row(i); - - if (dx_norm(i) <= trust_region_radii(i)) { - x.row(i) = pred_x_i; // Free to move to the predicted - // position - } else { - // Move to the boundary of the trust region - x.row(i) = x_i - + (trust_region_radii(i) / dx_norm(i)) - * (pred_x_i - x_i); - ++num_updates; - } - } - }); + tbb::parallel_for(0, static_cast(x.rows()), [&](int i) { + const VectorMax3d x_i = x.row(i); + const VectorMax3d pred_x_i = pred_x.row(i); + + if (dx_norm(i) <= trust_region_radii(i)) { + x.row(i) = pred_x_i; // Free to move to the predicted position + } else { + // Move to the boundary of the trust region + x.row(i) = + x_i + (trust_region_radii(i) / dx_norm(i)) * (pred_x_i - x_i); + num_updates.fetch_add(1, std::memory_order_relaxed); + } + }); // If a critical mass of vertices are restricted by the trust region, // we update the trust region to avoid excessive filtering. @@ -186,29 +179,25 @@ void TrustRegion::filter_step( assert(x.rows() == dx.rows() && x.cols() == dx.cols()); std::atomic num_updates(0); - tbb::parallel_for( - tbb::blocked_range(0, static_cast(x.rows())), - [&](const tbb::blocked_range& range) { - for (int i = range.begin(); i < range.end(); i++) { - const VectorMax3d ci = trust_region_centers.row(i); - const VectorMax3d xi = x.row(i); - const VectorMax3d dxi = dx.row(i); - - // Check that the current position is within the trust region. - assert((xi - ci).norm() <= trust_region_radii(i) + 1e-9); - - const double beta = compute_trust_region_beta( - xi, dxi, ci, trust_region_radii(i)); - if (beta < 1.0) { - dx.row(i).array() *= beta; - assert(approx( - (xi + dx.row(i).transpose() - ci).norm(), - trust_region_radii(i))); - - ++num_updates; - } - } - }); + tbb::parallel_for(0, static_cast(x.rows()), [&](int i) { + const VectorMax3d ci = trust_region_centers.row(i); + const VectorMax3d xi = x.row(i); + const VectorMax3d dxi = dx.row(i); + + // Check that the current position is within the trust region. + assert((xi - ci).norm() <= trust_region_radii(i) + 1e-9); + + const double beta = + compute_trust_region_beta(xi, dxi, ci, trust_region_radii(i)); + if (beta < 1.0) { + dx.row(i).array() *= beta; + assert(approx( + (xi + dx.row(i).transpose() - ci).norm(), + trust_region_radii(i))); + + num_updates.fetch_add(1, std::memory_order_relaxed); + } + }); // If a critical mass of vertices are restricted by the trust region, // we update the trust region to avoid excessive filtering. @@ -253,67 +242,61 @@ void TrustRegion::planar_filter_step( t_v_atomic[i].store(1.0, std::memory_order_relaxed); } - tbb::parallel_for( - tbb::blocked_range(0, candidates.size()), - [&](const tbb::blocked_range& range) { - for (size_t i = range.begin(); i < range.end(); i++) { - const CollisionStencil& c = candidates[i]; - const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); - const int nv = c.num_vertices(); - const VectorMax12d pos = - c.dof(vertices, mesh.edges(), mesh.faces()); - - // dv = c_first − c_second; positive coeffs → first primitive - VectorMax4d coeffs; - const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); - const double dist = dv.norm(); - if (dist < 1e-10) { - continue; - } - - const VectorMax3d n = dv / dist; - - VectorMax3d c_first = VectorMax3d::Zero(d); - VectorMax3d c_second = VectorMax3d::Zero(d); - double delta_first = 0, delta_second = 0; - - for (int j = 0; j < nv; ++j) { - const int fid = to_full(ids[j]); - Eigen::ConstRef xj = pos.segment(j * d, d); - Eigen::ConstRef dxj = dx.row(fid); - if (coeffs[j] > 0) { - c_first += coeffs[j] * xj; - delta_first = std::max(delta_first, -dxj.dot(n)); - } else if (coeffs[j] < 0) { - c_second -= coeffs[j] * xj; - delta_second = std::max(delta_second, dxj.dot(n)); - } - } - delta_first = std::max(delta_first, 0.0); - delta_second = std::max(delta_second, 0.0); - - const double lambda = (delta_first == 0 && delta_second == 0) - ? 0.5 - : delta_second / (delta_first + delta_second); - - // Division plane: p = c_second + λ·dv (λ=0 → plane at - // second prim, λ=1 → plane at first prim). - const VectorMax3d p = c_second + lambda * dv; - - for (int j = 0; j < nv; ++j) { - assert(ids[j] >= 0); // All candidates should be valid - const int fid = to_full(ids[j]); - Eigen::ConstRef xj = pos.segment(j * d, d); - Eigen::ConstRef dxj = dx.row(fid); - const double t_val = planar_truncation_ratio(xj, dxj, n, p); - double old_val = - t_v_atomic[fid].load(std::memory_order_relaxed); - while (t_val < old_val - && !t_v_atomic[fid].compare_exchange_weak( - old_val, t_val, std::memory_order_relaxed)) { } - } + tbb::parallel_for(size_t(0), candidates.size(), [&](size_t i) { + const CollisionStencil& c = candidates[i]; + const auto ids = c.vertex_ids(mesh.edges(), mesh.faces()); + const int nv = c.num_vertices(); + const VectorMax12d pos = c.dof(vertices, mesh.edges(), mesh.faces()); + + // dv = c_first − c_second; positive coeffs → first primitive + VectorMax4d coeffs; + const VectorMax3d dv = c.compute_distance_vector(pos, coeffs); + const double dist = dv.norm(); + if (dist < 1e-10) { + return; + } + + const VectorMax3d n = dv / dist; + + VectorMax3d c_first = VectorMax3d::Zero(d); + VectorMax3d c_second = VectorMax3d::Zero(d); + double delta_first = 0, delta_second = 0; + + for (int j = 0; j < nv; ++j) { + const int fid = to_full(ids[j]); + Eigen::ConstRef xj = pos.segment(j * d, d); + Eigen::ConstRef dxj = dx.row(fid); + if (coeffs[j] > 0) { + c_first += coeffs[j] * xj; + delta_first = std::max(delta_first, -dxj.dot(n)); + } else if (coeffs[j] < 0) { + c_second -= coeffs[j] * xj; + delta_second = std::max(delta_second, dxj.dot(n)); } - }); + } + delta_first = std::max(delta_first, 0.0); + delta_second = std::max(delta_second, 0.0); + + const double lambda = (delta_first == 0 && delta_second == 0) + ? 0.5 + : delta_second / (delta_first + delta_second); + + // Division plane: p = c_second + λ·dv (λ=0 → plane at + // second prim, λ=1 → plane at first prim). + const VectorMax3d p = c_second + lambda * dv; + + for (int j = 0; j < nv; ++j) { + assert(ids[j] >= 0); // All candidates should be valid + const int fid = to_full(ids[j]); + Eigen::ConstRef xj = pos.segment(j * d, d); + Eigen::ConstRef dxj = dx.row(fid); + const double t_val = planar_truncation_ratio(xj, dxj, n, p); + double old_val = t_v_atomic[fid].load(std::memory_order_relaxed); + while (t_val < old_val + && !t_v_atomic[fid].compare_exchange_weak( + old_val, t_val, std::memory_order_relaxed)) { } + } + }); // Convert atomics to a plain vector for the subsequent serial passes. Eigen::VectorXd t_v(x.rows()); @@ -324,35 +307,27 @@ void TrustRegion::planar_filter_step( // ---- Isotropic Fallback -------------------------------------------- // For primitives beyond the query radius, fall back to isotropic // trust-region clamping (same as filter_step) to prevent penetration. - tbb::parallel_for( - tbb::blocked_range(0, static_cast(x.rows())), - [&](const tbb::blocked_range& range) { - for (int i = range.begin(); i < range.end(); i++) { - // Use trust_region_inflation_radius instead of - // trust_region_radii(v). This is the key difference between - // Planar-DAT and Isotropic-DAT. This allows us to preserve - // more motion for vertices that are near the boundary of the - // trust region, rather than restricting them to a smaller - // trust region radius. - const double beta = compute_trust_region_beta( - x.row(i), dx.row(i), trust_region_centers.row(i), - trust_region_inflation_radius); - t_v[i] = std::min(t_v[i], beta); - } - }); + tbb::parallel_for(0, static_cast(x.rows()), [&](int i) { + // Use trust_region_inflation_radius instead of + // trust_region_radii(v). This is the key difference between + // Planar-DAT and Isotropic-DAT. This allows us to preserve + // more motion for vertices that are near the boundary of the + // trust region, rather than restricting them to a smaller + // trust region radius. + const double beta = compute_trust_region_beta( + x.row(i), dx.row(i), trust_region_centers.row(i), + trust_region_inflation_radius); + t_v[i] = std::min(t_v[i], beta); + }); // ---- Apply Truncations --------------------------------------------- std::atomic num_updates(0); - tbb::parallel_for( - tbb::blocked_range(0, static_cast(x.rows())), - [&](const tbb::blocked_range& range) { - for (int v = range.begin(); v < range.end(); v++) { - if (t_v[v] < 1.0) { - dx.row(v) *= t_v[v]; - ++num_updates; - } - } - }); + tbb::parallel_for(0, static_cast(x.rows()), [&](int v) { + if (t_v[v] < 1.0) { + dx.row(v) *= t_v[v]; + num_updates.fetch_add(1, std::memory_order_relaxed); + } + }); // Mirror filter_step: if a critical mass of vertices are restricted, // flag the trust region for re-centering on the next iteration. diff --git a/src/ipc/potentials/normal_potential.cpp b/src/ipc/potentials/normal_potential.cpp index d079c0a9a..f194bf9c9 100644 --- a/src/ipc/potentials/normal_potential.cpp +++ b/src/ipc/potentials/normal_potential.cpp @@ -30,24 +30,19 @@ Eigen::VectorXd NormalPotential::gauss_newton_hessian_diagonal( tbb::combinable diag_storage( Eigen::VectorXd::Zero(vertices.size())); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - const NormalCollision& collision = collisions[i]; + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const NormalCollision& collision = collisions[i]; - const VectorMax12d local_diag = - this->gauss_newton_hessian_diagonal( - collision, collision.dof(vertices, edges, faces)); + const VectorMax12d local_diag = this->gauss_newton_hessian_diagonal( + collision, collision.dof(vertices, edges, faces)); - const auto vids = collision.vertex_ids(edges, faces); + const auto vids = collision.vertex_ids(edges, faces); - // Don't be confused by the "gradient" in the name -- this just - // scatters a local vector into a global vector. - local_gradient_to_global_gradient( - local_diag, vids, dim, diag_storage.local()); - } - }); + // Don't be confused by the "gradient" in the name -- this just + // scatters a local vector into a global vector. + local_gradient_to_global_gradient( + local_diag, vids, dim, diag_storage.local()); + }); return diag_storage.combine([](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }); @@ -107,19 +102,15 @@ Eigen::SparseMatrix NormalPotential::shape_derivative( tbb::enumerable_thread_specific>> storage; - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& local_triplets = storage.local(); - - for (size_t i = r.begin(); i < r.end(); i++) { - this->shape_derivative( - collisions[i], collisions[i].vertex_ids(edges, faces), - collisions[i].dof(rest_positions, edges, faces), - collisions[i].dof(vertices, edges, faces), local_triplets, - mesh.num_vertices()); - } - }); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const NormalCollision& collision = collisions[i]; + auto& local_triplets = storage.local(); + this->shape_derivative( + collision, collision.vertex_ids(edges, faces), + collision.dof(rest_positions, edges, faces), + collision.dof(vertices, edges, faces), local_triplets, + mesh.num_vertices()); + }); Eigen::SparseMatrix shape_derivative(ndof, ndof); for (const auto& local_triplets : storage) { diff --git a/src/ipc/potentials/potential.cpp b/src/ipc/potentials/potential.cpp index 44f1fbe91..548fc5af9 100644 --- a/src/ipc/potentials/potential.cpp +++ b/src/ipc/potentials/potential.cpp @@ -70,22 +70,16 @@ Eigen::VectorXd Potential::gradient( tbb::combinable grad(Eigen::VectorXd::Zero(X.size())); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i < r.end(); i++) { - const TCollision& collision = collisions[i]; - - const VectorMaxNd local_grad = this->gradient( - collision, collision.dof(X, mesh.edges(), mesh.faces())); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const TCollision& collision = collisions[i]; - const std::array vids = - collision.vertex_ids(mesh.edges(), mesh.faces()); + const VectorMaxNd local_grad = this->gradient( + collision, collision.dof(X, mesh.edges(), mesh.faces())); - local_gradient_to_global_gradient( - local_grad, vids, dim, grad.local()); - } - }); + local_gradient_to_global_gradient( + local_grad, collision.vertex_ids(mesh.edges(), mesh.faces()), dim, + grad.local()); + }); return grad.combine([](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; @@ -117,27 +111,19 @@ Eigen::SparseMatrix Potential::hessian( tbb::enumerable_thread_specific storage( LocalThreadMatStorage(buffer_size, ndof, ndof)); - tbb::parallel_for( - tbb::blocked_range(0, collisions.size()), - [&](const tbb::blocked_range& r) { - auto& hess_triplets = storage.local(); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + auto& hess_triplets = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { + const TCollision& collision = collisions[i]; - const TCollision& collision = collisions[i]; + const MatrixMaxNd local_hess = this->hessian( + collisions[i], collisions[i].dof(X, edges, faces), + project_hessian_to_psd); - const MatrixMaxNd local_hess = this->hessian( - collisions[i], collisions[i].dof(X, edges, faces), - project_hessian_to_psd); - - const std::array vids = - collision.vertex_ids(edges, faces); - - local_hessian_to_global_triplets( - local_hess, vids, dim, *(hess_triplets.cache), - mesh.num_vertices()); - } - }); + local_hessian_to_global_triplets( + local_hess, collision.vertex_ids(edges, faces), dim, + *(hess_triplets.cache), mesh.num_vertices()); + }); if (storage.empty()) { return Eigen::SparseMatrix(); diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index a761cfb5b..5c19c783a 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -58,26 +58,19 @@ Eigen::VectorXd TangentialPotential::force( tbb::combinable storage( Eigen::VectorXd::Zero(velocities.size())); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - Eigen::VectorXd& global_force = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { - const auto& collision = collisions[i]; - - const VectorMax12d local_force = force( - collision, collision.dof(rest_positions, edges, faces), - collision.dof(lagged_displacements, edges, faces), - collision.dof(velocities, edges, faces), // - normal_potential, dmin, no_mu); - - const std::array vis = - collision.vertex_ids(mesh.edges(), mesh.faces()); - - local_gradient_to_global_gradient( - local_force, vis, dim, global_force); - } - }); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const auto& collision = collisions[i]; + + const VectorMax12d local_force = force( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), // + normal_potential, dmin, no_mu); + + local_gradient_to_global_gradient( + local_force, collision.vertex_ids(mesh.edges(), mesh.faces()), dim, + storage.local()); + }); return storage.combine([](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }); @@ -105,28 +98,20 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( tbb::enumerable_thread_specific>> storage; - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& jac_triplets = storage.local(); - - for (size_t i = r.begin(); i < r.end(); i++) { - const TangentialCollision& collision = collisions[i]; - - const MatrixMax12d local_force_jacobian = force_jacobian( - collision, collision.dof(rest_positions, edges, faces), - collision.dof(lagged_displacements, edges, faces), - collision.dof(velocities, edges, faces), // - normal_potential, wrt, dmin); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const TangentialCollision& collision = collisions[i]; - const std::array vis = - collision.vertex_ids(mesh.edges(), mesh.faces()); + const MatrixMax12d local_force_jacobian = force_jacobian( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), // + normal_potential, wrt, dmin); - local_hessian_to_global_triplets( - local_force_jacobian, vis, dim, jac_triplets, - mesh.num_vertices()); - } - }); + local_hessian_to_global_triplets( + local_force_jacobian, + collision.vertex_ids(mesh.edges(), mesh.faces()), dim, + storage.local(), mesh.num_vertices()); + }); Eigen::SparseMatrix jacobian(velocities.size(), velocities.size()); for (const auto& local_jac_triplets : storage) { @@ -189,7 +174,6 @@ double TangentialPotential::operator()( const VectorMax2d u_aniso = collision.mu_aniso.head(u.size()).cwiseProduct(u); - const int tangent_dim = u.size(); double mu_s, mu_k; friction_mu_for_evaluation(collision, false, mu_s, mu_k); @@ -627,25 +611,18 @@ Eigen::VectorXd TangentialPotential::smooth_contact_force( tbb::enumerable_thread_specific storage( Eigen::VectorXd::Zero(velocities.size())); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - Eigen::VectorXd& global_force = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { - const auto& collision = collisions[i]; + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const auto& collision = collisions[i]; - const VectorMaxNd local_force = smooth_contact_force( - collision, collision.dof(rest_positions, edges, faces), - collision.dof(lagged_displacements, edges, faces), - collision.dof(velocities, edges, faces), no_mu); + const VectorMaxNd local_force = smooth_contact_force( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), no_mu); - const auto vis = - collision.vertex_ids(mesh.edges(), mesh.faces()); - - local_gradient_to_global_gradient( - local_force, vis, dim, global_force); - } - }); + local_gradient_to_global_gradient( + local_force, collision.vertex_ids(mesh.edges(), mesh.faces()), dim, + storage.local()); + }); return storage.combine([](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }); @@ -677,59 +654,49 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( const Eigen::MatrixXd lagged_positions = rest_positions + lagged_displacements; - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& jac_triplets = storage.local(); - - for (size_t i = r.begin(); i < r.end(); i++) { - const TangentialCollision& collision = collisions[i]; - - // This jacobian doesn't include the derivatives of normal - // contact force - const MatrixMaxNd local_force_jacobian = - collision.normal_force_magnitude - * smooth_contact_force_jacobian_unit( - collision, - collision.dof(lagged_positions, edges, faces), - collision.dof(velocities, edges, faces), wrt, false); - - const auto vis = - collision.vertex_ids(mesh.edges(), mesh.faces()); - - local_hessian_to_global_triplets( - local_force_jacobian, vis, dim, jac_triplets, - mesh.num_vertices()); - - if (wrt == DiffWRT::VELOCITIES) { - continue; - } - - // The term that includes derivatives of normal contact force - const VectorMaxNd local_force = smooth_contact_force( - collision, collision.dof(rest_positions, edges, faces), - collision.dof(lagged_displacements, edges, faces), - collision.dof(velocities, edges, faces), false, true); - - // normal_force_grad is the gradient of contact force norm - Eigen::VectorXd normal_force_grad; - std::vector cc_vert_ids; - Eigen::MatrixXd Xt = rest_positions + lagged_displacements; - auto cc = collision.smooth_collision; - const Eigen::VectorXd contact_grad = - cc->gradient(cc->dof(Xt), params); - const Eigen::MatrixXd contact_hess = - cc->hessian(cc->dof(Xt), params); - normal_force_grad = - (1 / contact_grad.norm()) * (contact_hess * contact_grad); - cc_vert_ids = cc->vertex_ids(); - - local_jacobian_to_global_triplets( - local_force * normal_force_grad.transpose(), vis, - cc_vert_ids, dim, jac_triplets, mesh.num_vertices(), - mesh.num_vertices()); - } - }); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + auto& jac_triplets = storage.local(); + + const TangentialCollision& collision = collisions[i]; + + // This jacobian doesn't include the derivatives of normal + // contact force + const MatrixMaxNd local_force_jacobian = + collision.normal_force_magnitude + * smooth_contact_force_jacobian_unit( + collision, collision.dof(lagged_positions, edges, faces), + collision.dof(velocities, edges, faces), wrt, false); + + const auto vids = collision.vertex_ids(mesh.edges(), mesh.faces()); + + local_hessian_to_global_triplets( + local_force_jacobian, vids, dim, jac_triplets, mesh.num_vertices()); + + if (wrt == DiffWRT::VELOCITIES) { + return; + } + + // The term that includes derivatives of normal contact force + const VectorMaxNd local_force = smooth_contact_force( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), false, true); + + // normal_force_grad is the gradient of contact force norm + Eigen::VectorXd normal_force_grad; + std::vector cc_vert_ids; + Eigen::MatrixXd Xt = rest_positions + lagged_displacements; + auto cc = collision.smooth_collision; + const Eigen::VectorXd contact_grad = cc->gradient(cc->dof(Xt), params); + const Eigen::MatrixXd contact_hess = cc->hessian(cc->dof(Xt), params); + normal_force_grad = + (1 / contact_grad.norm()) * (contact_hess * contact_grad); + cc_vert_ids = cc->vertex_ids(); + + local_jacobian_to_global_triplets( + local_force * normal_force_grad.transpose(), vids, cc_vert_ids, dim, + jac_triplets, mesh.num_vertices(), mesh.num_vertices()); + }); Eigen::SparseMatrix jacobian(velocities.size(), velocities.size()); for (const auto& local_jac_triplets : storage) { @@ -794,9 +761,6 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( const VectorMax2d tau_aniso = collision.mu_aniso.head(tau.size()).cwiseProduct(tau); - // Get tangent space dimension (1 for 2D sim, 2 for 3D sim) - const int tangent_dim = tau.size(); - double mu_s, mu_k; friction_mu_for_evaluation(collision, no_mu, mu_s, mu_k); diff --git a/src/ipc/smooth_contact/smooth_collisions.cpp b/src/ipc/smooth_contact/smooth_collisions.cpp index c75e583da..bafc55027 100644 --- a/src/ipc/smooth_contact/smooth_collisions.cpp +++ b/src/ipc/smooth_contact/smooth_collisions.cpp @@ -262,18 +262,14 @@ double SmoothCollisions::compute_minimum_distance( tbb::enumerable_thread_specific storage( std::numeric_limits::infinity()); - tbb::parallel_for( - tbb::blocked_range(0, m_candidates.size()), - [&](tbb::blocked_range r) { - double& local_min_dist = storage.local(); + tbb::parallel_for(size_t(0), m_candidates.size(), [&](size_t i) { + double& local_min_dist = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { - const double dist = m_candidates[i].compute_distance( - m_candidates[i].dof(vertices, edges, faces)); + const double dist = m_candidates[i].compute_distance( + m_candidates[i].dof(vertices, edges, faces)); - local_min_dist = std::min(dist, local_min_dist); - } - }); + local_min_dist = std::min(dist, local_min_dist); + }); return storage.combine([](double a, double b) { return std::min(a, b); }); } @@ -290,19 +286,15 @@ double SmoothCollisions::compute_active_minimum_distance( tbb::enumerable_thread_specific storage( std::numeric_limits::infinity()); - tbb::parallel_for( - tbb::blocked_range(0, collisions.size()), - [&](tbb::blocked_range r) { - double& local_min_dist = storage.local(); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + double& local_min_dist = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { - const double dist = collisions[i]->compute_distance(vertices); + const double dist = collisions[i]->compute_distance(vertices); - if (collisions[i]->is_active() && dist < local_min_dist) { - local_min_dist = dist; - } - } - }); + if (collisions[i]->is_active() && dist < local_min_dist) { + local_min_dist = dist; + } + }); return storage.combine([](double a, double b) { return std::min(a, b); }); } diff --git a/src/ipc/smooth_contact/smooth_contact_potential.cpp b/src/ipc/smooth_contact/smooth_contact_potential.cpp index e22b88b00..2d5a308af 100644 --- a/src/ipc/smooth_contact/smooth_contact_potential.cpp +++ b/src/ipc/smooth_contact/smooth_contact_potential.cpp @@ -2,7 +2,6 @@ #include -#include #include #include #include @@ -22,15 +21,10 @@ double SmoothContactPotential::operator()( tbb::enumerable_thread_specific storage(0); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& local_potential = storage.local(); - for (size_t i = r.begin(); i < r.end(); i++) { - // Quadrature weight is premultiplied by local potential - local_potential += (*this)(collisions[i], collisions[i].dof(X)); - } - }); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + // Quadrature weight is premultiplied by local potential + storage.local() += (*this)(collisions[i], collisions[i].dof(X)); + }); return storage.combine([](double a, double b) { return a + b; }); } @@ -50,23 +44,17 @@ Eigen::VectorXd SmoothContactPotential::gradient( tbb::enumerable_thread_specific storage( Eigen::VectorXd::Zero(X.size())); - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& global_grad = storage.local(); + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const SmoothCollision& collision = collisions[i]; - for (size_t i = r.begin(); i < r.end(); i++) { - const SmoothCollision& collision = collisions[i]; + const Eigen::VectorXd local_grad = + this->gradient(collision, collision.dof(X)); - const Eigen::VectorXd local_grad = - this->gradient(collision, collision.dof(X)); + const std::vector vids = collision.vertex_ids(); - const std::vector vids = collision.vertex_ids(); - - local_gradient_to_global_gradient( - local_grad, vids, dim, global_grad); - } - }); + local_gradient_to_global_gradient( + local_grad, vids, dim, storage.local()); + }); Eigen::VectorXd grad; grad.setZero(X.size()); @@ -95,23 +83,16 @@ Eigen::SparseMatrix SmoothContactPotential::hessian( const int buffer_size = std::min(max_triplets_size, ndof); tbb::enumerable_thread_specific storage( LocalThreadMatStorage(buffer_size, ndof, ndof)); - tbb::parallel_for( - tbb::blocked_range(0, collisions.size()), - [&](const tbb::blocked_range& r) { - auto& hess_triplets = storage.local(); - - for (size_t i = r.begin(); i < r.end(); i++) { - const SmoothCollision& collision = collisions[i]; + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { + const SmoothCollision& collision = collisions[i]; - const Eigen::MatrixXd local_hess = this->hessian( - collisions[i], collisions[i].dof(X), - project_hessian_to_psd); + const Eigen::MatrixXd local_hess = this->hessian( + collisions[i], collisions[i].dof(X), project_hessian_to_psd); - local_hessian_to_global_triplets( - local_hess, collision.vertex_ids(), dim, - *(hess_triplets.cache), mesh.num_vertices()); - } - }); + local_hessian_to_global_triplets( + local_hess, collision.vertex_ids(), dim, *(storage.local().cache), + mesh.num_vertices()); + }); Eigen::SparseMatrix hess(ndof, ndof); From 1ba3274b8c552fd77022ef62a6587f65032fa53c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 29 Apr 2026 16:50:43 -0400 Subject: [PATCH 11/13] Refactor TBB parallel_for usage for clarity and efficiency - Replace tbb::blocked_range-based loops with simpler range-based tbb::parallel_for - Remove redundant tbb_parallel_block_range_for helper in spatial_hash.cpp - Improve readability and consistency in broad phase parallel loops --- src/ipc/broad_phase/hash_grid.cpp | 11 +- src/ipc/broad_phase/lbvh.cpp | 233 ++++++++++++--------------- src/ipc/broad_phase/spatial_hash.cpp | 74 ++++----- 3 files changed, 136 insertions(+), 182 deletions(-) diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index b4fd0db59..89eeb86c2 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -64,14 +64,9 @@ void HashGrid::insert_boxes( { tbb::enumerable_thread_specific> storage; - tbb::parallel_for( - tbb::blocked_range(0L, long(boxes.size())), - [&](const tbb::blocked_range& range) { - auto& local_items = storage.local(); - for (long i = range.begin(); i != range.end(); i++) { - insert_box(boxes[i], i, local_items); - } - }); + tbb::parallel_for(0L, long(boxes.size()), [&](long i) { + insert_box(boxes[i], i, storage.local()); + }); merge_thread_local_vectors(storage, items); diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index 2b0988495..0d61f112b 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -74,25 +74,17 @@ void LBVH::build( 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)); - } - }); + tbb::parallel_for(size_t(0), size_t(edges.rows()), [&](size_t 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)); - } - }); + tbb::parallel_for(size_t(0), size_t(faces.rows()), [&](size_t 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. @@ -218,27 +210,22 @@ void LBVH::init_bvh( 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]; + tbb::parallel_for(size_t(0), boxes.size(), [&](size_t 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; + 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; - } - }); + 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; + }); } { @@ -260,110 +247,100 @@ void LBVH::init_bvh( 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 - // A leaf's rightmost leaf is itself - rightmost_leaves[LEAF_OFFSET + i] = i; - } + tbb::parallel_for(size_t(0), boxes.size(), [&](size_t 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 + // A leaf's rightmost leaf is itself + rightmost_leaves[LEAF_OFFSET + i] = i; + } - if (i < LEAF_OFFSET) { - // Find out which range of objects the node corresponds - // to. (This is where the magic happens!) + 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); + 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); + // 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_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 - } + // 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); - } + // 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); - } - } - }); + // 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); - - // Compute rightmost leaf: max of children's rightmost - rightmost_leaves[node_idx] = std::max( - rightmost_leaves[lbvh[node_idx].left], - rightmost_leaves[lbvh[node_idx].right]); - - if (node_idx == 0) { - break; // root node - } - node_idx = info.parent; - } + tbb::parallel_for(size_t(0), boxes.size(), [&](size_t 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); + + // Compute rightmost leaf: max of children's rightmost + rightmost_leaves[node_idx] = std::max( + rightmost_leaves[lbvh[node_idx].left], + rightmost_leaves[lbvh[node_idx].right]); + + if (node_idx == 0) { + break; // root node + } + node_idx = info.parent; + } + }); } } diff --git a/src/ipc/broad_phase/spatial_hash.cpp b/src/ipc/broad_phase/spatial_hash.cpp index 0f4ee5cd6..6bd45a00e 100644 --- a/src/ipc/broad_phase/spatial_hash.cpp +++ b/src/ipc/broad_phase/spatial_hash.cpp @@ -22,20 +22,6 @@ SpatialHash::SpatialHash() : impl(std::make_unique()) { } SpatialHash::~SpatialHash() = default; namespace { - inline void tbb_parallel_block_range_for( - const size_t start_i, - const size_t end_i, - const std::function& body) - { - tbb::parallel_for( - tbb::blocked_range(start_i, end_i), - [&](const tbb::blocked_range& range) { - for (size_t i = range.begin(); i != range.end(); i++) { - body(i); - } - }); - } - void fill_primitive_to_voxels( Eigen::ConstRef min_voxel, Eigen::ConstRef max_voxel, @@ -149,14 +135,14 @@ void SpatialHash::build( // ------------------------------------------------------------------------ impl->point_to_voxels.resize(num_vertices); - tbb_parallel_block_range_for(0, num_vertices, [&](size_t vi) { + tbb::parallel_for(size_t(0), num_vertices, [&](size_t vi) { fill_primitive_to_voxels( vertex_min_voxel_axis_index[vi], vertex_max_voxel_axis_index[vi], voxel_count, voxel_count_0x1, impl->point_to_voxels[vi]); }); impl->edge_to_voxels.resize(edges.rows()); - tbb_parallel_block_range_for(0, edges.rows(), [&](size_t ei) { + tbb::parallel_for(size_t(0), size_t(edges.rows()), [&](size_t ei) { fill_primitive_to_voxels( vertex_min_voxel_axis_index[edges(ei, 0)].min( vertex_min_voxel_axis_index[edges(ei, 1)]), @@ -166,7 +152,7 @@ void SpatialHash::build( }); impl->face_to_voxels.resize(faces.rows()); - tbb_parallel_block_range_for(0, faces.rows(), [&](size_t fi) { + tbb::parallel_for(size_t(0), size_t(faces.rows()), [&](size_t fi) { fill_primitive_to_voxels( vertex_min_voxel_axis_index[faces(fi, 0)] .min(vertex_min_voxel_axis_index[faces(fi, 1)]) @@ -203,37 +189,33 @@ namespace { { tbb::enumerable_thread_specific> storage; - tbb::parallel_for( - tbb::blocked_range(size_t(0), boxesA.size()), - [&](const tbb::blocked_range& range) { - auto& local_candidates = storage.local(); - - for (size_t i = range.begin(); i != range.end(); i++) { - unordered_set js; - query_A_for_Bs(i, js); - - for (const int j : js) { - int ai = i, bi = j; - if constexpr (swap_order) { - std::swap(ai, bi); - } - - if constexpr (triangular) { - if (ai >= bi) { - continue; - } - } - - if (!can_collide(ai, bi)) { - continue; - } - - if (boxesA[i].intersects(boxesB[j])) { - local_candidates.emplace_back(ai, bi); - } + tbb::parallel_for(size_t(0), boxesA.size(), [&](size_t i) { + auto& local_candidates = storage.local(); + + unordered_set js; + query_A_for_Bs(i, js); + + for (const int j : js) { + int ai = i, bi = j; + if constexpr (swap_order) { + std::swap(ai, bi); + } + + if constexpr (triangular) { + if (ai >= bi) { + continue; } } - }); + + if (!can_collide(ai, bi)) { + continue; + } + + if (boxesA[i].intersects(boxesB[j])) { + local_candidates.emplace_back(ai, bi); + } + } + }); merge_thread_local_vectors(storage, candidates); } From a18936945664b0d1d259ac14b049be94af9e1110 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 29 Apr 2026 17:08:08 -0400 Subject: [PATCH 12/13] Refactor collision builder to use per-candidate methods - Replace batch add_*_collisions methods with per-candidate add_*_collision - Update parallel_for usage to iterate over candidates by index - Simplify method signatures and improve clarity - Update duplicate removal functions to per-candidate style --- .../collisions/normal/normal_collisions.cpp | 60 +- .../normal/normal_collisions_builder.cpp | 717 +++++++++--------- .../normal/normal_collisions_builder.hpp | 56 +- 3 files changed, 383 insertions(+), 450 deletions(-) diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 92fdc2cfe..8f6506e66 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -55,35 +55,27 @@ void NormalCollisions::build( collision_set_type() == CollisionSetType::OGC); tbb::parallel_for( - tbb::blocked_range(size_t(0), candidates.vv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_vertex_vertex_collisions( - mesh, vertices, candidates.vv_candidates, is_active, r.begin(), - r.end()); + size_t(0), candidates.vv_candidates.size(), [&](size_t i) { + storage.local().add_vertex_vertex_collision( + mesh, vertices, candidates.vv_candidates[i], is_active); }); tbb::parallel_for( - tbb::blocked_range(size_t(0), candidates.ev_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_edge_vertex_collisions( - mesh, vertices, candidates.ev_candidates, is_active, r.begin(), - r.end()); + size_t(0), candidates.ev_candidates.size(), [&](size_t i) { + storage.local().add_edge_vertex_collision( + mesh, vertices, candidates.ev_candidates[i], is_active); }); tbb::parallel_for( - tbb::blocked_range(size_t(0), candidates.ee_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_edge_edge_collisions( - mesh, vertices, candidates.ee_candidates, is_active, r.begin(), - r.end()); + size_t(0), candidates.ee_candidates.size(), [&](size_t i) { + storage.local().add_edge_edge_collision( + mesh, vertices, candidates.ee_candidates[i], is_active); }); tbb::parallel_for( - tbb::blocked_range(size_t(0), candidates.fv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_face_vertex_collisions( - mesh, vertices, candidates.fv_candidates, is_active, r.begin(), - r.end()); + size_t(0), candidates.fv_candidates.size(), [&](size_t i) { + storage.local().add_face_vertex_collision( + mesh, vertices, candidates.fv_candidates[i], is_active); }); if (collision_set_type() == CollisionSetType::IMPROVED_MAX_APPROX) { @@ -93,11 +85,10 @@ void NormalCollisions::build( mesh, vertices, is_active); tbb::parallel_for( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { + size_t(0), vv_candidates.size(), [&](size_t i) { storage.local() - .add_edge_vertex_negative_vertex_vertex_collisions( - mesh, vertices, vv_candidates, r.begin(), r.end()); + .add_edge_vertex_negative_vertex_vertex_collision( + mesh, vertices, vv_candidates[i]); }); } @@ -107,11 +98,10 @@ void NormalCollisions::build( candidates.edge_edge_to_edge_vertex(mesh, vertices, is_active); tbb::parallel_for( - tbb::blocked_range(size_t(0), ev_candidates.size()), - [&](const tbb::blocked_range& r) { + size_t(0), ev_candidates.size(), [&](size_t i) { storage.local() - .add_edge_edge_negative_edge_vertex_collisions( - mesh, vertices, ev_candidates, r.begin(), r.end()); + .add_edge_edge_negative_edge_vertex_collision( + mesh, vertices, ev_candidates[i]); }); } @@ -121,11 +111,10 @@ void NormalCollisions::build( mesh, vertices, is_active); tbb::parallel_for( - tbb::blocked_range(size_t(0), ev_candidates.size()), - [&](const tbb::blocked_range& r) { + size_t(0), ev_candidates.size(), [&](size_t i) { storage.local() - .add_face_vertex_negative_edge_vertex_collisions( - mesh, vertices, ev_candidates, r.begin(), r.end()); + .add_face_vertex_negative_edge_vertex_collision( + mesh, vertices, ev_candidates[i]); }); // Convert face-vertex to vertex-vertex @@ -133,11 +122,10 @@ void NormalCollisions::build( mesh, vertices, is_active); tbb::parallel_for( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { + size_t(0), vv_candidates.size(), [&](size_t i) { storage.local() - .add_face_vertex_positive_vertex_vertex_collisions( - mesh, vertices, vv_candidates, r.begin(), r.end()); + .add_face_vertex_positive_vertex_vertex_collision( + mesh, vertices, vv_candidates[i]); }); } } diff --git a/src/ipc/collisions/normal/normal_collisions_builder.cpp b/src/ipc/collisions/normal/normal_collisions_builder.cpp index 3039d0752..bf5889b1a 100644 --- a/src/ipc/collisions/normal/normal_collisions_builder.cpp +++ b/src/ipc/collisions/normal/normal_collisions_builder.cpp @@ -23,97 +23,87 @@ NormalCollisionsBuilder::NormalCollisionsBuilder( // ============================================================================ -void NormalCollisionsBuilder::add_vertex_vertex_collisions( +void NormalCollisionsBuilder::add_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i) + const VertexVertexCandidate& candidate, + const std::function& is_active) { - for (size_t i = start_i; i < end_i; i++) { - const auto& [vi, vj] = candidates[i]; + const auto& [vi, vj] = candidate; - const double distance = - point_point_distance(vertices.row(vi), vertices.row(vj)); + const double distance = point_point_distance(vertices.row(vi), vertices.row(vj)); - if (!is_active(distance)) { - continue; - } - - // ÷ 2 to handle double counting. Sum vertex areas because duplicate - // vertex-vertex candidates were removed. - const double weight = use_area_weighting - ? (0.5 * (mesh.vertex_area(vi) + mesh.vertex_area(vj))) - : 1; - - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.5 - * (mesh.vertex_area_gradient(vi) - + mesh.vertex_area_gradient(vj))) - : Eigen::SparseVector(vertices.size()); - } + if (!is_active(distance)) { + return; + } - VertexVertexNormalCollision vv(vi, vj, weight, weight_gradient); - vv_to_id.emplace(vv, vv_collisions.size()); - vv_collisions.push_back(vv); + // ÷ 2 to handle double counting. Sum vertex areas because duplicate + // vertex-vertex candidates were removed. + const double weight = use_area_weighting + ? (0.5 * (mesh.vertex_area(vi) + mesh.vertex_area(vj))) + : 1; + + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (0.5 + * (mesh.vertex_area_gradient(vi) + + mesh.vertex_area_gradient(vj))) + : Eigen::SparseVector(vertices.size()); } + + VertexVertexNormalCollision vv(vi, vj, weight, weight_gradient); + vv_to_id.emplace(vv, vv_collisions.size()); + vv_collisions.push_back(vv); } -void NormalCollisionsBuilder::add_edge_vertex_collisions( +void NormalCollisionsBuilder::add_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i) + const EdgeVertexCandidate& candidate, + const std::function& is_active) { - for (size_t i = start_i; i < end_i; i++) { - const auto& [ei, vi] = candidates[i]; - const auto [v, e0, e1, _] = - candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); - - const PointEdgeDistanceType dtype = point_edge_distance_type(v, e0, e1); - const double distance_sqr = point_edge_distance(v, e0, e1, dtype); - - if (!is_active(distance_sqr) - || (use_ogc - && !ogc::is_edge_vertex_feasible( - mesh, vertices, candidates[i], dtype))) { - continue; - } + const auto& [ei, vi] = candidate; + const auto [v, e0, e1, _] = + candidate.vertices(vertices, mesh.edges(), mesh.faces()); - // ÷ 2 to handle double counting for correct integration - double weight = use_area_weighting ? (0.5 * mesh.vertex_area(vi)) : 1; + const PointEdgeDistanceType dtype = point_edge_distance_type(v, e0, e1); + const double distance_sqr = point_edge_distance(v, e0, e1, dtype); - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.5 * mesh.vertex_area_gradient(vi)) - : Eigen::SparseVector(vertices.size()); - } + if (!is_active(distance_sqr) + || (use_ogc + && !ogc::is_edge_vertex_feasible( + mesh, vertices, candidate, dtype))) { + return; + } - // ÷ n to handle duplicate counting for correct integration - if (use_ogc && int(dtype) < int(PointEdgeDistanceType::P_E)) { - // Divide by the number of incident edges of vj - // NOTE: This only works for OGC because vj is in the block of - // vi iff vi is in the block of vj. - const index_t vj = mesh.edges()(ei, int(dtype)); - const int n = mesh.vertex_edge_adjacencies()[vj].size(); - assert(n >= 1); - if (n > 1) { - weight /= n; - if (use_area_weighting && enable_shape_derivatives) { - weight_gradient /= n; - } + // ÷ 2 to handle double counting for correct integration + double weight = use_area_weighting ? (0.5 * mesh.vertex_area(vi)) : 1; + + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (0.5 * mesh.vertex_area_gradient(vi)) + : Eigen::SparseVector(vertices.size()); + } + + // ÷ n to handle duplicate counting for correct integration + if (use_ogc && int(dtype) < int(PointEdgeDistanceType::P_E)) { + // Divide by the number of incident edges of vj + // NOTE: This only works for OGC because vj is in the block of + // vi iff vi is in the block of vj. + const index_t vj = mesh.edges()(ei, int(dtype)); + const int n = mesh.vertex_edge_adjacencies()[vj].size(); + assert(n >= 1); + if (n > 1) { + weight /= n; + if (use_area_weighting && enable_shape_derivatives) { + weight_gradient /= n; } } - - add_edge_vertex_collision( - mesh, candidates[i], dtype, weight, weight_gradient); } + + add_edge_vertex_collision(mesh, candidate, dtype, weight, weight_gradient); } void NormalCollisionsBuilder::add_edge_vertex_collision( @@ -147,223 +137,210 @@ void NormalCollisionsBuilder::add_edge_vertex_collision( } } -void NormalCollisionsBuilder::add_edge_edge_collisions( +void NormalCollisionsBuilder::add_edge_edge_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i) + const EdgeEdgeCandidate& candidate, + const std::function& is_active) { - for (size_t i = start_i; i < end_i; i++) { - const auto& [eai, ebi] = candidates[i]; + const auto& [eai, ebi] = candidate; - const auto [ea0i, ea1i, eb0i, eb1i] = - candidates[i].vertex_ids(mesh.edges(), mesh.faces()); + const auto [ea0i, ea1i, eb0i, eb1i] = + candidate.vertex_ids(mesh.edges(), mesh.faces()); - const auto [ea0, ea1, eb0, eb1] = - candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); + const auto [ea0, ea1, eb0, eb1] = + candidate.vertices(vertices, mesh.edges(), mesh.faces()); - const EdgeEdgeDistanceType actual_dtype = - edge_edge_distance_type(ea0, ea1, eb0, eb1); + const EdgeEdgeDistanceType actual_dtype = + edge_edge_distance_type(ea0, ea1, eb0, eb1); - const double distance_sqr = - edge_edge_distance(ea0, ea1, eb0, eb1, actual_dtype); + const double distance_sqr = + edge_edge_distance(ea0, ea1, eb0, eb1, actual_dtype); - if (!is_active(distance_sqr) - || (use_ogc - && !ogc::is_edge_edge_feasible( - mesh, vertices, candidates[i], actual_dtype))) { - continue; - } - - const double eps_x = edge_edge_mollifier_threshold( - mesh.rest_positions().row(ea0i), mesh.rest_positions().row(ea1i), - mesh.rest_positions().row(eb0i), mesh.rest_positions().row(eb1i)); - - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - - // NOTE: This may not actually be the distance type, but all EE - // pairs requiring mollification must be mollified later. - const EdgeEdgeDistanceType dtype = ee_cross_norm_sqr < eps_x - ? EdgeEdgeDistanceType::EA_EB - : actual_dtype; - - // ÷ 4 to handle double counting and PT + EE for correct integration. - // Sum edge areas because duplicate edge candidates were removed. - const double weight = use_area_weighting - ? (0.25 * (mesh.edge_area(eai) + mesh.edge_area(ebi))) - : 1; + if (!is_active(distance_sqr) + || (use_ogc + && !ogc::is_edge_edge_feasible( + mesh, vertices, candidate, actual_dtype))) { + return; + } - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.25 - * (mesh.edge_area_gradient(eai) - + mesh.edge_area_gradient(ebi))) - : Eigen::SparseVector(vertices.size()); - } + const double eps_x = edge_edge_mollifier_threshold( + mesh.rest_positions().row(ea0i), mesh.rest_positions().row(ea1i), + mesh.rest_positions().row(eb0i), mesh.rest_positions().row(eb1i)); + + const double ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + + // NOTE: This may not actually be the distance type, but all EE + // pairs requiring mollification must be mollified later. + const EdgeEdgeDistanceType dtype = + ee_cross_norm_sqr < eps_x ? EdgeEdgeDistanceType::EA_EB : actual_dtype; + + // ÷ 4 to handle double counting and PT + EE for correct integration. + // Sum edge areas because duplicate edge candidates were removed. + const double weight = use_area_weighting + ? (0.25 * (mesh.edge_area(eai) + mesh.edge_area(ebi))) + : 1; + + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (0.25 + * (mesh.edge_area_gradient(eai) + mesh.edge_area_gradient(ebi))) + : Eigen::SparseVector(vertices.size()); + } - switch (dtype) { - case EdgeEdgeDistanceType::EA0_EB0: - add_vertex_vertex_collision(ea0i, eb0i, weight, weight_gradient); - break; + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + add_vertex_vertex_collision(ea0i, eb0i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA0_EB1: - add_vertex_vertex_collision(ea0i, eb1i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA0_EB1: + add_vertex_vertex_collision(ea0i, eb1i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA1_EB0: - add_vertex_vertex_collision(ea1i, eb0i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA1_EB0: + add_vertex_vertex_collision(ea1i, eb0i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA1_EB1: - add_vertex_vertex_collision(ea1i, eb1i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA1_EB1: + add_vertex_vertex_collision(ea1i, eb1i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA_EB0: - add_edge_vertex_collision(eai, eb0i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA_EB0: + add_edge_vertex_collision(eai, eb0i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA_EB1: - add_edge_vertex_collision(eai, eb1i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA_EB1: + add_edge_vertex_collision(eai, eb1i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA0_EB: - add_edge_vertex_collision(ebi, ea0i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA0_EB: + add_edge_vertex_collision(ebi, ea0i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA1_EB: - add_edge_vertex_collision(ebi, ea1i, weight, weight_gradient); - break; + case EdgeEdgeDistanceType::EA1_EB: + add_edge_vertex_collision(ebi, ea1i, weight, weight_gradient); + break; - case EdgeEdgeDistanceType::EA_EB: - ee_collisions.emplace_back( - eai, ebi, eps_x, weight, weight_gradient, actual_dtype); - ee_to_id.emplace(ee_collisions.back(), ee_collisions.size() - 1); - break; + case EdgeEdgeDistanceType::EA_EB: + ee_collisions.emplace_back( + eai, ebi, eps_x, weight, weight_gradient, actual_dtype); + ee_to_id.emplace(ee_collisions.back(), ee_collisions.size() - 1); + break; - case EdgeEdgeDistanceType::AUTO: - default: - assert(false); - break; - } + case EdgeEdgeDistanceType::AUTO: + default: + assert(false); + break; } } -void NormalCollisionsBuilder::add_face_vertex_collisions( +void NormalCollisionsBuilder::add_face_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i) + const FaceVertexCandidate& candidate, + const std::function& is_active) { - for (size_t i = start_i; i < end_i; i++) { - const auto& [fi, vi] = candidates[i]; - const index_t f0i = mesh.faces()(fi, 0), f1i = mesh.faces()(fi, 1), - f2i = mesh.faces()(fi, 2); - - const auto [v, f0, f1, f2] = - candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); - - // Compute distance type - const PointTriangleDistanceType dtype = - point_triangle_distance_type(v, f0, f1, f2); - const double distance_sqr = - point_triangle_distance(v, f0, f1, f2, dtype); - - if (!is_active(distance_sqr) - || (use_ogc - && !ogc::is_face_vertex_feasible( - mesh, vertices, candidates[i], dtype))) { - continue; - } + const auto& [fi, vi] = candidate; + const index_t f0i = mesh.faces()(fi, 0), f1i = mesh.faces()(fi, 1), + f2i = mesh.faces()(fi, 2); + + const auto [v, f0, f1, f2] = + candidate.vertices(vertices, mesh.edges(), mesh.faces()); + + // Compute distance type + const PointTriangleDistanceType dtype = + point_triangle_distance_type(v, f0, f1, f2); + const double distance_sqr = point_triangle_distance(v, f0, f1, f2, dtype); + + if (!is_active(distance_sqr) + || (use_ogc + && !ogc::is_face_vertex_feasible( + mesh, vertices, candidate, dtype))) { + return; + } - // ÷ 4 to handle double counting and PT + EE for correct integration - double weight = use_area_weighting ? (0.25 * mesh.vertex_area(vi)) : 1; + // ÷ 4 to handle double counting and PT + EE for correct integration + double weight = use_area_weighting ? (0.25 * mesh.vertex_area(vi)) : 1; - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.25 * mesh.vertex_area_gradient(vi)) - : Eigen::SparseVector(vertices.size()); - } + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (0.25 * mesh.vertex_area_gradient(vi)) + : Eigen::SparseVector(vertices.size()); + } - // ÷ n to handle duplicate counting for correct integration - if (use_ogc) { - int n = 1; // Default to 1 for P_T - if (int(dtype) < int(PointTriangleDistanceType::P_E0)) { - // Divide by the number of incident faces of vj - // NOTE: This only works for OGC because vj is in the block of - // vi iff vi is in the block of vj. - const index_t vj = mesh.faces()(fi, int(dtype)); - n = mesh.vertex_face_adjacencies()[vj].size(); - } else if (int(dtype) < int(PointTriangleDistanceType::P_T)) { - // Divide by the number of incident edges of vj - // NOTE: This only works for OGC because if vj is in the block - // of ei then vi is in the block of ei on its neighboring face. - const index_t ej = mesh.faces_to_edges()(fi, int(dtype) - 3); - n = mesh.edge_vertex_adjacencies()[ej].size(); - } - assert(n >= 1); - if (n > 1) { - weight /= n; - if (use_area_weighting && enable_shape_derivatives) { - weight_gradient /= n; - } + // ÷ n to handle duplicate counting for correct integration + if (use_ogc) { + int n = 1; // Default to 1 for P_T + if (int(dtype) < int(PointTriangleDistanceType::P_E0)) { + // Divide by the number of incident faces of vj + // NOTE: This only works for OGC because vj is in the block of + // vi iff vi is in the block of vj. + const index_t vj = mesh.faces()(fi, int(dtype)); + n = mesh.vertex_face_adjacencies()[vj].size(); + } else if (int(dtype) < int(PointTriangleDistanceType::P_T)) { + // Divide by the number of incident edges of vj + // NOTE: This only works for OGC because if vj is in the block + // of ei then vi is in the block of ei on its neighboring face. + const index_t ej = mesh.faces_to_edges()(fi, int(dtype) - 3); + n = mesh.edge_vertex_adjacencies()[ej].size(); + } + assert(n >= 1); + if (n > 1) { + weight /= n; + if (use_area_weighting && enable_shape_derivatives) { + weight_gradient /= n; } } + } - switch (dtype) { - case PointTriangleDistanceType::P_T0: - add_vertex_vertex_collision(vi, f0i, weight, weight_gradient); - break; + switch (dtype) { + case PointTriangleDistanceType::P_T0: + add_vertex_vertex_collision(vi, f0i, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_T1: - add_vertex_vertex_collision(vi, f1i, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_T1: + add_vertex_vertex_collision(vi, f1i, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_T2: - add_vertex_vertex_collision(vi, f2i, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_T2: + add_vertex_vertex_collision(vi, f2i, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_E0: - add_edge_vertex_collision( - mesh.faces_to_edges()(fi, 0), vi, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_E0: + add_edge_vertex_collision( + mesh.faces_to_edges()(fi, 0), vi, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_E1: - add_edge_vertex_collision( - mesh.faces_to_edges()(fi, 1), vi, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_E1: + add_edge_vertex_collision( + mesh.faces_to_edges()(fi, 1), vi, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_E2: - add_edge_vertex_collision( - mesh.faces_to_edges()(fi, 2), vi, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_E2: + add_edge_vertex_collision( + mesh.faces_to_edges()(fi, 2), vi, weight, weight_gradient); + break; - case PointTriangleDistanceType::P_T: - fv_collisions.emplace_back(fi, vi, weight, weight_gradient); - break; + case PointTriangleDistanceType::P_T: + fv_collisions.emplace_back(fi, vi, weight, weight_gradient); + break; - case PointTriangleDistanceType::AUTO: - default: - assert(false); - break; - } + case PointTriangleDistanceType::AUTO: + default: + assert(false); + break; } } // ============================================================================ -void NormalCollisionsBuilder::add_edge_vertex_negative_vertex_vertex_collisions( +void NormalCollisionsBuilder::add_edge_vertex_negative_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) + const VertexVertexCandidate& candidate) { const auto add_weight = [&](const size_t vi, const size_t vj, double& weight, @@ -386,31 +363,27 @@ void NormalCollisionsBuilder::add_edge_vertex_negative_vertex_vertex_collisions( } }; - for (size_t i = start_i; i < end_i; i++) { - const auto& [vi, vj] = candidates[i]; - assert(vi != vj); + const auto& [vi, vj] = candidate; + assert(vi != vj); - double weight = 0; - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = Eigen::SparseVector(vertices.size()); - } + double weight = 0; + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = Eigen::SparseVector(vertices.size()); + } - add_weight(vi, vj, weight, weight_gradient); - add_weight(vj, vi, weight, weight_gradient); + add_weight(vi, vj, weight, weight_gradient); + add_weight(vj, vi, weight, weight_gradient); - if (weight != 0) { - add_vertex_vertex_collision(vi, vj, weight, weight_gradient); - } + if (weight != 0) { + add_vertex_vertex_collision(vi, vj, weight, weight_gradient); } } -void NormalCollisionsBuilder::add_face_vertex_positive_vertex_vertex_collisions( +void NormalCollisionsBuilder::add_face_vertex_positive_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) + const VertexVertexCandidate& candidate) { const auto add_weight = [&](const size_t vi, const size_t vj, double& weight, @@ -432,153 +405,141 @@ void NormalCollisionsBuilder::add_face_vertex_positive_vertex_vertex_collisions( } }; - for (size_t i = start_i; i < end_i; i++) { - const auto& [vi, vj] = candidates[i]; - assert(vi != vj); + const auto& [vi, vj] = candidate; + assert(vi != vj); - double weight = 0; - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = Eigen::SparseVector(vertices.size()); - } + double weight = 0; + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = Eigen::SparseVector(vertices.size()); + } - add_weight(vi, vj, weight, weight_gradient); - add_weight(vj, vi, weight, weight_gradient); + add_weight(vi, vj, weight, weight_gradient); + add_weight(vj, vi, weight, weight_gradient); - if (weight != 0) { - add_vertex_vertex_collision(vi, vj, weight, weight_gradient); - } + if (weight != 0) { + add_vertex_vertex_collision(vi, vj, weight, weight_gradient); } } -void NormalCollisionsBuilder::add_face_vertex_negative_edge_vertex_collisions( +void NormalCollisionsBuilder::add_face_vertex_negative_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) + const EdgeVertexCandidate& candidate) { - for (size_t i = start_i; i < end_i; i++) { - const auto& [ei, vi] = candidates[i]; - assert(vi != mesh.edges()(ei, 0) && vi != mesh.edges()(ei, 1)); + const auto& [ei, vi] = candidate; + assert(vi != mesh.edges()(ei, 0) && vi != mesh.edges()(ei, 1)); - const auto& incident_vertices = mesh.edge_vertex_adjacencies()[ei]; - assert( - std::is_sorted(incident_vertices.begin(), incident_vertices.end())); - const bool is_vi_incident = std::binary_search( - incident_vertices.begin(), incident_vertices.end(), vi); - const index_t incident_triangle_amt = - incident_vertices.size() - index_t(is_vi_incident); + const auto& incident_vertices = mesh.edge_vertex_adjacencies()[ei]; + assert(std::is_sorted(incident_vertices.begin(), incident_vertices.end())); + const bool is_vi_incident = std::binary_search( + incident_vertices.begin(), incident_vertices.end(), vi); + const index_t incident_triangle_amt = + incident_vertices.size() - index_t(is_vi_incident); - if (incident_triangle_amt > 1) { - // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = (1 - incident_triangle_amt) - * (use_area_weighting ? (0.25 * mesh.vertex_area(vi)) : 1); - - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.25 * (1 - incident_triangle_amt) - * mesh.vertex_area_gradient(vi)) - : Eigen::SparseVector(vertices.size()); - } + if (incident_triangle_amt > 1) { + // ÷ 4 to handle double counting and PT + EE for correct integration + const double weight = (1 - incident_triangle_amt) + * (use_area_weighting ? (0.25 * mesh.vertex_area(vi)) : 1); - add_edge_vertex_collision( - mesh, candidates[i], - point_edge_distance_type( - vertices.row(vi), vertices.row(mesh.edges()(ei, 0)), - vertices.row(mesh.edges()(ei, 1))), - weight, weight_gradient); + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (0.25 * (1 - incident_triangle_amt) + * mesh.vertex_area_gradient(vi)) + : Eigen::SparseVector(vertices.size()); } + + add_edge_vertex_collision( + mesh, candidate, + point_edge_distance_type( + vertices.row(vi), vertices.row(mesh.edges()(ei, 0)), + vertices.row(mesh.edges()(ei, 1))), + weight, weight_gradient); } } -void NormalCollisionsBuilder::add_edge_edge_negative_edge_vertex_collisions( +void NormalCollisionsBuilder::add_edge_edge_negative_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) + const EdgeVertexCandidate& candidate) { // Notation: (ea, p) ∈ C, ea = (ea0, ea1) ∈ E, p ∈ eb = (p, q) ∈ E - for (size_t i = start_i; i < end_i; i++) { - const auto& [ea, p] = candidates[i]; - const index_t ea0 = mesh.edges()(ea, 0), ea1 = mesh.edges()(ea, 1); - assert(p != ea0 && p != ea1); - - // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = - use_area_weighting ? (-0.25 * mesh.edge_area(ea)) : -1; - Eigen::SparseVector weight_gradient; - if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (-0.25 * mesh.edge_area_gradient(ea)) - : Eigen::SparseVector(vertices.size()); - } - - const PointEdgeDistanceType dtype = point_edge_distance_type( - vertices.row(p), vertices.row(ea0), vertices.row(ea1)); - - index_t nonmollified_incident_edge_amt = 0; + const auto& [ea, p] = candidate; + const index_t ea0 = mesh.edges()(ea, 0), ea1 = mesh.edges()(ea, 1); + assert(p != ea0 && p != ea1); + + // ÷ 4 to handle double counting and PT + EE for correct integration + const double weight = + use_area_weighting ? (-0.25 * mesh.edge_area(ea)) : -1; + Eigen::SparseVector weight_gradient; + if (enable_shape_derivatives) { + weight_gradient = use_area_weighting + ? (-0.25 * mesh.edge_area_gradient(ea)) + : Eigen::SparseVector(vertices.size()); + } - const auto& incident_edges = mesh.vertex_edge_adjacencies()[p]; - for (const index_t eb : incident_edges) { - const index_t eb0 = mesh.edges()(eb, 0), eb1 = mesh.edges()(eb, 1); - const index_t q = mesh.edges()(eb, index_t(p == eb0)); - assert(p != q); - if (q == ea0 || q == ea1) { - continue; - } + const PointEdgeDistanceType dtype = point_edge_distance_type( + vertices.row(p), vertices.row(ea0), vertices.row(ea1)); - const double eps_x = edge_edge_mollifier_threshold( - mesh.rest_positions().row(ea0), mesh.rest_positions().row(ea1), - mesh.rest_positions().row(eb0), mesh.rest_positions().row(eb1)); + index_t nonmollified_incident_edge_amt = 0; - const double ee_cross_norm_sqr = edge_edge_cross_squarednorm( - vertices.row(ea0), vertices.row(ea1), vertices.row(eb0), - vertices.row(eb1)); + const auto& incident_edges = mesh.vertex_edge_adjacencies()[p]; + for (const index_t eb : incident_edges) { + const index_t eb0 = mesh.edges()(eb, 0), eb1 = mesh.edges()(eb, 1); + const index_t q = mesh.edges()(eb, index_t(p == eb0)); + assert(p != q); + if (q == ea0 || q == ea1) { + continue; + } - if (ee_cross_norm_sqr >= eps_x) { - nonmollified_incident_edge_amt++; - continue; - } + const double eps_x = edge_edge_mollifier_threshold( + mesh.rest_positions().row(ea0), mesh.rest_positions().row(ea1), + mesh.rest_positions().row(eb0), mesh.rest_positions().row(eb1)); - // Add mollified EE collision with specified distance type - // Convert the PE distance type to an EE distance type - EdgeEdgeDistanceType ee_dtype = EdgeEdgeDistanceType::AUTO; - switch (dtype) { - case PointEdgeDistanceType::P_E0: - ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA0_EB0 - : EdgeEdgeDistanceType::EA0_EB1; - break; - case PointEdgeDistanceType::P_E1: - ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA1_EB0 - : EdgeEdgeDistanceType::EA1_EB1; - break; - case PointEdgeDistanceType::P_E: - ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA_EB0 - : EdgeEdgeDistanceType::EA_EB1; - break; - default: - assert(false); - break; - } + const double ee_cross_norm_sqr = edge_edge_cross_squarednorm( + vertices.row(ea0), vertices.row(ea1), vertices.row(eb0), + vertices.row(eb1)); - add_edge_edge_collision( - ea, eb, eps_x, weight, weight_gradient, ee_dtype); + if (ee_cross_norm_sqr >= eps_x) { + nonmollified_incident_edge_amt++; + continue; } - if (nonmollified_incident_edge_amt == 1) { - continue; // no collision to add because (ρ(x) - 1) = 0 + // Add mollified EE collision with specified distance type + // Convert the PE distance type to an EE distance type + EdgeEdgeDistanceType ee_dtype = EdgeEdgeDistanceType::AUTO; + switch (dtype) { + case PointEdgeDistanceType::P_E0: + ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA0_EB0 + : EdgeEdgeDistanceType::EA0_EB1; + break; + case PointEdgeDistanceType::P_E1: + ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA1_EB0 + : EdgeEdgeDistanceType::EA1_EB1; + break; + case PointEdgeDistanceType::P_E: + ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA_EB0 + : EdgeEdgeDistanceType::EA_EB1; + break; + default: + assert(false); + break; } - // if nonmollified_incident_edge_amt == 0, then we need to explicitly - // add a positive collision. - add_edge_vertex_collision( - mesh, candidates[i], dtype, - (nonmollified_incident_edge_amt - 1) * weight, - (nonmollified_incident_edge_amt - 1) * weight_gradient); + + add_edge_edge_collision( + ea, eb, eps_x, weight, weight_gradient, ee_dtype); + } + + if (nonmollified_incident_edge_amt == 1) { + return; // no collision to add because (ρ(x) - 1) = 0 } + // if nonmollified_incident_edge_amt == 0, then we need to explicitly + // add a positive collision. + add_edge_vertex_collision( + mesh, candidate, dtype, (nonmollified_incident_edge_amt - 1) * weight, + (nonmollified_incident_edge_amt - 1) * weight_gradient); } // ============================================================================ diff --git a/src/ipc/collisions/normal/normal_collisions_builder.hpp b/src/ipc/collisions/normal/normal_collisions_builder.hpp index 9d29499e4..cc2eef4bd 100644 --- a/src/ipc/collisions/normal/normal_collisions_builder.hpp +++ b/src/ipc/collisions/normal/normal_collisions_builder.hpp @@ -21,68 +21,52 @@ class NormalCollisionsBuilder { const bool enable_shape_derivatives, const bool use_ogc); - void add_vertex_vertex_collisions( + void add_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i); + const VertexVertexCandidate& candidate, + const std::function& is_active); - void add_edge_vertex_collisions( + void add_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i); + const EdgeVertexCandidate& candidate, + const std::function& is_active); - void add_edge_edge_collisions( + void add_edge_edge_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i); + const EdgeEdgeCandidate& candidate, + const std::function& is_active); - void add_face_vertex_collisions( + void add_face_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const std::function& is_active, - const size_t start_i, - const size_t end_i); + const FaceVertexCandidate& candidate, + const std::function& is_active); // ------------------------------------------------------------------------ // Duplicate removal functions - void add_edge_vertex_negative_vertex_vertex_collisions( + void add_edge_vertex_negative_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i); + const VertexVertexCandidate& candidate); - void add_face_vertex_positive_vertex_vertex_collisions( + void add_face_vertex_positive_vertex_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i); + const VertexVertexCandidate& candidate); - void add_face_vertex_negative_edge_vertex_collisions( + void add_face_vertex_negative_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i); + const EdgeVertexCandidate& candidate); - void add_edge_edge_negative_edge_vertex_collisions( + void add_edge_edge_negative_edge_vertex_collision( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i); + const EdgeVertexCandidate& candidate); // ------------------------------------------------------------------------ From e262118118433899c6f7dfac7a4c506d55ac5f8b Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 29 Apr 2026 17:12:38 -0400 Subject: [PATCH 13/13] Apply clang-format --- .../collisions/normal/normal_collisions.cpp | 42 ++++++++----------- 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 8f6506e66..da63d4605 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -84,12 +84,11 @@ void NormalCollisions::build( const auto vv_candidates = candidates.edge_vertex_to_vertex_vertex( mesh, vertices, is_active); - tbb::parallel_for( - size_t(0), vv_candidates.size(), [&](size_t i) { - storage.local() - .add_edge_vertex_negative_vertex_vertex_collision( - mesh, vertices, vv_candidates[i]); - }); + tbb::parallel_for(size_t(0), vv_candidates.size(), [&](size_t i) { + storage.local() + .add_edge_vertex_negative_vertex_vertex_collision( + mesh, vertices, vv_candidates[i]); + }); } if (!candidates.ee_candidates.empty()) { @@ -97,12 +96,10 @@ void NormalCollisions::build( const auto ev_candidates = candidates.edge_edge_to_edge_vertex(mesh, vertices, is_active); - tbb::parallel_for( - size_t(0), ev_candidates.size(), [&](size_t i) { - storage.local() - .add_edge_edge_negative_edge_vertex_collision( - mesh, vertices, ev_candidates[i]); - }); + tbb::parallel_for(size_t(0), ev_candidates.size(), [&](size_t i) { + storage.local().add_edge_edge_negative_edge_vertex_collision( + mesh, vertices, ev_candidates[i]); + }); } if (!candidates.fv_candidates.empty()) { @@ -110,23 +107,20 @@ void NormalCollisions::build( const auto ev_candidates = candidates.face_vertex_to_edge_vertex( mesh, vertices, is_active); - tbb::parallel_for( - size_t(0), ev_candidates.size(), [&](size_t i) { - storage.local() - .add_face_vertex_negative_edge_vertex_collision( - mesh, vertices, ev_candidates[i]); - }); + tbb::parallel_for(size_t(0), ev_candidates.size(), [&](size_t i) { + storage.local().add_face_vertex_negative_edge_vertex_collision( + mesh, vertices, ev_candidates[i]); + }); // Convert face-vertex to vertex-vertex const auto vv_candidates = candidates.face_vertex_to_vertex_vertex( mesh, vertices, is_active); - tbb::parallel_for( - size_t(0), vv_candidates.size(), [&](size_t i) { - storage.local() - .add_face_vertex_positive_vertex_vertex_collision( - mesh, vertices, vv_candidates[i]); - }); + tbb::parallel_for(size_t(0), vv_candidates.size(), [&](size_t i) { + storage.local() + .add_face_vertex_positive_vertex_vertex_collision( + mesh, vertices, vv_candidates[i]); + }); } }