diff --git a/.github/workflows/continuous.yml b/.github/workflows/continuous.yml index 822cbfe70..a98802c70 100644 --- a/.github/workflows/continuous.yml +++ b/.github/workflows/continuous.yml @@ -7,7 +7,7 @@ on: paths: - '.github/workflows/continuous.yml' - 'cmake/**' - - 'src/**' + - 'src/**' - 'tests/**' - 'CMakeLists.txt' @@ -70,7 +70,8 @@ jobs: - name: Prepare ccache run: | ccache --max-size=1.0G - ccache -V && ccache --show-stats && ccache --zero-stats + ccache -V && ccache --show-config + ccache --show-stats && ccache --zero-stats - name: Configure (Linux/macOS) if: runner.os != 'Windows' diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index a9ec0a037..74896f211 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -40,6 +40,7 @@ PYBIND11_MODULE(ipctk, m) define_point_static_plane(m); // collisions + define_distance_type(m); // define early because it is used next define_collision_constraint(m); define_collision_constraints(m); define_edge_edge_constraint(m); @@ -49,7 +50,6 @@ PYBIND11_MODULE(ipctk, m) define_vertex_vertex_constraint(m); // distance - define_distance_type(m); define_edge_edge_mollifier(m); define_edge_edge_distance(m); define_line_line_distance(m); diff --git a/python/src/collisions/edge_edge.cpp b/python/src/collisions/edge_edge.cpp index 8bc9a506e..2c8830dea 100644 --- a/python/src/collisions/edge_edge.cpp +++ b/python/src/collisions/edge_edge.cpp @@ -10,11 +10,14 @@ void define_edge_edge_constraint(py::module_& m) py::class_( m, "EdgeEdgeConstraint") .def( - py::init(), "", py::arg("edge0_id"), - py::arg("edge1_id"), py::arg("eps_x")) + py::init(), "", + py::arg("edge0_id"), py::arg("edge1_id"), py::arg("eps_x"), + py::arg("dtype") = ipc::EdgeEdgeDistanceType::AUTO) .def( - py::init(), "", - py::arg("candidate"), py::arg("eps_x")) + py::init< + const EdgeEdgeCandidate&, double, ipc::EdgeEdgeDistanceType>(), + "", py::arg("candidate"), py::arg("eps_x"), + py::arg("dtype") = ipc::EdgeEdgeDistanceType::AUTO) .def( "compute_potential", &EdgeEdgeConstraint::compute_potential, "", py::arg("vertices"), py::arg("edges"), py::arg("faces"), diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 2960b9b47..2b77d591c 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -105,8 +105,8 @@ CollisionMesh::CollisionMesh( m_faces_to_edges = construct_faces_to_edges(m_faces, m_edges); init_areas(); + init_adjacencies(); // Compute these manually if needed. - // init_adjacencies(); // init_area_jacobian(); } @@ -152,10 +152,13 @@ Eigen::SparseMatrix CollisionMesh::vertex_matrix_to_dof_matrix( void CollisionMesh::init_adjacencies() { m_vertex_vertex_adjacencies.resize(num_vertices()); + m_vertex_edge_adjacencies.resize(num_vertices()); // Edges includes the edges of the faces for (int i = 0; i < m_edges.rows(); i++) { m_vertex_vertex_adjacencies[m_edges(i, 0)].insert(m_edges(i, 1)); m_vertex_vertex_adjacencies[m_edges(i, 1)].insert(m_edges(i, 0)); + m_vertex_edge_adjacencies[m_edges(i, 0)].insert(i); + m_vertex_edge_adjacencies[m_edges(i, 1)].insert(i); } m_edge_vertex_adjacencies.resize(m_edges.rows()); @@ -250,59 +253,78 @@ void CollisionMesh::init_areas() (vertex_edge_areas.array() < 0).select(1, vertex_edge_areas), vertex_face_areas); - // Select the area based on the order face, codim - m_edge_areas = (m_edge_areas.array() < 0).select(1, m_edge_areas); + for (int i = 0; i < m_edge_areas.size(); i++) { + if (m_edge_areas[i] < 0) { + // Use the edge length for codim edges + const VectorMax3d e0 = m_rest_positions.row(m_edges(i, 0)); + const VectorMax3d e1 = m_rest_positions.row(m_edges(i, 1)); + m_edge_areas[i] = (e1 - e0).norm(); + } + } } void CollisionMesh::init_area_jacobians() { - // Compute vertex areas as the sum of ½ the length of connected edges + std::vector was_vertex_visited(num_vertices(), false); + std::vector was_edge_visited(num_edges(), false); + m_vertex_area_jacobian.resize( num_vertices(), Eigen::SparseVector(ndof())); - for (int i = 0; i < m_edges.rows(); i++) { - const VectorMax3d e0 = m_rest_positions.row(m_edges(i, 0)); - const VectorMax3d e1 = m_rest_positions.row(m_edges(i, 1)); + m_edge_area_jacobian.resize( + num_edges(), Eigen::SparseVector(ndof())); + + // Compute vertex/edge areas as the sum of ⅓ the area of connected face + for (int i = 0; i < m_faces.rows(); i++) { + assert(dim() == 3); + const Eigen::Vector3d f0 = m_rest_positions.row(m_faces(i, 0)); + const Eigen::Vector3d f1 = m_rest_positions.row(m_faces(i, 1)); + const Eigen::Vector3d f2 = m_rest_positions.row(m_faces(i, 2)); - const VectorMax6d edge_len_gradient = edge_length_gradient(e0, e1) / 2; + const Vector9d face_area_gradient = + triangle_area_gradient(f0, f1, f2) / 3.0; - for (int j = 0; j < m_edges.cols(); j++) { + for (int j = 0; j < m_faces.cols(); ++j) { + // compute gradient of area + + was_vertex_visited[m_faces(i, j)] = true; local_gradient_to_global_gradient( - edge_len_gradient, m_edges.row(i), dim(), - m_vertex_area_jacobian[m_edges(i, j)]); + face_area_gradient, m_faces.row(i), dim(), + m_vertex_area_jacobian[m_faces(i, j)]); + + was_edge_visited[m_faces_to_edges(i, j)] = true; + local_gradient_to_global_gradient( + face_area_gradient, m_faces.row(i), dim(), + m_edge_area_jacobian[m_faces_to_edges(i, j)]); } } - // Compute vertex/edge areas as the sum of ⅓ the area of connected face - m_edge_area_jacobian.resize( - m_edges.rows(), Eigen::SparseVector(ndof())); - if (dim() == 3) { - std::vector visited_vertex_before(num_vertices(), false); - for (int i = 0; i < m_faces.rows(); i++) { - const Eigen::Vector3d f0 = m_rest_positions.row(m_faces(i, 0)); - const Eigen::Vector3d f1 = m_rest_positions.row(m_faces(i, 1)); - const Eigen::Vector3d f2 = m_rest_positions.row(m_faces(i, 2)); - - const Vector9d face_area_gradient = - triangle_area_gradient(f0, f1, f2) / 3.0; - - for (int j = 0; j < m_faces.cols(); ++j) { - if (!visited_vertex_before[m_faces(i, j)]) { - // remove the computed value from vertex_edge_areas - m_vertex_area_jacobian[m_faces(i, j)].setZero(); - visited_vertex_before[m_faces(i, j)] = true; - } + // Compute unvisited vertex areas as the sum of ½ the length of connected + // edges + for (int i = 0; i < m_edges.rows(); i++) { + const int e0i = m_edges(i, 0), e1i = m_edges(i, 1); + const VectorMax3d e0 = m_rest_positions.row(e0i); + const VectorMax3d e1 = m_rest_positions.row(e1i); - // compute gradient of area + assert(was_vertex_visited[e0i] == was_vertex_visited[e1i]); + if (was_vertex_visited[e0i] && was_edge_visited[i]) { + continue; + } - local_gradient_to_global_gradient( - face_area_gradient, m_faces.row(i), dim(), - m_vertex_area_jacobian[m_faces(i, j)]); + const VectorMax6d edge_len_gradient = edge_length_gradient(e0, e1); + if (!was_vertex_visited[e0i]) { + for (int j = 0; j < m_edges.cols(); j++) { local_gradient_to_global_gradient( - face_area_gradient, m_faces.row(i), dim(), - m_edge_area_jacobian[m_faces_to_edges(i, j)]); + edge_len_gradient / 2, m_edges.row(i), dim(), + m_vertex_area_jacobian[m_edges(i, j)]); } } + + if (!was_edge_visited[i]) { + local_gradient_to_global_gradient( + edge_len_gradient, m_edges.row(i), dim(), + m_edge_area_jacobian[i]); + } } } diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 738bf4fa6..809852b84 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -164,12 +164,22 @@ class CollisionMesh { return m_vertex_vertex_adjacencies; } + /// @brief Get the vertex-edge adjacency matrix. + const std::vector>& vertex_edge_adjacencies() const + { + if (!are_adjacencies_initialized()) { + throw std::runtime_error( + "Vertex-edge adjacencies not initialized. Call init_adjacencies() first."); + } + return m_vertex_edge_adjacencies; + } + /// @brief Get the edge-vertex adjacency matrix. const std::vector>& edge_vertex_adjacencies() const { if (!are_adjacencies_initialized()) { throw std::runtime_error( - "Edge-vertex adjacencies not initialized. Call init_area_jacobians() first."); + "Edge-vertex adjacencies not initialized. Call init_adjacencies() first."); } return m_edge_vertex_adjacencies; } @@ -178,6 +188,7 @@ class CollisionMesh { bool are_adjacencies_initialized() const { return !m_vertex_vertex_adjacencies.empty() + && !m_vertex_edge_adjacencies.empty() && !m_edge_vertex_adjacencies.empty(); } @@ -305,7 +316,9 @@ class CollisionMesh { /// @brief Vertices adjacent to vertices std::vector> m_vertex_vertex_adjacencies; - /// @brief Edges adjacent to edges + /// @brief Edges adjacent to vertices + std::vector> m_vertex_edge_adjacencies; + /// @brief Vertices adjacent to edges std::vector> m_edge_vertex_adjacencies; // std::vector> m_vertices_to_faces; diff --git a/src/ipc/collisions/collision_constraint.cpp b/src/ipc/collisions/collision_constraint.cpp index 6f520f186..18aacf9ab 100644 --- a/src/ipc/collisions/collision_constraint.cpp +++ b/src/ipc/collisions/collision_constraint.cpp @@ -4,6 +4,13 @@ namespace ipc { +CollisionConstraint::CollisionConstraint( + const double weight, const Eigen::SparseVector& weight_gradient) + : weight(weight) + , weight_gradient(weight_gradient) +{ +} + double CollisionConstraint::compute_potential( const Eigen::MatrixXd& vertices, const Eigen::MatrixXi& edges, diff --git a/src/ipc/collisions/collision_constraint.hpp b/src/ipc/collisions/collision_constraint.hpp index 8e1f68d58..36c2812c7 100644 --- a/src/ipc/collisions/collision_constraint.hpp +++ b/src/ipc/collisions/collision_constraint.hpp @@ -11,6 +11,12 @@ namespace ipc { class CollisionConstraint : virtual public CollisionStencil { public: + CollisionConstraint() = default; + + CollisionConstraint( + const double weight, + const Eigen::SparseVector& weight_gradient); + virtual ~CollisionConstraint() { } virtual double compute_potential( diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index bb52b86c6..fc5dd7fdf 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -1,10 +1,15 @@ #include "collision_constraints.hpp" #include -// #include +#include +#include +#include +#include +#include #include #include +#include #include #include @@ -12,6 +17,126 @@ namespace ipc { +namespace { + /// @brief Convert element-vertex candidates to vertex-vertex candidates + /// @param elements Elements matrix of the mesh + /// @param vertices Vertex positions of the mesh + /// @param ev_candidates Element-vertex candidates + /// @param is_active Function to determine if a candidate is active + /// @return Vertex-vertex candidates + template + std::vector + element_vertex_to_vertex_vertex_candidates( + const Eigen::MatrixXi& elements, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active) + { + std::vector vv_candidates; + for (const auto& [ei, vi] : candidates) { + for (int j = 0; j < elements.cols(); j++) { + const int vj = elements(ei, j); + if (is_active(point_point_distance( + vertices.row(vi), vertices.row(vj)))) { + vv_candidates.emplace_back(vi, vj); + } + } + } + + // Remove duplicates + tbb::parallel_sort(vv_candidates.begin(), vv_candidates.end()); + vv_candidates.erase( + std::unique(vv_candidates.begin(), vv_candidates.end()), + vv_candidates.end()); + + return vv_candidates; + } + + std::vector edge_vertex_to_vertex_vertex_candidates( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& ev_candidates, + const std::function& is_active) + { + return element_vertex_to_vertex_vertex_candidates( + mesh.edges(), vertices, ev_candidates, is_active); + } + + std::vector face_vertex_to_vertex_vertex_candidates( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& fv_candidates, + const std::function& is_active) + { + return element_vertex_to_vertex_vertex_candidates( + mesh.faces(), vertices, fv_candidates, is_active); + } + + std::vector face_vertex_to_edge_vertex_candidates( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& fv_candidates, + const std::function& is_active) + { + std::vector ev_candidates; + for (const auto& [fi, vi] : fv_candidates) { + for (int j = 0; j < 3; j++) { + const int ei = mesh.faces_to_edges()(fi, j); + const int vj = mesh.edges()(ei, 0); + const int vk = mesh.edges()(ei, 1); + if (is_active(point_edge_distance( + vertices.row(vi), // + vertices.row(vj), vertices.row(vk)))) { + ev_candidates.emplace_back(ei, vi); + } + } + } + + // Remove duplicates + tbb::parallel_sort(ev_candidates.begin(), ev_candidates.end()); + ev_candidates.erase( + std::unique(ev_candidates.begin(), ev_candidates.end()), + ev_candidates.end()); + + return ev_candidates; + } + + std::vector edge_edge_to_edge_vertex_candidates( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& ee_candidates, + const std::function& is_active) + { + std::vector ev_candidates; + for (const EdgeEdgeCandidate& ee : ee_candidates) { + for (int i = 0; i < 2; i++) { + const int ei = i == 0 ? ee.edge0_id : ee.edge1_id; + const int ej = i == 0 ? ee.edge1_id : ee.edge0_id; + + const int ei0 = mesh.edges()(ei, 0); + const int ei1 = mesh.edges()(ei, 1); + + for (int j = 0; j < 2; j++) { + const int vj = mesh.edges()(ej, j); + if (is_active(point_edge_distance( + vertices.row(vj), // + vertices.row(ei0), vertices.row(ei1)))) { + ev_candidates.emplace_back(ei, vj); + } + } + } + } + + // Remove duplicates + tbb::parallel_sort(ev_candidates.begin(), ev_candidates.end()); + ev_candidates.erase( + std::unique(ev_candidates.begin(), ev_candidates.end()), + ev_candidates.end()); + + return ev_candidates; + } +} // namespace + void CollisionConstraints::build( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, @@ -48,7 +173,7 @@ void CollisionConstraints::build( }; tbb::enumerable_thread_specific storage( - CollisionConstraintsBuilder(*this)); + use_convergent_formulation(), are_shape_derivatives_enabled()); tbb::parallel_for( tbb::blocked_range(size_t(0), candidates.ev_candidates.size()), @@ -74,8 +199,71 @@ void CollisionConstraints::build( r.end()); }); + if (use_convergent_formulation()) { + if (candidates.ev_candidates.size() > 0) { + // Convert edge-vertex to vertex-vertex + const std::vector vv_candidates = + edge_vertex_to_vertex_vertex_candidates( + mesh, vertices, candidates.ev_candidates, 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_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); + }); + } + + if (candidates.ee_candidates.size() > 0) { + // Convert edge-edge to edge-vertex + const auto ev_candidates = edge_edge_to_edge_vertex_candidates( + mesh, vertices, candidates.ee_candidates, 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_constraints( + mesh, vertices, ev_candidates, r.begin(), r.end()); + }); + } + + if (candidates.fv_candidates.size() > 0) { + // Convert face-vertex to edge-vertex + const std::vector ev_candidates = + face_vertex_to_edge_vertex_candidates( + mesh, vertices, candidates.fv_candidates, 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_constraints( + mesh, vertices, ev_candidates, r.begin(), r.end()); + }); + + // Convert face-vertex to vertex-vertex + const std::vector vv_candidates = + face_vertex_to_vertex_vertex_candidates( + mesh, vertices, candidates.fv_candidates, 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_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); + }); + } + } + + // ------------------------------------------------------------------------- + CollisionConstraintsBuilder::merge(storage, *this); + // logger().debug(to_string(mesh, vertices)); + for (size_t ci = 0; ci < size(); ci++) { CollisionConstraint& constraint = (*this)[ci]; constraint.minimum_distance = dmin; @@ -415,4 +603,57 @@ const CollisionConstraint& CollisionConstraints::operator[](size_t idx) const throw std::out_of_range("Constraint index is out of range!"); } +std::string CollisionConstraints::to_string( + const CollisionMesh& mesh, const Eigen::MatrixXd& vertices) const +{ + std::stringstream ss; + for (const auto& vv : vv_constraints) { + ss << "\n" + << fmt::format( + "vv: {} {}, w: {:g}, d: {:g}", vv.vertex0_id, vv.vertex1_id, + vv.weight, + point_point_distance( + vertices.row(vv.vertex0_id), + vertices.row(vv.vertex1_id))); + } + for (const auto& ev : ev_constraints) { + ss << "\n" + << fmt::format( + "ev: {}=({}, {}) {}, w: {:g}, d: {:g}", ev.edge_id, + mesh.edges()(ev.edge_id, 0), mesh.edges()(ev.edge_id, 1), + ev.vertex_id, ev.weight, + point_line_distance( + vertices.row(ev.vertex_id), + vertices.row(mesh.edges()(ev.edge_id, 0)), + vertices.row(mesh.edges()(ev.edge_id, 1)))); + } + for (const auto& ee : ee_constraints) { + ss << "\n" + << fmt::format( + "ee: {}=({}, {}) {}=({}, {}), w: {:g}, dtype: {}, d: {:g}", + ee.edge0_id, mesh.edges()(ee.edge0_id, 0), + mesh.edges()(ee.edge0_id, 1), ee.edge1_id, + mesh.edges()(ee.edge1_id, 0), mesh.edges()(ee.edge1_id, 1), + ee.weight, int(ee.dtype), + edge_edge_distance( + vertices.row(mesh.edges()(ee.edge0_id, 0)), + vertices.row(mesh.edges()(ee.edge0_id, 1)), + vertices.row(mesh.edges()(ee.edge1_id, 0)), + vertices.row(mesh.edges()(ee.edge1_id, 1)), ee.dtype)); + } + for (const auto& fv : fv_constraints) { + ss << "\n" + << fmt::format( + "fv: {}=({}, {}, {}) {}, w: {:g}, d: {:g}", fv.face_id, + mesh.faces()(fv.face_id, 0), mesh.faces()(fv.face_id, 1), + mesh.faces()(fv.face_id, 2), fv.vertex_id, fv.weight, + point_plane_distance( + vertices.row(fv.vertex_id), + vertices.row(mesh.faces()(fv.face_id, 0)), + vertices.row(mesh.faces()(fv.face_id, 1)), + vertices.row(mesh.faces()(fv.face_id, 2)))); + } + return ss.str(); +} + } // namespace ipc diff --git a/src/ipc/collisions/collision_constraints.hpp b/src/ipc/collisions/collision_constraints.hpp index 94e8670ff..a352c1852 100644 --- a/src/ipc/collisions/collision_constraints.hpp +++ b/src/ipc/collisions/collision_constraints.hpp @@ -148,6 +148,9 @@ class CollisionConstraints { void set_are_shape_derivatives_enabled(const bool are_shape_derivatives_enabled); + std::string + to_string(const CollisionMesh& mesh, const Eigen::MatrixXd& vertices) const; + public: std::vector vv_constraints; std::vector ev_constraints; diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 81b8c3105..f12e52374 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -9,10 +9,11 @@ namespace ipc { CollisionConstraintsBuilder::CollisionConstraintsBuilder( - const CollisionConstraints& empty_constraints) + const bool use_convergent_formulation, + const bool should_compute_weight_gradient) + : use_convergent_formulation(use_convergent_formulation) + , should_compute_weight_gradient(should_compute_weight_gradient) { - assert(empty_constraints.empty()); - constraints = empty_constraints; } // ============================================================================ @@ -31,46 +32,55 @@ void CollisionConstraintsBuilder::add_edge_vertex_constraints( const auto [v, e0, e1, _] = candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); - PointEdgeDistanceType dtype = point_edge_distance_type(v, e0, e1); - double distance_sqr = point_edge_distance(v, e0, e1, dtype); + 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)) continue; // ÷ 2 to handle double counting for correct integration const double weight = - use_convergent_formulation() ? (mesh.vertex_area(vi) / 2) : 1; + use_convergent_formulation ? (mesh.vertex_area(vi) / 2) : 1; Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { - weight_gradient = use_convergent_formulation() + if (should_compute_weight_gradient) { + weight_gradient = use_convergent_formulation ? (mesh.vertex_area_gradient(vi) / 2) : Eigen::SparseVector(vertices.size()); } - switch (dtype) { - case PointEdgeDistanceType::P_E0: - add_vertex_vertex_constraint(vi, e0i, weight, weight_gradient); - break; - - case PointEdgeDistanceType::P_E1: - add_vertex_vertex_constraint(vi, e1i, weight, weight_gradient); - break; - - case PointEdgeDistanceType::P_E: - // ev_candidates is a set, so no duplicate EV CollisionConstraints - constraints.ev_constraints.emplace_back(ei, vi); - constraints.ev_constraints.back().weight = weight; - constraints.ev_constraints.back().weight_gradient = weight_gradient; - ev_to_id.emplace( - constraints.ev_constraints.back(), - constraints.ev_constraints.size() - 1); - break; + add_edge_vertex_constraint( + mesh, candidates[i], dtype, weight, weight_gradient); + } +} - case PointEdgeDistanceType::AUTO: - assert(false); - break; - } +void CollisionConstraintsBuilder::add_edge_vertex_constraint( + const CollisionMesh& mesh, + const EdgeVertexCandidate& candidate, + const PointEdgeDistanceType dtype, + const double weight, + const Eigen::SparseVector& weight_gradient) +{ + const auto& [ei, vi] = candidate; + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + add_vertex_vertex_constraint( + vi, mesh.edges()(ei, 0), weight, weight_gradient); + break; + + case PointEdgeDistanceType::P_E1: + add_vertex_vertex_constraint( + vi, mesh.edges()(ei, 1), weight, weight_gradient); + break; + + case PointEdgeDistanceType::P_E: + add_edge_vertex_constraint(ei, vi, weight, weight_gradient); + break; + + case PointEdgeDistanceType::AUTO: + assert(false); + break; } } @@ -91,11 +101,11 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( const auto [ea0, ea1, eb0, eb1] = candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); - EdgeEdgeDistanceType dtype = + const EdgeEdgeDistanceType actual_dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); const double distance_sqr = - edge_edge_distance(ea0, ea1, eb0, eb1, dtype); + edge_edge_distance(ea0, ea1, eb0, eb1, actual_dtype); if (!is_active(distance_sqr)) continue; @@ -107,21 +117,21 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( const double ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // NOTE: This may not actually be the distance type, but all EE - // pairs requiring mollification must be mollified later. - dtype = EdgeEdgeDistanceType::EA_EB; - } + // 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_convergent_formulation() + const double weight = use_convergent_formulation ? ((mesh.edge_area(eai) + mesh.edge_area(ebi)) / 4) : 1; Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { - weight_gradient = use_convergent_formulation() + if (should_compute_weight_gradient) { + weight_gradient = use_convergent_formulation ? ((mesh.edge_area_gradient(eai) + mesh.edge_area_gradient(ebi)) / 4) : Eigen::SparseVector(vertices.size()); @@ -161,9 +171,9 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( break; case EdgeEdgeDistanceType::EA_EB: - constraints.ee_constraints.emplace_back(eai, ebi, eps_x); - constraints.ee_constraints.back().weight = weight; - constraints.ee_constraints.back().weight_gradient = weight_gradient; + ee_constraints.emplace_back( + eai, ebi, eps_x, weight, weight_gradient, actual_dtype); + ee_to_id.emplace(ee_constraints.back(), ee_constraints.size() - 1); break; case EdgeEdgeDistanceType::AUTO: @@ -200,11 +210,11 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( // ÷ 4 to handle double counting and PT + EE for correct integration const double weight = - use_convergent_formulation() ? (mesh.vertex_area(vi) / 4) : 1; + use_convergent_formulation ? (mesh.vertex_area(vi) / 4) : 1; Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { - weight_gradient = use_convergent_formulation() + if (should_compute_weight_gradient) { + weight_gradient = use_convergent_formulation ? (mesh.vertex_area_gradient(vi) / 4) : Eigen::SparseVector(vertices.size()); } @@ -238,9 +248,7 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( break; case PointTriangleDistanceType::P_T: - constraints.fv_constraints.emplace_back(fi, vi); - constraints.fv_constraints.back().weight = weight; - constraints.fv_constraints.back().weight_gradient = weight_gradient; + fv_constraints.emplace_back(fi, vi, weight, weight_gradient); break; case PointTriangleDistanceType::AUTO: @@ -252,49 +260,278 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( // ============================================================================ +void CollisionConstraintsBuilder:: + add_edge_vertex_negative_vertex_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i) +{ + const auto add_weight = [&](const size_t vi, const size_t vj, + double& weight, + Eigen::SparseVector& weight_gradient) { + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vj]; + const int incident_edge_amt = incident_vertices.size() + - int(incident_vertices.find(vi) != incident_vertices.end()); + + if (incident_edge_amt > 1) { + // ÷ 2 to handle double counting for correct integration + weight += (1 - incident_edge_amt) + * (use_convergent_formulation ? (mesh.vertex_area(vi) / 2) : 1); + + if (should_compute_weight_gradient && use_convergent_formulation) { + weight_gradient += (1 - incident_edge_amt) / 2.0 + * mesh.vertex_area_gradient(vi); + } + } + }; + + for (size_t i = start_i; i < end_i; i++) { + const auto& [vi, vj] = candidates[i]; + assert(vi != vj); + + double weight = 0; + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient) { + weight_gradient = Eigen::SparseVector(vertices.size()); + } + + add_weight(vi, vj, weight, weight_gradient); + add_weight(vj, vi, weight, weight_gradient); + + if (weight != 0) { + add_vertex_vertex_constraint(vi, vj, weight, weight_gradient); + } + } +} + +void CollisionConstraintsBuilder:: + add_face_vertex_positive_vertex_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i) +{ + const auto add_weight = [&](const size_t vi, const size_t vj, + double& weight, + Eigen::SparseVector& weight_gradient) { + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vj]; + if (mesh.is_vertex_on_boundary(vj) + || incident_vertices.find(vi) != incident_vertices.end()) { + return; // Skip boundary vertices and incident vertices + } + + // ÷ 4 to handle double counting and PT + EE for correct integration. + weight += use_convergent_formulation ? (mesh.vertex_area(vi) / 4) : 1; + + if (should_compute_weight_gradient && use_convergent_formulation) { + weight_gradient += mesh.vertex_area_gradient(vi) / 4; + } + }; + + for (size_t i = start_i; i < end_i; i++) { + const auto& [vi, vj] = candidates[i]; + assert(vi != vj); + + double weight = 0; + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient) { + weight_gradient = Eigen::SparseVector(vertices.size()); + } + + add_weight(vi, vj, weight, weight_gradient); + add_weight(vj, vi, weight, weight_gradient); + + if (weight != 0) { + add_vertex_vertex_constraint(vi, vj, weight, weight_gradient); + } + } +} + +void CollisionConstraintsBuilder:: + add_face_vertex_negative_edge_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i) +{ + 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& incident_vertices = mesh.edge_vertex_adjacencies()[ei]; + const int incident_triangle_amt = incident_vertices.size() + - int(incident_vertices.find(vi) != incident_vertices.end()); + + if (incident_triangle_amt > 1) { + // ÷ 4 to handle double counting and PT + EE for correct integration + const double weight = (1 - incident_triangle_amt) + * (use_convergent_formulation ? (mesh.vertex_area(vi) / 4) : 1); + + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient) { + weight_gradient = use_convergent_formulation + ? ((1 - incident_triangle_amt) / 4.0 + * mesh.vertex_area_gradient(vi)) + : Eigen::SparseVector(vertices.size()); + } + + add_edge_vertex_constraint( + 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); + } + } +} + +void CollisionConstraintsBuilder:: + add_edge_edge_negative_edge_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i) +{ + // 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 int 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_convergent_formulation ? (-0.25 * mesh.edge_area(ea)) : -1; + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient) { + weight_gradient = use_convergent_formulation + ? (-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)); + + int nonmollified_incident_edge_amt = 0; + + const auto& incident_edges = mesh.vertex_edge_adjacencies()[p]; + for (const int eb : incident_edges) { + const int eb0 = mesh.edges()(eb, 0), eb1 = mesh.edges()(eb, 1); + const int q = mesh.edges()(eb, int(p == eb0)); + assert(p != q); + if (q == ea0 || q == ea1) { + 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)); + + const double ee_cross_norm_sqr = edge_edge_cross_squarednorm( + vertices.row(ea0), vertices.row(ea1), vertices.row(eb0), + vertices.row(eb1)); + + if (ee_cross_norm_sqr >= eps_x) { + nonmollified_incident_edge_amt++; + continue; + } + + // Add mollified EE constraint 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; + } + + add_edge_edge_constraint( + ea, eb, eps_x, weight, weight_gradient, ee_dtype); + } + + if (nonmollified_incident_edge_amt == 1) { + continue; // no constraint to add because (ρ(x) - 1) = 0 + } + // if nonmollified_incident_edge_amt == 0, then we need to explicitly + // add a positive constraint. + add_edge_vertex_constraint( + mesh, candidates[i], dtype, + (nonmollified_incident_edge_amt - 1) * weight, + (nonmollified_incident_edge_amt - 1) * weight_gradient); + } +} + +// ============================================================================ + void CollisionConstraintsBuilder::add_vertex_vertex_constraint( - const long v0i, - const long v1i, - const double weight, - const Eigen::SparseVector& weight_gradient, + const VertexVertexConstraint& vv_constraint, unordered_map& vv_to_id, std::vector& vv_constraints) { - VertexVertexConstraint vv_constraint(v0i, v1i); auto found_item = vv_to_id.find(vv_constraint); if (found_item != vv_to_id.end()) { // Constraint already exists, so increase weight - vv_constraints[found_item->second].weight += weight; - vv_constraints[found_item->second].weight_gradient += weight_gradient; + vv_constraints[found_item->second].weight += vv_constraint.weight; + vv_constraints[found_item->second].weight_gradient += + vv_constraint.weight_gradient; } else { // New constraint, so add it to the end of vv_constraints vv_to_id.emplace(vv_constraint, vv_constraints.size()); vv_constraints.push_back(vv_constraint); - vv_constraints.back().weight = weight; - vv_constraints.back().weight_gradient = weight_gradient; } } void CollisionConstraintsBuilder::add_edge_vertex_constraint( - const long ei, - const long vi, - const double weight, - const Eigen::SparseVector& weight_gradient, + const EdgeVertexConstraint& ev_constraint, unordered_map& ev_to_id, std::vector& ev_constraints) { - EdgeVertexConstraint ev_constraint(ei, vi); auto found_item = ev_to_id.find(ev_constraint); if (found_item != ev_to_id.end()) { // Constraint already exists, so increase weight - ev_constraints[found_item->second].weight += weight; - ev_constraints[found_item->second].weight_gradient += weight_gradient; + ev_constraints[found_item->second].weight += ev_constraint.weight; + ev_constraints[found_item->second].weight_gradient += + ev_constraint.weight_gradient; } else { - // New constraint, so add it to the end of vv_constraints + // New constraint, so add it to the end of ev_constraints ev_to_id.emplace(ev_constraint, ev_constraints.size()); ev_constraints.push_back(ev_constraint); - ev_constraints.back().weight = weight; - ev_constraints.back().weight_gradient = weight_gradient; + } +} + +void CollisionConstraintsBuilder::add_edge_edge_constraint( + const EdgeEdgeConstraint& ee_constraint, + unordered_map& ee_to_id, + std::vector& ee_constraints) +{ + auto found_item = ee_to_id.find(ee_constraint); + if (found_item != ee_to_id.end()) { + // Constraint already exists, so increase weight + assert(ee_constraint == ee_constraints[found_item->second]); + ee_constraints[found_item->second].weight += ee_constraint.weight; + ee_constraints[found_item->second].weight_gradient += + ee_constraint.weight_gradient; + } else { + // New constraint, so add it to the end of ee_constraints + ee_to_id.emplace(ee_constraint, ee_constraints.size()); + ee_constraints.push_back(ee_constraint); } } @@ -307,6 +544,7 @@ void CollisionConstraintsBuilder::merge( { unordered_map vv_to_id; unordered_map ev_to_id; + unordered_map ee_to_id; auto& vv_constraints = merged_constraints.vv_constraints; auto& ev_constraints = merged_constraints.ev_constraints; auto& ee_constraints = merged_constraints.ee_constraints; @@ -316,10 +554,10 @@ void CollisionConstraintsBuilder::merge( size_t n_vv = 0, n_ev = 0, n_ee = 0, n_fv = 0; for (const auto& storage : local_storage) { // This is an conservative estimate - n_vv += storage.constraints.vv_constraints.size(); - n_ev += storage.constraints.ev_constraints.size(); - n_ee += storage.constraints.ee_constraints.size(); - n_fv += storage.constraints.fv_constraints.size(); + n_vv += storage.vv_constraints.size(); + n_ev += storage.ev_constraints.size(); + n_ee += storage.ee_constraints.size(); + n_fv += storage.fv_constraints.size(); } vv_constraints.reserve(n_vv); ev_constraints.reserve(n_ev); @@ -328,41 +566,58 @@ void CollisionConstraintsBuilder::merge( // merge for (const auto& builder : local_storage) { - const auto& local_constraints = builder.constraints; - if (vv_constraints.empty()) { vv_to_id = builder.vv_to_id; - vv_constraints.insert( - vv_constraints.end(), local_constraints.vv_constraints.begin(), - local_constraints.vv_constraints.end()); + vv_constraints = builder.vv_constraints; } else { - for (const auto& vv : local_constraints.vv_constraints) { - add_vertex_vertex_constraint( - vv.vertex0_id, vv.vertex1_id, vv.weight, vv.weight_gradient, - vv_to_id, vv_constraints); + for (const auto& vv : builder.vv_constraints) { + add_vertex_vertex_constraint(vv, vv_to_id, vv_constraints); } } if (ev_constraints.empty()) { ev_to_id = builder.ev_to_id; - ev_constraints.insert( - ev_constraints.end(), local_constraints.ev_constraints.begin(), - local_constraints.ev_constraints.end()); + ev_constraints = builder.ev_constraints; + } else { + for (const auto& ev : builder.ev_constraints) { + add_edge_vertex_constraint(ev, ev_to_id, ev_constraints); + } + } + + if (ee_constraints.empty()) { + ee_to_id = builder.ee_to_id; + ee_constraints = builder.ee_constraints; } else { - for (const auto& ev : local_constraints.ev_constraints) { - add_edge_vertex_constraint( - ev.edge_id, ev.vertex_id, ev.weight, ev.weight_gradient, - ev_to_id, ev_constraints); + for (const auto& ee : builder.ee_constraints) { + add_edge_edge_constraint(ee, ee_to_id, ee_constraints); } } - ee_constraints.insert( - ee_constraints.end(), local_constraints.ee_constraints.begin(), - local_constraints.ee_constraints.end()); fv_constraints.insert( - fv_constraints.end(), local_constraints.fv_constraints.begin(), - local_constraints.fv_constraints.end()); + fv_constraints.end(), builder.fv_constraints.begin(), + builder.fv_constraints.end()); } + + // If positive and negative vertex-vertex constraints cancel out, remove + // them. This can happen when edge-vertex constraints reduce to + // vertex-vertex constraints. This will avoid unnecessary computation. + vv_constraints.erase( + std::remove_if( + vv_constraints.begin(), vv_constraints.end(), + [&](const VertexVertexConstraint& vv) { return vv.weight == 0; }), + vv_constraints.end()); + // Same for edge-vertex constraints. + ev_constraints.erase( + std::remove_if( + ev_constraints.begin(), ev_constraints.end(), + [&](const EdgeVertexConstraint& ev) { return ev.weight == 0; }), + ev_constraints.end()); + // Same for edge-edge constraints. + ee_constraints.erase( + std::remove_if( + ee_constraints.begin(), ee_constraints.end(), + [&](const EdgeEdgeConstraint& ee) { return ee.weight == 0; }), + ee_constraints.end()); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/collision_constraints_builder.hpp b/src/ipc/collisions/collision_constraints_builder.hpp index 7ac33a3e2..3b6a71b31 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -1,8 +1,6 @@ #pragma once #include -#include -#include #include #include @@ -13,7 +11,9 @@ namespace ipc { class CollisionConstraintsBuilder { public: - CollisionConstraintsBuilder(const CollisionConstraints& empty_constraints); + CollisionConstraintsBuilder( + const bool use_convergent_formulation, + const bool are_shape_derivatives_enabled); void add_edge_vertex_constraints( const CollisionMesh& mesh, @@ -39,64 +39,125 @@ class CollisionConstraintsBuilder { const size_t start_i, const size_t end_i); + // ------------------------------------------------------------------------ + // Duplicate removal functions + + void add_edge_vertex_negative_vertex_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i); + + void add_face_vertex_positive_vertex_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i); + + void add_face_vertex_negative_edge_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i); + + void add_edge_edge_negative_edge_vertex_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const size_t start_i, + const size_t end_i); + + // ------------------------------------------------------------------------ + static void merge( const tbb::enumerable_thread_specific& local_storage, CollisionConstraints& merged_constraints); + // ------------------------------------------------------------------------- protected: static void add_vertex_vertex_constraint( - const long v0i, - const long v1i, - const double weight, - const Eigen::SparseVector& weight_gradient, + const VertexVertexConstraint& vv_constraint, unordered_map& vv_to_id, std::vector& vv_constraints); - static void add_edge_vertex_constraint( - const long ei, - const long vi, - const double weight, - const Eigen::SparseVector& weight_gradient, - unordered_map& ev_to_id, - std::vector& ev_constraints); - void add_vertex_vertex_constraint( - const long v0i, - const long v1i, + const long vertex0_id, + const long vertex1_id, const double weight, const Eigen::SparseVector& weight_gradient) { add_vertex_vertex_constraint( - v0i, v1i, weight, weight_gradient, vv_to_id, - constraints.vv_constraints); + VertexVertexConstraint( + vertex0_id, vertex1_id, weight, weight_gradient), + vv_to_id, vv_constraints); } + // ------------------------------------------------------------------------- + + static void add_edge_vertex_constraint( + const EdgeVertexConstraint& ev_constraint, + unordered_map& ev_to_id, + std::vector& ev_constraints); + void add_edge_vertex_constraint( - const long ei, - const long vi, + const long edge_id, + const long vertex_id, const double weight, const Eigen::SparseVector& weight_gradient) { add_edge_vertex_constraint( - ei, vi, weight, weight_gradient, ev_to_id, - constraints.ev_constraints); + EdgeVertexConstraint(edge_id, vertex_id, weight, weight_gradient), + ev_to_id, ev_constraints); } - bool use_convergent_formulation() const - { - return constraints.use_convergent_formulation(); - } + void add_edge_vertex_constraint( + const CollisionMesh& mesh, + const EdgeVertexCandidate& candidate, + const PointEdgeDistanceType dtype, + const double weight, + const Eigen::SparseVector& weight_gradient); + + // ------------------------------------------------------------------------- - bool should_compute_weight_gradient() const + static void add_edge_edge_constraint( + const EdgeEdgeConstraint& ee_constraint, + unordered_map& ee_to_id, + std::vector& ee_constraints); + + void add_edge_edge_constraint( + const long edge0_id, + const long edge1_id, + const double eps_x, + const double weight, + const Eigen::SparseVector& weight_gradient, + const EdgeEdgeDistanceType dtype) { - return constraints.are_shape_derivatives_enabled(); + add_edge_edge_constraint( + EdgeEdgeConstraint( + edge0_id, edge1_id, eps_x, weight, weight_gradient, dtype), + ee_to_id, ee_constraints); } - // Store the indices to VV and EV pairs to avoid duplicates. + // ------------------------------------------------------------------------- + + // Store the indices to pairs to avoid duplicates. unordered_map vv_to_id; unordered_map ev_to_id; - CollisionConstraints constraints; + unordered_map ee_to_id; + + // Constructed constraints + std::vector vv_constraints; + std::vector ev_constraints; + std::vector ee_constraints; + std::vector fv_constraints; + // std::vector pv_constraints; + + const bool use_convergent_formulation; + const bool should_compute_weight_gradient; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/edge_edge.cpp b/src/ipc/collisions/edge_edge.cpp index 18075826c..17ebab7d0 100644 --- a/src/ipc/collisions/edge_edge.cpp +++ b/src/ipc/collisions/edge_edge.cpp @@ -7,16 +7,37 @@ namespace ipc { EdgeEdgeConstraint::EdgeEdgeConstraint( - long edge0_id, long edge1_id, double eps_x) + const long edge0_id, + const long edge1_id, + const double eps_x, + const EdgeEdgeDistanceType dtype) : EdgeEdgeCandidate(edge0_id, edge1_id) , eps_x(eps_x) + , dtype(dtype) { } EdgeEdgeConstraint::EdgeEdgeConstraint( - const EdgeEdgeCandidate& candidate, double eps_x) + const EdgeEdgeCandidate& candidate, + const double eps_x, + const EdgeEdgeDistanceType dtype) : EdgeEdgeCandidate(candidate) , eps_x(eps_x) + , dtype(dtype) +{ +} + +EdgeEdgeConstraint::EdgeEdgeConstraint( + const long edge0_id, + const long edge1_id, + const double eps_x, + const double weight, + const Eigen::SparseVector& weight_gradient, + const EdgeEdgeDistanceType dtype) + : EdgeEdgeCandidate(edge0_id, edge1_id) + , CollisionConstraint(weight, weight_gradient) + , eps_x(eps_x) + , dtype(dtype) { } @@ -40,33 +61,24 @@ VectorMax12d EdgeEdgeConstraint::compute_potential_gradient( const Eigen::MatrixXi& faces, const double dhat) const { - const double adjusted_dhat = 2 * minimum_distance * dhat + dhat * dhat; - const double min_dist_squared = minimum_distance * minimum_distance; - - // ∇[m(x) * b(d(x))] = (∇m(x)) * b(d(x)) + m(x) * b'(d(x)) * ∇d(x) const auto& [ea0, ea1, eb0, eb1] = this->vertices(vertices, edges, faces); - // The distance type is unknown because of mollified PP and PE - // constraints where also added as EE constraints. - const EdgeEdgeDistanceType dtype = - edge_edge_distance_type(ea0, ea1, eb0, eb1); - const double distance = edge_edge_distance(ea0, ea1, eb0, eb1, dtype); - const Vector12d distance_grad = - edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); + // b(d(x)) + const double barrier = + CollisionConstraint::compute_potential(vertices, edges, faces, dhat); + // ∇ b(d(x)) + const VectorMax12d barrier_grad = + CollisionConstraint::compute_potential_gradient( + vertices, edges, faces, dhat); // m(x) const double mollifier = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); - // ∇m(x) + // ∇ m(x) const Vector12d mollifier_grad = edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); - // b(d(x)) - const double b = barrier(distance - min_dist_squared, adjusted_dhat); - // b'(d(x)) - const double grad_b = - barrier_gradient(distance - min_dist_squared, adjusted_dhat); - - return weight * (mollifier_grad * b + mollifier * grad_b * distance_grad); + // ∇[m(x) * b(d(x))] = ∇m(x)) * b(d(x)) + m(x) * ∇ b(d(x)) + return mollifier_grad * barrier + mollifier * barrier_grad; } MatrixMax12d EdgeEdgeConstraint::compute_potential_hessian( @@ -76,54 +88,56 @@ MatrixMax12d EdgeEdgeConstraint::compute_potential_hessian( const double dhat, const bool project_hessian_to_psd) const { - const double adjusted_dhat = 2 * minimum_distance * dhat + dhat * dhat; - const double min_dist_squared = minimum_distance * minimum_distance; - - // ∇²[m(x) * b(d(x))] = ∇[∇m(x) * b(d(x)) + m(x) * b'(d(x)) * ∇d(x)] - // = ∇²m(x) * b(d(x)) + b'(d(x)) * ∇d(x) * ∇m(x)ᵀ - // + ∇m(x) * b'(d(x)) * ∇d(x))ᵀ - // + m(x) * b"(d(x)) * ∇d(x) * ∇d(x)ᵀ - // + m(x) * b'(d(x)) * ∇²d(x) const auto& [ea0, ea1, eb0, eb1] = this->vertices(vertices, edges, faces); - // Compute distance derivatives - // The distance type is unknown because of mollified PP and PE - // constraints where also added as EE constraints. - const EdgeEdgeDistanceType dtype = - edge_edge_distance_type(ea0, ea1, eb0, eb1); - const double distance = edge_edge_distance(ea0, ea1, eb0, eb1, dtype); - const Vector12d distance_grad = - edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); - const Matrix12d distance_hess = - edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); - - // Compute mollifier derivatives + // b(d(x)) + const double barrier = + CollisionConstraint::compute_potential(vertices, edges, faces, dhat); + // ∇ b(d(x)) + const Vector12d barrier_grad = + CollisionConstraint::compute_potential_gradient( + vertices, edges, faces, dhat); + // ∇² b(d(x)) + const Matrix12d barrier_hess = + CollisionConstraint::compute_potential_hessian( + vertices, edges, faces, dhat, /*project_hessian_to_psd=*/false); + + // m(x) const double mollifier = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); - const VectorMax12d mollifier_grad = + // ∇ m(x) + const Vector12d mollifier_grad = edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); - const MatrixMax12d mollifier_hess = + // ∇² m(x) + const Matrix12d mollifier_hess = edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x); - // Compute barrier derivatives - const double b = barrier(distance - min_dist_squared, adjusted_dhat); - const double grad_b = - barrier_gradient(distance - min_dist_squared, adjusted_dhat); - const double hess_b = - barrier_hessian(distance - min_dist_squared, adjusted_dhat); - - MatrixMax12d hess = mollifier_hess * b - + grad_b - * (distance_grad * mollifier_grad.transpose() - + mollifier_grad * distance_grad.transpose()) - + mollifier - * (hess_b * distance_grad * distance_grad.transpose() - + grad_b * distance_hess); - - if (project_hessian_to_psd) { - hess = project_to_psd(hess); - } + // ∇²[m(x) * b(d(x))] = ∇[∇m(x) * b(d(x)) + m(x) * ∇b(d(x))] + // = ∇²m(x) * b(d(x)) + ∇b(d(x)) * ∇m(x)ᵀ + // + ∇m(x) * ∇b(d(x))ᵀ + m(x) * ∇²b(d(x)) + const Matrix12d grad_b_grad_m = barrier_grad * mollifier_grad.transpose(); - return weight * hess; + const Matrix12d hess = mollifier_hess * barrier + grad_b_grad_m + + grad_b_grad_m.transpose() + mollifier * barrier_hess; + + return project_hessian_to_psd ? project_to_psd(hess) : hess; +} + +bool EdgeEdgeConstraint::operator==(const EdgeEdgeConstraint& other) const +{ + return EdgeEdgeCandidate::operator==(other) && dtype == other.dtype; +} + +bool EdgeEdgeConstraint::operator!=(const EdgeEdgeConstraint& other) const +{ + return !(*this == other); +} + +bool EdgeEdgeConstraint::operator<(const EdgeEdgeConstraint& other) const +{ + if (EdgeEdgeCandidate::operator==(other)) { + return dtype < other.dtype; + } + return EdgeEdgeCandidate::operator<(other); } } // namespace ipc diff --git a/src/ipc/collisions/edge_edge.hpp b/src/ipc/collisions/edge_edge.hpp index f33cee74d..5e68d20f8 100644 --- a/src/ipc/collisions/edge_edge.hpp +++ b/src/ipc/collisions/edge_edge.hpp @@ -9,8 +9,24 @@ namespace ipc { class EdgeEdgeConstraint : public EdgeEdgeCandidate, public CollisionConstraint { public: - EdgeEdgeConstraint(long edge0_id, long edge1_id, double eps_x); - EdgeEdgeConstraint(const EdgeEdgeCandidate& candidate, double eps_x); + EdgeEdgeConstraint( + const long edge0_id, + const long edge1_id, + const double eps_x, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); + + EdgeEdgeConstraint( + const EdgeEdgeCandidate& candidate, + const double eps_x, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); + + EdgeEdgeConstraint( + const long edge0_id, + const long edge1_id, + const double eps_x, + const double weight, + const Eigen::SparseVector& weight_gradient, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); double compute_potential( const Eigen::MatrixXd& vertices, @@ -31,14 +47,31 @@ class EdgeEdgeConstraint : public EdgeEdgeCandidate, const double dhat, const bool project_hessian_to_psd) const override; + // ------------------------------------------------------------------------ + + bool operator==(const EdgeEdgeConstraint& other) const; + bool operator!=(const EdgeEdgeConstraint& other) const; + bool operator<(const EdgeEdgeConstraint& other) const; + template friend H AbslHashValue(H h, const EdgeEdgeConstraint& ee) { - return AbslHashValue( - std::move(h), static_cast(ee)); + return H::combine( + std::move(h), static_cast(ee), ee.dtype); } + // ------------------------------------------------------------------------ + + /// @brief Mollifier activation threshold. + /// @see edge_edge_mollifier double eps_x; + + /// @brief Cached distance type. + /// Some EE constraints are mollified EV or VV constraints. + EdgeEdgeDistanceType dtype; + +protected: + virtual EdgeEdgeDistanceType known_dtype() const override { return dtype; } }; } // namespace ipc diff --git a/src/ipc/collisions/edge_vertex.hpp b/src/ipc/collisions/edge_vertex.hpp index 66f68c5ef..326a1ee21 100644 --- a/src/ipc/collisions/edge_vertex.hpp +++ b/src/ipc/collisions/edge_vertex.hpp @@ -15,6 +15,16 @@ class EdgeVertexConstraint : public EdgeVertexCandidate, { } + EdgeVertexConstraint( + const long edge_id, + const long vertex_id, + const double weight, + const Eigen::SparseVector& weight_gradient) + : EdgeVertexCandidate(edge_id, vertex_id) + , CollisionConstraint(weight, weight_gradient) + { + } + template friend H AbslHashValue(H h, const EdgeVertexConstraint& ev) { diff --git a/src/ipc/collisions/face_vertex.hpp b/src/ipc/collisions/face_vertex.hpp index fa41058a9..e74d9fdf7 100644 --- a/src/ipc/collisions/face_vertex.hpp +++ b/src/ipc/collisions/face_vertex.hpp @@ -15,6 +15,16 @@ class FaceVertexConstraint : public FaceVertexCandidate, { } + FaceVertexConstraint( + const long face_id, + const long vertex_id, + const double weight, + const Eigen::SparseVector& weight_gradient) + : FaceVertexCandidate(face_id, vertex_id) + , CollisionConstraint(weight, weight_gradient) + { + } + template friend H AbslHashValue(H h, const FaceVertexConstraint& fv) { diff --git a/src/ipc/collisions/vertex_vertex.hpp b/src/ipc/collisions/vertex_vertex.hpp index a3bf615ba..3c8ccf189 100644 --- a/src/ipc/collisions/vertex_vertex.hpp +++ b/src/ipc/collisions/vertex_vertex.hpp @@ -16,6 +16,16 @@ class VertexVertexConstraint : public VertexVertexCandidate, { } + VertexVertexConstraint( + const long vertex0_id, + const long vertex1_id, + const double weight, + const Eigen::SparseVector& weight_gradient) + : VertexVertexCandidate(vertex0_id, vertex1_id) + , CollisionConstraint(weight, weight_gradient) + { + } + template friend H AbslHashValue(H h, const VertexVertexConstraint& vv) { diff --git a/src/ipc/utils/unordered_map_and_set.hpp b/src/ipc/utils/unordered_map_and_set.hpp index ea5ad46f6..3230f333a 100644 --- a/src/ipc/utils/unordered_map_and_set.hpp +++ b/src/ipc/utils/unordered_map_and_set.hpp @@ -17,10 +17,14 @@ template struct Hash { template static Hash&& combine(const Hash&& h, Value value) { - std::hash hash; - return std::move(Hash( - h.hash - ^ (hash(value) + 0x9e3779b9 + (h.hash << 6) + (h.hash >> 2)))); + if constexpr (std::is_default_constructible>::value) { + std::hash hash; + return std::move(Hash( + h.hash + ^ (hash(value) + 0x9e3779b9 + (h.hash << 6) + (h.hash >> 2)))); + } else { + return std::move(AbslHashValue(h, value)); + } } template diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index c822b04ed..1828c565a 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -1,14 +1,14 @@ #include -#include -#include - -#include +#include "test_utils.hpp" #include #include +#include +#include -#include "test_utils.hpp" +#include +#include using namespace ipc; @@ -310,4 +310,123 @@ TEST_CASE("Benchmark IPC shape derivative", "[ipc][shape_opt][!benchmark]") JF_wrt_X = collision_constraints.compute_shape_derivative(mesh, V, dhat); }; +} + +TEST_CASE("Test convergent formulation", "[ipc][convergent]") +{ + const bool use_convergent_formulation = GENERATE(false, true); + const double dhat = 1e-3; + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + SECTION("2D Edge-Vertex") + { + // . + // .-------.-------. + V.resize(4, 2); + V.row(0) << 0, 1e-4; + V.row(1) << -1, 0; + V.row(2) << 1e-4, 0; + V.row(3) << 1, 0; + + E.resize(2, 2); + E.row(0) << 1, 2; + E.row(1) << 2, 3; + + CHECK(point_point_distance(V.row(0), V.row(2)) < dhat * dhat); + } + SECTION("3D Face-Vertex") + { + V.resize(5, 3); + V.row(0) << 0, 1e-4, 0; + V.row(1) << -1, 0, 0; + V.row(2) << 1e-4, 0, -1; + V.row(3) << 1e-4, 0, 1; + V.row(4) << 1, 0, 0; + + F.resize(2, 3); + F.row(0) << 1, 2, 3; + F.row(1) << 2, 3, 4; + + igl::edges(F, E); + + CHECK(point_edge_distance(V.row(0), V.row(2), V.row(3)) < dhat * dhat); + } + SECTION("3D Edge-Edge") + { + V.resize(5, 3); + // + V.row(0) << 0, 1e-4, -1; + V.row(1) << 0, 1e-4, 1; + // + V.row(2) << -1e-4, 0, 0; + V.row(3) << -1, 0, 0; + V.row(4) << 1, 0, 0; + + E.resize(3, 2); + E.row(0) << 0, 1; + E.row(1) << 3, 2; + E.row(2) << 2, 4; + + CHECK(point_edge_distance(V.row(2), V.row(0), V.row(1)) < dhat * dhat); + } + SECTION("3D Edge-Edge Parallel") + { + V.resize(5, 3); + // + V.row(0) << -0.5, 1e-5, -1e-3; + V.row(1) << 0.5, 1e-5, 1e-3; + // + V.row(2) << -1, -1e-5, 0; + V.row(3) << 0, -1e-5, 0; + V.row(4) << 1, -1e-5, 0; + + E.resize(3, 2); + E.row(0) << 0, 1; + E.row(1) << 2, 3; + E.row(2) << 3, 4; + + CHECK(point_edge_distance(V.row(3), V.row(0), V.row(1)) < dhat * dhat); + } + + const CollisionMesh mesh(V, E, F); + + CollisionConstraints collision_constraints; + collision_constraints.set_use_convergent_formulation( + use_convergent_formulation); + + collision_constraints.build(mesh, V, dhat); + CHECK(collision_constraints.size() > 0); + + const Eigen::VectorXd grad_b = + collision_constraints.compute_potential_gradient(mesh, V, dhat); + + // const Eigen::MatrixXd force = -fd::unflatten(grad_b, V.cols()); + // std::cout << "force:\n" << force << std::endl; + + // if (use_convergent_formulation) { + // constexpr double eps = std::numeric_limits::epsilon(); + // CHECK(grad_b(0) == Catch::Approx(0).margin(eps)); + // CHECK(grad_b(2 * V.cols()) == Catch::Approx(0).margin(eps)); + // } else { + // CHECK(grad_b(0) != 0); + // CHECK(grad_b(2 * V.cols()) != 0); + // } + + // Compute the gradient using finite differences + auto f = [&](const Eigen::VectorXd& x) { + const Eigen::MatrixXd fd_V = fd::unflatten(x, mesh.dim()); + + CollisionConstraints fd_collision_constraints; + fd_collision_constraints.set_use_convergent_formulation( + use_convergent_formulation); + + fd_collision_constraints.build(mesh, fd_V, dhat); + + return fd_collision_constraints.compute_potential(mesh, fd_V, dhat); + }; + Eigen::VectorXd fgrad_b; + fd::finite_gradient(fd::flatten(V), f, fgrad_b); + + CHECK(fd::compare_gradient(grad_b, fgrad_b)); } \ No newline at end of file