Add Support for Codim. Edge-Point Collision in 3D (#79)

zfergus · web-flow · commit 52028df22fd7 · 2023-12-11T16:40:37.000-05:00
* Add codim_edges to CollisionMesh

* Add implementation of CE2CV contacts

* Test codim. edge-vertex
diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp
@@ -67,6 +67,9 @@ void define_collision_mesh(py::module_& m)
         .def_property_readonly(
             "num_codim_vertices", &CollisionMesh::num_codim_vertices,
             "Get the number of codimensional vertices in the collision mesh.")
+        .def_property_readonly(
+            "num_codim_edges", &CollisionMesh::num_codim_edges,
+            "Get the number of codimensional edges in the collision mesh.")
         .def_property_readonly(
             "num_edges", &CollisionMesh::num_edges,
             "Get the number of edges in the collision mesh.")
@@ -90,6 +93,9 @@ void define_collision_mesh(py::module_& m)
         .def_property_readonly(
             "codim_vertices", &CollisionMesh::codim_vertices,
             "Get the indices of codimensional vertices of the collision mesh (#CV x 1).")
+        .def_property_readonly(
+            "codim_edges", &CollisionMesh::codim_edges,
+            "Get the indices of codimensional edges of the collision mesh (#CE x 1).")
         .def_property_readonly(
             "edges", &CollisionMesh::edges,
             "Get the edges of the collision mesh (#E × 2).")
@@ -303,8 +309,8 @@ void define_collision_mesh(py::module_& m)
         .def_readwrite(
             "can_collide", &CollisionMesh::can_collide,
             R"ipc_Qu8mg5v7(
-            A function that takes two vertex IDs and returns true if the vertices
-(and faces or edges containing the vertices) can collide. By default all
-primitives can collide with all other primitives.
+            A function that takes two vertex IDs and returns true if the vertices (and faces or edges containing the vertices) can collide.
+
+            By default all primitives can collide with all other primitives.
             )ipc_Qu8mg5v7");
 }
diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp
@@ -5,6 +5,7 @@
 
 #include <ipc/config.hpp>
 
+#include <igl/remove_unreferenced.h>
 #include <tbb/parallel_for.h>
 #include <tbb/blocked_range.h>
 #include <shared_mutex>
@@ -19,6 +20,21 @@ namespace {
         return method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE
             && method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE_GPU;
     }
