diff --git a/docs/source/references.bib b/docs/source/references.bib index 8d662e9b6..79445e160 100644 --- a/docs/source/references.bib +++ b/docs/source/references.bib @@ -108,6 +108,15 @@ @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 = {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}}, 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..995cdfa14 100644 --- a/docs/source/tutorials/ogc.rst +++ b/docs/source/tutorials/ogc.rst @@ -383,4 +383,138 @@ 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. + +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`` +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``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:: + + .. 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); + + // Optionally tune the relaxation ratio via the struct member: + // trust_region.relaxed_radius_scaling = 0.9; // default + + .. 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) + + # Optionally tune the relaxation ratio via the struct member: + # trust_region.relaxed_radius_scaling = 0.9 # default + +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); + + // 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) + + # Update positions + x += dx + + # Check convergence... + +.. note:: + 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 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + ++-----------------------------------+-----------------------------------------------+ +| 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/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 b034fe038..9b6eb2dbd 100644 --- a/python/src/ogc/trust_region.cpp +++ b/python/src/ogc/trust_region.cpp @@ -81,6 +81,26 @@ 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 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. + + 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). + )ipc_Qu8mg5v7", + "mesh"_a, "x"_a, "dx"_a) .def_readwrite( "trust_region_centers", &ogc::TrustRegion::trust_region_centers, "Centers of the trust regions for each vertex.") @@ -118,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/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/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/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/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); } 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/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/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/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/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 92fdc2cfe..da63d4605 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) { @@ -92,13 +84,11 @@ void NormalCollisions::build( const auto vv_candidates = candidates.edge_vertex_to_vertex_vertex( mesh, vertices, is_active); - tbb::parallel_for( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local() - .add_edge_vertex_negative_vertex_vertex_collisions( - mesh, vertices, vv_candidates, r.begin(), r.end()); - }); + 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()) { @@ -106,13 +96,10 @@ void NormalCollisions::build( const auto ev_candidates = 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) { - storage.local() - .add_edge_edge_negative_edge_vertex_collisions( - mesh, vertices, ev_candidates, r.begin(), r.end()); - }); + 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()) { @@ -120,25 +107,20 @@ void NormalCollisions::build( const auto ev_candidates = candidates.face_vertex_to_edge_vertex( mesh, vertices, is_active); - tbb::parallel_for( - tbb::blocked_range(size_t(0), ev_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local() - .add_face_vertex_negative_edge_vertex_collisions( - mesh, vertices, ev_candidates, r.begin(), r.end()); - }); + 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( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local() - .add_face_vertex_positive_vertex_vertex_collisions( - mesh, vertices, vv_candidates, r.begin(), r.end()); - }); + 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]); + }); } } 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); // ------------------------------------------------------------------------ diff --git a/src/ipc/ogc/trust_region.cpp b/src/ipc/ogc/trust_region.cpp index 42d16001a..56402412b 100644 --- a/src/ipc/ogc/trust_region.cpp +++ b/src/ipc/ogc/trust_region.cpp @@ -1,5 +1,9 @@ #include "trust_region.hpp" +#include + +#include + namespace ipc::ogc { TrustRegion::TrustRegion(double dhat) : trust_region_inflation_radius(2 * dhat) @@ -37,8 +41,8 @@ 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++) { + std::atomic num_updates(0); + 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); @@ -48,9 +52,9 @@ void TrustRegion::warm_start_time_step( // 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; + 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. @@ -78,15 +82,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 @@ -107,7 +110,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); } @@ -116,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( @@ -125,8 +178,8 @@ 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++) { + std::atomic num_updates(0); + 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); @@ -134,56 +187,181 @@ 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 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. + if (num_updates > update_threshold * mesh.num_vertices()) { + logger().trace( + "{:.1f}% of vertices restricted by trust region. Updating trust region.", + 100.0 * num_updates / mesh.num_vertices()); + should_update_trust_region = true; + } +} + +void TrustRegion::planar_filter_step( + const CollisionMesh& mesh, + Eigen::ConstRef x, + Eigen::Ref dx) +{ + assert(x.rows() == dx.rows() && x.cols() == dx.cols()); + assert(relaxed_radius_scaling > 0 && relaxed_radius_scaling < 1); - const double a = dxi.squaredNorm(); - assert(a > 0); // Δx should not be zero + const int d = x.cols(); - // b can be positive or negative - const double b = 2 * dxi.dot(xi - ci); + // 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; + if (has_hidden_dofs) { + vertices = mesh.vertices(x); + } else { + vertices = x; + } - const double c = (xi - ci).squaredNorm() - - trust_region_radii(i) * trust_region_radii(i); - assert(c <= 0); // Inside the trust region, so c <= 0 + // 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) + : static_cast(coll_id); + }; - // Roots must be real - assert(b * b - 4 * a * c >= 0); + // 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 double sign_b = (b >= 0) ? 1.0 : -1.0; - const double sqrt_d = sqrt(b * b - 4 * a * c); + 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; + } - // β = (-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); + 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)); } - // β should be the positive root - assert(beta > 0); + } + 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)) { } + } + }); - dx.row(i).array() *= beta; - assert(approx( - (xi + dx.row(i).transpose() - ci).norm(), - trust_region_radii(i))); + // 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); + } - num_updates++; + // ---- 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(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(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); } - } + }); - // If a critical mass of vertices are restricted by the trust region, - // we update the trust region to avoid excessive filtering. + // 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 trust region. Updating trust region.", + "{:.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( + 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) { + 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 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; +} + } // 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..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. @@ -97,6 +101,46 @@ 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 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). + void planar_filter_step( + const CollisionMesh& mesh, + Eigen::ConstRef x, + Eigen::Ref dx); + +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. + /// @return Truncation ratio in (0, 1]. + double planar_truncation_ratio( + 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/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); diff --git a/tests/src/tests/ogc/test_trust_region.cpp b/tests/src/tests/ogc/test_trust_region.cpp index 8270d1bfe..e23ec8879 100644 --- a/tests/src/tests/ogc/test_trust_region.cpp +++ b/tests/src/tests/ogc/test_trust_region.cpp @@ -8,6 +8,57 @@ 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 +185,406 @@ 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 capped by trust region", + "[ogc][planar_dat]") +{ + // 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); + + 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; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // 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); + + // 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, 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 by planar DAT", + "[ogc][planar_dat]") +{ + // 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); + + 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; + Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3); + // Vertex 3 moves sideways (contact normal is (0,1,0), dx is in xz-plane) + // 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); + + // 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]") +{ + // 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); + tr.update(mesh, V, collisions); + + 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); + + // 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 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); + + 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()); + + // 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; + + // 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); + 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, 0.3, 0.0; + tr.filter_step(mesh, x, dx_iso); + CHECK(dx_iso(3, 1) < 0.3 - 1e-10); +} + +// 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 }; +} + +// 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( + "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); + tr.update(mesh, V, collisions); + + 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); + + // 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); +} + +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(!tr.candidates.vv_candidates.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); + + // 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(!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); + + // 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); + 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]") +{ + // 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 + // 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(!tr.candidates.fv_candidates.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); + + // 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(!tr.candidates.ev_candidates.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); + + const double final_y = x(2, 1) + dx(2, 1); + CHECK(final_y > 0.0); + CHECK(std::abs(dx(2, 1)) < 0.6); +} 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 // ============================================================================