Adjust planar_filter_step to better handle parallel edges

CMakePresets.json

@@ -118,13 +118,13 @@
       "description": "Debug build with CUDA support"
     },
     {
-      "name": "test-build",
+      "name": "test",
       "configurePreset": "test",
       "displayName": "Unit Tests",
       "description": "Build for running tests"
     },
     {
-      "name": "python-build",
+      "name": "python",
       "configurePreset": "python",
       "displayName": "Python Bindings",
       "description": "Build with Python bindings enabled"

src/ipc/candidates/candidates.cpp

@@ -540,6 +540,41 @@ const CollisionStencil& Candidates::operator[](size_t i) const
     throw std::out_of_range("Candidate index is out of range!");
 }
 
+bool Candidates::is_vertex_vertex(size_t i) const
+{
+    return i < vv_candidates.size();
+}
+
+bool Candidates::is_edge_vertex(size_t i) const
+{
+    return i >= vv_candidates.size()
+        && i < vv_candidates.size() + ev_candidates.size();
+}
+
+bool Candidates::is_edge_edge(size_t i) const
+{
+    return i >= vv_candidates.size() + ev_candidates.size()
+        && i
+        < vv_candidates.size() + ev_candidates.size() + ee_candidates.size();
+}
+
+bool Candidates::is_face_vertex(size_t i) const
+{
+    return i
+        >= vv_candidates.size() + ev_candidates.size() + ee_candidates.size()
+        && i < vv_candidates.size() + ev_candidates.size()
+            + ee_candidates.size() + fv_candidates.size();
+}
+
+bool Candidates::is_plane_vertex(size_t i) const
+{
+    return i >= vv_candidates.size() + ev_candidates.size()
+            + ee_candidates.size() + fv_candidates.size()
+        && i < vv_candidates.size() + ev_candidates.size()
+            + ee_candidates.size() + fv_candidates.size()
+            + pv_candidates.size();
+}
+
 // == Convert to subelement candidates ========================================
 
 namespace {

src/ipc/candidates/candidates.hpp

@@ -64,6 +64,31 @@ class Candidates {
     /// @return A const reference to the collision stencil.
     const CollisionStencil& operator[](size_t i) const;
 
+    /// @brief Get if the collision at i is a vertex-vertex collision.
+    /// @param i The index of the collision.
+    /// @return If the collision at i is a vertex-vertex collision.
+    bool is_vertex_vertex(size_t i) const;
+
+    /// @brief Get if the collision at i is an edge-vertex collision.
+    /// @param i The index of the collision.
+    /// @return If the collision at i is an edge-vertex collision.
+    bool is_edge_vertex(size_t i) const;
+
+    /// @brief Get if the collision at i is an edge-edge collision.
+    /// @param i The index of the collision.
+    /// @return If the collision at i is an edge-edge collision.
+    bool is_edge_edge(size_t i) const;
+
+    /// @brief Get if the collision at i is a face-vertex collision.
+    /// @param i The index of the collision.
+    /// @return If the collision at i is a face-vertex collision.
+    bool is_face_vertex(size_t i) const;
+
+    /// @brief Get if the collision at i is a plane-vertex collision.
+    /// @param i The index of the collision.
+    /// @return If the collision at i is a plane-vertex collision.
+    bool is_plane_vertex(size_t i) const;
+
     /// @brief Determine if the step is collision free from the set of candidates.
     /// @note Assumes the trajectory is linear.
     /// @param mesh The collision mesh.

src/ipc/ogc/trust_region.cpp

@@ -165,7 +165,7 @@ namespace {
             beta = (-b + sqrt_d) / (2 * a);
         }
         // β should be the positive root
-        assert(beta > 0);
+        assert(beta >= 0);
 
         return beta;
     }
@@ -258,8 +258,11 @@ void TrustRegion::planar_filter_step(
 
         const VectorMax3d n = dv / dist;
 
+        // Compute the closest points on the two primitives: c_first/c_second.
         VectorMax3d c_first = VectorMax3d::Zero(d);
         VectorMax3d c_second = VectorMax3d::Zero(d);
+        // Compute the approach velocity of the vertices relative to the
+        // division plane: max(-Δxⱼ⋅n)
         double delta_first = 0, delta_second = 0;
 
         for (int j = 0; j < nv; ++j) {
@@ -277,12 +280,29 @@ void TrustRegion::planar_filter_step(
         delta_first = std::max(delta_first, 0.0);
         delta_second = std::max(delta_second, 0.0);
 
+        // Skip if the approach velocity is negligible relative to the distance.
+        // This prevents numerical noise in n from nearly-parallel edges from
+        // truncating uniform displacements.
+        constexpr double APPROACH_TOL = 1e-3;
+        if (candidates.is_edge_edge(i)
+            && delta_first + delta_second <= APPROACH_TOL * dist) {
+            // Check that the edges are nearly parallel:
+            constexpr double EE_PARALLEL_TOL_SQ = 1e-6; // sin of ~0.057°
+            const Eigen::Vector3d ea = pos.segment<3>(3) - pos.segment<3>(0);
+            const Eigen::Vector3d eb = pos.segment<3>(9) - pos.segment<3>(6);
+            const double sin_angle_sq = ea.cross(eb).squaredNorm()
+                / (ea.squaredNorm() * eb.squaredNorm());
+            if (sin_angle_sq < EE_PARALLEL_TOL_SQ) {
+                return;
+            }
+        }
+
         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).
+        // Point on the 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) {

tests/src/tests/ogc/test_trust_region.cpp

@@ -4,6 +4,8 @@
 #include <ipc/collision_mesh.hpp>
 #include <ipc/collisions/normal/normal_collisions.hpp>
 
+#include <igl/edges.h>
+
 #include <Eigen/Dense>
 
 using namespace ipc;
@@ -588,3 +590,126 @@ TEST_CASE(
     CHECK(final_y > 0.0);
     CHECK(std::abs(dx(2, 1)) < 0.6);
 }
+
+// Helper: (N+1)×(N+1) cloth grid in the xz-plane at y = y_offset.
+// Vertices are on a regular grid; edges are extracted from a triangulation.
+static std::tuple<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXi>
+make_grid_layer(int N, double spacing, double y_offset)
+{
+    const int nv = (N + 1) * (N + 1);
+    Eigen::MatrixXd V(nv, 3);
+    for (int i = 0; i <= N; ++i) {
+        for (int j = 0; j <= N; ++j) {
+            V.row(i * (N + 1) + j) << j * spacing, y_offset, i * spacing;
+        }
+    }
+
+    Eigen::MatrixXi F(2 * N * N, 3);
+    int f = 0;
+    for (int i = 0; i < N; ++i) {
+        for (int j = 0; j < N; ++j) {
+            const int a = i * (N + 1) + j, b = a + 1;
+            const int c = (i + 1) * (N + 1) + j, d = c + 1;
+            F.row(f++) << a, b, d;
+            F.row(f++) << a, d, c;
+        }
+    }
+
+    Eigen::MatrixXi E;
+    igl::edges(F, E);
+    return { V, E, F };
+}
+
+TEST_CASE(
+    "planar_filter_step: stacked parallel cloth layers", "[ogc][planar_dat]")
+{
+    // Flat cloth grids stacked at y = gap * i (layer 0 at bottom).
+    // Even-indexed layers (0, 2, ...) move downward (−y);
+    // odd-indexed layers (1, 3, ...) move upward (+y).
+    // Adjacent pairs where an odd layer is directly below an even layer
+    // (e.g. layers 1 and 2) approach each other and must be truncated.
+    // Adjacent pairs where an even layer is below an odd layer (e.g. 0 and 1)
+    // move apart and must not be spuriously truncated.
+
+    const int N_LAYERS = GENERATE(range(1, 5)); // test 1 to 4 layers
+
+#ifdef NDEBUG
+    const int N = GENERATE(2, 10);
+#else
+    const int N = 2; // smaller N for debug mode to keep test fast
+#endif
+    const int NV = (N + 1) * (N + 1);
+    constexpr double spacing = 0.1;
+    constexpr double gap = 0.3;   // y-separation between adjacent layers
+    constexpr double dhat = 0.35; // > gap so adjacent layers are active
+    constexpr double step = 0.2;  // approach speed; combined 0.4 > gap
+
+    Eigen::MatrixXd V;
+    Eigen::MatrixXi E, F;
+    for (int i = 0; i < N_LAYERS; ++i) {
+        auto [Vi, Ei, Fi] = make_grid_layer(N, spacing, gap * i);
+        const int prev_nv = static_cast<int>(V.rows());
+        V.conservativeResize(V.rows() + Vi.rows(), 3);
+        E.conservativeResize(E.rows() + Ei.rows(), 2);
+        F.conservativeResize(F.rows() + Fi.rows(), 3);
+        V.bottomRows(Vi.rows()) = Vi;
+        E.bottomRows(Ei.rows()) = Ei.array() + prev_nv;
+        F.bottomRows(Fi.rows()) = Fi.array() + prev_nv;
+    }
+
+    CollisionMesh mesh(V, E, F);
+
+    ipc::ogc::TrustRegion tr(dhat);
+    ipc::NormalCollisions collisions;
+    collisions.set_collision_set_type(
+        ipc::NormalCollisions::CollisionSetType::OGC);
+    tr.update(mesh, V, collisions);
+
+    if (N_LAYERS > 1) {
+        REQUIRE(!collisions.empty());
+    }
+
+    Eigen::MatrixXd x = V;
+    Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(V.rows(), 3);
+    for (int i = 0; i < N_LAYERS; ++i) {
+        const double dir = (i % 2 == 0) ? -1.0 : 1.0; // even down, odd up
+        dx.middleRows(i * NV, NV).col(1).array() = dir * step;
+    }
+
+    tr.planar_filter_step(mesh, x, dx);
+
+    // Every layer must still move in its original direction — intra-layer
+    // parallel-edge pairs must not produce spurious truncations.
+    for (int i = 0; i < N_LAYERS; ++i) {
+        for (int j = 0; j < NV; ++j) {
+            const int k = NV * i + j;
+            CHECK(dx(k, 0) == Catch::Approx(0.0));
+            CHECK(dx(k, 2) == Catch::Approx(0.0));
+            if (i % 2 == 0) {
+                CHECK(dx(k, 1) < 0.0); // even: downward
+            } else {
+                CHECK(dx(k, 1) > 0.0); // odd: upward
+            }
+        }
+    }
+
+    // Single layer: no collision pairs, so the step must be completely
+    // unmodified.
+    if (N_LAYERS == 1) {
+        for (int j = 0; j < NV; ++j) {
+            CHECK(dx(j, 1) == Catch::Approx(-step));
+        }
+    }
+
+    // Approaching pairs (odd layer i below even layer i+1): step must be
+    // truncated and the result must be penetration-free.
+    const Eigen::MatrixXd x_new = x + dx;
+    for (int i = 1; i + 1 < N_LAYERS; i += 2) {
+        CHECK(dx(i * NV, 1) < step - 1e-10);
+
+        const double max_yi = x_new.middleRows(i * NV, NV).col(1).maxCoeff();
+        const double min_yi1 =
+            x_new.middleRows((i + 1) * NV, NV).col(1).minCoeff();
+        CHECK(min_yi1 > max_yi);
+    }
+}