From 961461e06baa3819ca03b70971f243af1c2a3434 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 17 Oct 2023 13:27:55 -0400 Subject: [PATCH 1/4] Add codim_edges to CollisionMesh --- python/src/collision_mesh.cpp | 52 ++++++++++++++++++++----------- src/ipc/candidates/candidates.cpp | 23 +++++++++++--- src/ipc/collision_mesh.cpp | 21 +++++++++++++ src/ipc/collision_mesh.hpp | 9 ++++++ 4 files changed, 83 insertions(+), 22 deletions(-) diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index 36bbd9c4..b51ecfc4 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -47,7 +47,7 @@ void define_collision_mesh(py::module_& m) Helper function that automatically builds include_vertex using construct_is_on_surface. Parameters: - full_rest_positions: The full vertices at rest (#V × dim). + full_rest_positions: The full vertices at rest (#FV × dim). edges: The edge matrix of mesh (#E × 2). faces: The face matrix of mesh (#F × 3). @@ -64,6 +64,12 @@ void define_collision_mesh(py::module_& m) .def_property_readonly( "num_vertices", &CollisionMesh::num_vertices, "Get the number of vertices in the collision mesh.") + .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.") @@ -83,9 +89,15 @@ void define_collision_mesh(py::module_& m) .def_property_readonly( "rest_positions", &CollisionMesh::rest_positions, "Get the vertices of the collision mesh at rest (#V × dim).") + .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).") + "Get the edges of the collision mesh (#E × 2).") .def_property_readonly( "faces", &CollisionMesh::faces, "Get the faces of the collision mesh (#F × 3).") @@ -107,13 +119,13 @@ void define_collision_mesh(py::module_& m) .def( "displace_vertices", &CollisionMesh::displace_vertices, R"ipc_Qu8mg5v7( - Compute the vertex positions from vertex displacements on the full mesh (#FV × dim). + Compute the vertex positions from vertex displacements on the full mesh. Parameters: - full_displacements: The vertex displacements on the full mesh (#V × dim). + full_displacements: The vertex displacements on the full mesh (#FV × dim). Returns: - The vertex positions of the collision mesh. + The vertex positions of the collision mesh (#V × dim). )ipc_Qu8mg5v7", py::arg("full_displacements")) .def( @@ -122,10 +134,10 @@ void define_collision_mesh(py::module_& m) Map vertex displacements on the full mesh to vertex displacements on the collision mesh. Parameters: - full_displacements: The vertex displacements on the full mesh. + full_displacements: The vertex displacements on the full mesh (#FV × dim). Returns: - The vertex displacements on the collision mesh. + The vertex displacements on the collision mesh (#V × dim). )ipc_Qu8mg5v7", py::arg("full_displacements")) .def( @@ -172,10 +184,13 @@ void define_collision_mesh(py::module_& m) Matrix quantity on the full mesh with size equal to full_ndof() × full_ndof(). )ipc_Qu8mg5v7", py::arg("X")) - .def_property_readonly( + .def( "vertex_vertex_adjacencies", &CollisionMesh::vertex_vertex_adjacencies, "Get the vertex-vertex adjacency matrix.") + .def_property_readonly( + "vertex_edge_adjacencies", &CollisionMesh::vertex_edge_adjacencies, + "Get the vertex-edge adjacency matrix.") .def_property_readonly( "edge_vertex_adjacencies", &CollisionMesh::edge_vertex_adjacencies, "Get the edge-vertex adjacency matrix.") @@ -217,13 +232,13 @@ void define_collision_mesh(py::module_& m) return self.vertex_area_gradient(vi); }, R"ipc_Qu8mg5v7( - Get the gradient of the barycentric area of a vertex wrt the rest - positions of all points. + Get the gradient of the barycentric area of a vertex wrt the rest positions of all points. + Parameters: vi: Vertex ID. + Returns: - Gradient of the barycentric area of vertex vi wrt the rest - positions of all points. + Gradient of the barycentric area of vertex vi wrt the rest positions of all points. )ipc_Qu8mg5v7", py::arg("vi")) .def( @@ -248,13 +263,13 @@ void define_collision_mesh(py::module_& m) return self.edge_area_gradient(ei); }, R"ipc_Qu8mg5v7( - Get the gradient of the barycentric area of an edge wrt the rest - positions of all points. + Get the gradient of the barycentric area of an edge wrt the rest positions of all points. + Parameters: ei: Edge ID. + Returns: - Gradient of the barycentric area of edge ei wrt the rest - positions of all points. + Gradient of the barycentric area of edge ei wrt the rest positions of all points. )ipc_Qu8mg5v7", py::arg("ei")) .def( @@ -269,6 +284,7 @@ void define_collision_mesh(py::module_& m) Parameters: num_vertices: The number of vertices in the mesh. edges: The surface edges of the mesh (#E × 2). + codim_vertices: The indices of codimensional vertices (#CV x 1). Returns: A vector of bools indicating whether each vertex is on the surface. @@ -293,7 +309,7 @@ void define_collision_mesh(py::module_& m) "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. +(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 index 9dccf7dd..c2734a61 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -38,19 +38,34 @@ void Candidates::build( if (mesh.num_codim_vertices()) { if (!implements_vertex_vertex(broad_phase_method)) { logger().warn( - "STQ broad phase does not support codim. point-point, skipping."); + "STQ broad phase does not support codim. point-point nor point-edge, skipping."); return; } + const Eigen::MatrixXd CV = vertices(mesh.codim_vertices(), Eigen::all); + + Eigen::MatrixXi CE; + // TODO: This will now work because CE refers to indices of V not CV. + // if (mesh.dim() == 3 && mesh.num_codim_edges()) { + // CE = mesh.edges()(mesh.codim_edges(), Eigen::all); + // } + broad_phase->clear(); - broad_phase->build( - vertices(mesh.codim_vertices(), Eigen::all), // - Eigen::MatrixXi(), Eigen::MatrixXi(), inflation_radius); + broad_phase->build(CV, CE, 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]; } + + if (CE.size() > 0) { + broad_phase->detect_edge_vertex_candidates(ev_candidates); + for (auto& [ei, vi] : ev_candidates) { + ei = mesh.codim_edges()[ei]; + vi = mesh.codim_vertices()[vi]; + } + } } } diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 187bd31d..bc074795 100644 --- 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 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 index ace5e6b7..15ace75a 100644 --- 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). From 796042d59b1ab47b225774836a9f0e6b9d0a1b80 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 15 Nov 2023 23:00:15 -0500 Subject: [PATCH 2/4] def_property_readonly --- python/src/collision_mesh.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index 0f972087..86664782 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -185,7 +185,7 @@ void define_collision_mesh(py::module_& m) Matrix quantity on the full mesh with size equal to full_ndof() × full_ndof(). )ipc_Qu8mg5v7", py::arg("X")) - .def( + .def_property_readonly( "vertex_vertex_adjacencies", &CollisionMesh::vertex_vertex_adjacencies, "Get the vertex-vertex adjacency matrix.") From e97627dd5c3dee84823fc45f61287bd573c51800 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 11 Dec 2023 14:27:59 -0500 Subject: [PATCH 3/4] Add implementation of CE2CV contacts --- src/ipc/candidates/candidates.cpp | 75 ++++++++++++++++++++++--------- 1 file changed, 54 insertions(+), 21 deletions(-) diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index f8b270b4..73110d5d 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -5,6 +5,7 @@ #include +#include #include #include #include @@ -37,36 +38,68 @@ 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 nor point-edge, skipping."); - return; - } - - const Eigen::MatrixXd CV = vertices(mesh.codim_vertices(), Eigen::all); - - Eigen::MatrixXi CE; - // TODO: This will now work because CE refers to indices of V not CV. - // if (mesh.dim() == 3 && mesh.num_codim_edges()) { - // CE = mesh.edges()(mesh.codim_edges(), Eigen::all); - // } + if (!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(CV, CE, Eigen::MatrixXi(), inflation_radius); + 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]; } + } - if (CE.size() > 0) { - broad_phase->detect_edge_vertex_candidates(ev_candidates); - for (auto& [ei, vi] : ev_candidates) { - ei = mesh.codim_edges()[ei]; - vi = mesh.codim_vertices()[vi]; - } + // 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) + { + // Pad codim_edges with -1 because remove_unreferenced requires a + // N×3 matrix. + Eigen::MatrixXi CE_padded(mesh.num_codim_edges(), 2); + CE_padded.leftCols(CE.cols()) = CE; + CE_padded.col(2).setConstant(-1); + + Eigen::VectorXi _I, _J; // unused mappings + igl::remove_unreferenced(vertices, CE_padded, CE_V, CE, _I, _J); + + CE = CE.leftCols(2); // Remove padding + } + + 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 } } } From 59c42521559d3d85da3d150ca57b9c3f97a0fb68 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 11 Dec 2023 15:56:31 -0500 Subject: [PATCH 4/4] Test codim edge-vertex --- src/ipc/candidates/candidates.cpp | 97 +++++++++++++++---- .../src/tests/collisions/test_constraints.cpp | 96 ++++++++++++++++++ 2 files changed, 175 insertions(+), 18 deletions(-) diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 73110d5d..bbed6234 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -20,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( @@ -38,7 +53,8 @@ void Candidates::build( broad_phase->build(vertices, mesh.edges(), mesh.faces(), inflation_radius); broad_phase->detect_collision_candidates(dim, *this); - if (!implements_vertex_vertex(broad_phase_method)) { + 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."); @@ -68,16 +84,12 @@ void Candidates::build( Eigen::MatrixXd CE_V; // vertices of codim. edges Eigen::MatrixXi CE; // codim. edges (indices into CEV) { - // Pad codim_edges with -1 because remove_unreferenced requires a - // N×3 matrix. - Eigen::MatrixXi CE_padded(mesh.num_codim_edges(), 2); - CE_padded.leftCols(CE.cols()) = CE; - CE_padded.col(2).setConstant(-1); - Eigen::VectorXi _I, _J; // unused mappings - igl::remove_unreferenced(vertices, CE_padded, CE_V, CE, _I, _J); - - CE = CE.leftCols(2); // Remove padding + 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(); @@ -122,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( @@ -158,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/tests/src/tests/collisions/test_constraints.cpp b/tests/src/tests/collisions/test_constraints.cpp index d39df18c..5409a922 100644 --- a/tests/src/tests/collisions/test_constraints.cpp +++ b/tests/src/tests/collisions/test_constraints.cpp @@ -227,6 +227,102 @@ TEST_CASE("Codim. Vertex-Vertex Constraints", "[constraints][codim]") } } +TEST_CASE("Codim. Edge-Vertex Constraints", "[constraints][codim]") +{ + constexpr double thickness = 1e-3; + constexpr double min_distance = 2 * thickness; + + Eigen::MatrixXd V(8, 3); + V << 0, 0, 0, // + 1, 0, 0, // + 0, 0, -1, // + -1, 0, 0, // + 0, 0, 1, // + 0, 1, 0, // + 0, 2, 0, // + 0, 3, 0; + Eigen::MatrixXi E(4, 2); + E << 0, 1, // + 0, 2, // + 0, 3, // + 0, 4; + + CollisionMesh mesh(V, E, Eigen::MatrixXi()); + mesh.init_area_jacobians(); + + CHECK(mesh.num_vertices() == 8); + CHECK(mesh.num_codim_vertices() == 3); + CHECK(mesh.num_codim_edges() == 4); + CHECK(mesh.num_edges() == 4); + CHECK(mesh.num_faces() == 0); + + const BroadPhaseMethod method = GENERATE_BROAD_PHASE_METHODS(); + CAPTURE(method); + + // These methods do not support vertex-vertex candidates + if (method == ipc::BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE + || method == ipc::BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE_GPU) { + return; + } + + SECTION("Candidates") + { + Eigen::MatrixXd V1 = V; + V1.bottomRows(3).col(1).array() -= 4; // Translate the codim vertices + + Candidates candidates; + candidates.build(mesh, V, V1, thickness, method); + + CHECK(candidates.size() == 15); + CHECK(candidates.vv_candidates.size() == 3); + CHECK(candidates.ev_candidates.size() == 12); + CHECK(candidates.ee_candidates.size() == 0); + CHECK(candidates.fv_candidates.size() == 0); + + CHECK(!candidates.is_step_collision_free(mesh, V, V1, min_distance)); + + // Account for conservative rescaling +#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD + constexpr double conservative_min_dist = 1e-4; +#else + constexpr double conservative_min_dist = 0.2 * (1 - min_distance); +#endif + constexpr double expected_toi = + (1 - (min_distance + conservative_min_dist)) / 4; + CHECK( + candidates.compute_collision_free_stepsize( + mesh, V, V1, min_distance) + == Catch::Approx(expected_toi)); + } + + SECTION("Constraints") + { + const bool use_convergent_formulation = GENERATE(false, true); + const bool are_shape_derivatives_enabled = GENERATE(false, true); + + CollisionConstraints constraints; + constraints.set_use_convergent_formulation(use_convergent_formulation); + constraints.set_are_shape_derivatives_enabled( + are_shape_derivatives_enabled); + + const double dhat = 0.25; + constraints.build(mesh, V, dhat, /*min_distance=*/0.8, method); + + const int expected_num_constraints = + 6 + int(use_convergent_formulation); + const int expected_num_vv_constraints = + 2 + int(use_convergent_formulation); + + CHECK(constraints.size() == expected_num_constraints); + CHECK(constraints.vv_constraints.size() == expected_num_vv_constraints); + CHECK(constraints.ev_constraints.size() == 4); + CHECK(constraints.ee_constraints.size() == 0); + CHECK(constraints.fv_constraints.size() == 0); + + CHECK(constraints.compute_potential(mesh, V, dhat) > 0.0); + } +} + TEST_CASE("Vertex-Vertex Constraint", "[constraint][vertex-vertex]") { CHECK(VertexVertexConstraint(0, 1) == VertexVertexConstraint(0, 1));