+
+    // Pad codim_edges because remove_unreferenced requires a N×3 matrix.
+    Eigen::MatrixXi pad_edges(const Eigen::MatrixXi& E)
+    {
+        assert(E.cols() == 2);
+        Eigen::MatrixXi E_padded(E.rows(), 3);
+        E_padded.leftCols(2) = E;
+        E_padded.col(2) = E.col(1);
+        return E_padded;
+    }
+
+    Eigen::MatrixXi unpad_edges(const Eigen::MatrixXi& E_padded)
+    {
+        return E_padded.leftCols(2);
+    }
 } // namespace
 
 void Candidates::build(
@@ -37,23 +53,67 @@ void Candidates::build(
     broad_phase->build(vertices, mesh.edges(), mesh.faces(), inflation_radius);
     broad_phase->detect_collision_candidates(dim, *this);
 
-    if (mesh.num_codim_vertices()) {
-        if (!implements_vertex_vertex(broad_phase_method)) {
-            logger().warn(
-                "STQ broad phase does not support codim. point-point, skipping.");
-            return;
-        }
+    if (mesh.num_codim_vertices()
+        && !implements_vertex_vertex(broad_phase_method)) {
+        // TODO: Assumes this is the same as implements_edge_vertex
+        logger().warn(
+            "STQ broad phase does not support codim. point-point nor point-edge, skipping.");
+        return;
+    }
 
+    // Codim. vertices to codim. vertices:
+    if (mesh.num_codim_vertices()) {
         broad_phase->clear();
         broad_phase->build(
             vertices(mesh.codim_vertices(), Eigen::all), //
             Eigen::MatrixXi(), Eigen::MatrixXi(), inflation_radius);
+
         broad_phase->detect_vertex_vertex_candidates(vv_candidates);
         for (auto& [vi, vj] : vv_candidates) {
             vi = mesh.codim_vertices()[vi];
             vj = mesh.codim_vertices()[vj];
         }
     }
+
+    // Codim. edges to codim. vertices:
+    // Only need this in 3D because in 2D, the codim. edges are the same as the
+    // edges of the boundary. Only need codim. edge to codim. vertex because
+    // codim. edge to non-codim. vertex is the same as edge-edge or face-vertex.
+    if (dim == 3 && mesh.num_codim_vertices() && mesh.num_codim_edges()) {
+        // Extract the vertices of the codim. edges
+        Eigen::MatrixXd CE_V; // vertices of codim. edges
+        Eigen::MatrixXi CE;   // codim. edges (indices into CEV)
+        {
+            Eigen::VectorXi _I, _J; // unused mappings
+            igl::remove_unreferenced(
+                vertices,
+                pad_edges(mesh.edges()(mesh.codim_edges(), Eigen::all)), CE_V,
+                CE, _I, _J);
+            CE = unpad_edges(CE);
+        }
+
+        const size_t nCV = mesh.num_codim_vertices();
+        Eigen::MatrixXd V(nCV + CE_V.rows(), dim);
+        V.topRows(nCV) = vertices(mesh.codim_vertices(), Eigen::all);
+        V.bottomRows(CE_V.rows()) = CE_V;
+
+        CE.array() += nCV; // Offset indices to account for codim. vertices
+
+        // TODO: Can we reuse the broad phase from above?
+        broad_phase->clear();
+        broad_phase->can_vertices_collide = [&](size_t vi, size_t vj) {
+            // Ignore c-edge to c-edge and c-vertex to c-vertex
+            return ((vi < nCV) ^ (vj < nCV)) && mesh.can_collide(vi, vj);
+        };
+        broad_phase->build(V, CE, Eigen::MatrixXi(), inflation_radius);
+
+        broad_phase->detect_edge_vertex_candidates(ev_candidates);
+        for (auto& [ei, vi] : ev_candidates) {
+            assert(vi < mesh.codim_vertices().size());
+            ei = mesh.codim_edges()[ei];    // Map back to mesh.edges
+            vi = mesh.codim_vertices()[vi]; // Map back to vertices
+        }
+    }
 }
 
 void Candidates::build(
@@ -74,24 +134,74 @@ void Candidates::build(
         vertices_t0, vertices_t1, mesh.edges(), mesh.faces(), inflation_radius);
     broad_phase->detect_collision_candidates(dim, *this);
 
-    if (mesh.num_codim_vertices()) {
-        if (!implements_vertex_vertex(broad_phase_method)) {
-            logger().warn(
-                "STQ broad phase does not support codim. point-point, skipping.");
-            return;
-        }
+    if (mesh.num_codim_vertices()
+        && !implements_vertex_vertex(broad_phase_method)) {
+        // TODO: Assumes this is the same as implements_edge_vertex
+        logger().warn(
+            "STQ broad phase does not support codim. point-point nor point-edge, skipping.");
+        return;
+    }
 
+    // Codim. vertices to codim. vertices:
+    if (mesh.num_codim_vertices()) {
         broad_phase->clear();
         broad_phase->build(
             vertices_t0(mesh.codim_vertices(), Eigen::all),
             vertices_t1(mesh.codim_vertices(), Eigen::all), //
             Eigen::MatrixXi(), Eigen::MatrixXi(), inflation_radius);
+
         broad_phase->detect_vertex_vertex_candidates(vv_candidates);
         for (auto& [vi, vj] : vv_candidates) {
             vi = mesh.codim_vertices()[vi];
             vj = mesh.codim_vertices()[vj];
         }
     }
+
+    // Codim. edges to codim. vertices:
+    // Only need this in 3D because in 2D, the codim. edges are the same as the
+    // edges of the boundary. Only need codim. edge to codim. vertex because
+    // codim. edge to non-codim. vertex is the same as edge-edge or face-vertex.
+    if (dim == 3 && mesh.num_codim_vertices() && mesh.num_codim_edges()) {
+        // Extract the vertices of the codim. edges
+        Eigen::MatrixXd CE_V_t0, CE_V_t1; // vertices of codim. edges
+        Eigen::MatrixXi CE;               // codim. edges (indices into CEV)
+        {
+            Eigen::VectorXi _I, J;
+            igl::remove_unreferenced(
+                vertices_t0,
+                pad_edges(mesh.edges()(mesh.codim_edges(), Eigen::all)),
+                CE_V_t0, CE, _I, J);
+            CE_V_t1 = vertices_t1(J, Eigen::all);
+            CE = unpad_edges(CE);
+        }
+
+        const size_t nCV = mesh.num_codim_vertices();
+
+        Eigen::MatrixXd V_t0(nCV + CE_V_t0.rows(), dim);
+        V_t0.topRows(nCV) = vertices_t0(mesh.codim_vertices(), Eigen::all);
+        V_t0.bottomRows(CE_V_t0.rows()) = CE_V_t0;
+
+        Eigen::MatrixXd V_t1(nCV + CE_V_t1.rows(), dim);
+        V_t1.topRows(nCV) = vertices_t1(mesh.codim_vertices(), Eigen::all);
+        V_t1.bottomRows(CE_V_t1.rows()) = CE_V_t1;
+
+        CE.array() += nCV; // Offset indices to account for codim. vertices
+
+        // TODO: Can we reuse the broad phase from above?
+        broad_phase->clear();
+        broad_phase->can_vertices_collide = [&](size_t vi, size_t vj) {
+            // Ignore c-edge to c-edge and c-vertex to c-vertex
+            return ((vi < nCV) ^ (vj < nCV)) && mesh.can_collide(vi, vj);
+        };
+        broad_phase->build(V_t0, V_t1, CE, Eigen::MatrixXi(), inflation_radius);
+
+        broad_phase->detect_edge_vertex_candidates(ev_candidates);
+        for (auto& [ei, vi] : ev_candidates) {
+            assert(vi < mesh.codim_vertices().size());
+            ei = mesh.codim_edges()[ei];    // Map back to mesh.edges
+            vi = mesh.codim_vertices()[vi]; // Map back to vertices
+        }
+    }
 }
 
 bool Candidates::is_step_collision_free(
@@ -110,8 +220,7 @@ bool Candidates::is_step_collision_free(
         double toi;
         bool is_collision = (*this)[i].ccd(
             vertices_t0, vertices_t1, mesh.edges(), mesh.faces(), toi,
-            min_distance,
-            /*tmax=*/1.0, tolerance, max_iterations);
+            min_distance, /*tmax=*/1.0, tolerance, max_iterations);
 
         if (is_collision) {
             return false;
diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp
@@ -106,6 +106,7 @@ CollisionMesh::CollisionMesh(
     m_faces_to_edges = construct_faces_to_edges(m_faces, m_edges);
 
     init_codim_vertices();
+    init_codim_edges();
     init_areas();
     init_adjacencies();
     // Compute these manually if needed.
@@ -134,6 +135,26 @@ void CollisionMesh::init_codim_vertices()
     assert(j == m_codim_vertices.size());
 }
 
+void CollisionMesh::init_codim_edges()
+{
+    std::vector<bool> is_codim_edge(num_edges(), true);
+    for (int i : m_faces_to_edges.reshaped()) {
+        is_codim_edge[i] = false;
+    }
+
+    m_codim_edges.resize(
+        std::count(is_codim_edge.begin(), is_codim_edge.end(), true));
+
+    int j = 0;
+    for (int i = 0; i < num_edges(); i++) {
+        if (is_codim_edge[i]) {
+            assert(j < m_codim_edges.size());
+            m_codim_edges[j++] = i;
+        }
+    }
+    assert(j == m_codim_edges.size());
+}
+
 // ============================================================================
 
 void CollisionMesh::init_selection_matrices(const int dim)
diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp
@@ -72,6 +72,9 @@ class CollisionMesh {
     /// @brief Get the number of codimensional vertices in the collision mesh.
     size_t num_codim_vertices() const { return codim_vertices().size(); }
 
+    /// @brief Get the number of codimensional edges in the collision mesh.
+    size_t num_codim_edges() const { return codim_edges().size(); }
+
     /// @brief Get the number of edges in the collision mesh.
     size_t num_edges() const { return edges().rows(); }
 
@@ -96,6 +99,9 @@ class CollisionMesh {
     /// @brief Get the indices of codimensional vertices of the collision mesh (#CV x 1).
     const Eigen::VectorXi& codim_vertices() const { return m_codim_vertices; }
 
+    /// @brief Get the indices of codimensional edges of the collision mesh (#CE x 1).
+    const Eigen::VectorXi& codim_edges() const { return m_codim_edges; }
+
     /// @brief Get the edges of the collision mesh (#E × 2).
     const Eigen::MatrixXi& edges() const { return m_edges; }
 
@@ -283,6 +289,7 @@ class CollisionMesh {
     // Helper initialization functions
 
     void init_codim_vertices();
+    void init_codim_edges();
 
     /// @brief Initialize the selection matrix from full vertices/DOF to collision vertices/DOF.
     void init_selection_matrices(const int dim);
@@ -302,6 +309,8 @@ class CollisionMesh {
     Eigen::MatrixXd m_rest_positions;
     /// @brief The indices of codimensional vertices (#CV x 1).
     Eigen::VectorXi m_codim_vertices;
+    /// @brief The indices of codimensional edges (#CE x 1).
+    Eigen::VectorXi m_codim_edges;
     /// @brief Edges as rows of indicies into vertices (#E × 2).
     Eigen::MatrixXi m_edges;
     /// @brief Triangular faces as rows of indicies into vertices (#F × 3).
diff --git a/tests/src/tests/collisions/test_constraints.cpp b/tests/src/tests/collisions/test_constraints.cpp
