From c2418566e31a6cd39fa08f27a7236fb38021223b Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Aug 2023 02:33:47 -0400 Subject: [PATCH 01/18] Add 2D improved max op --- src/ipc/collisions/collision_constraints.cpp | 79 +++++++++++++++++++ .../collision_constraints_builder.cpp | 58 ++++++++++++++ .../collision_constraints_builder.hpp | 8 ++ 3 files changed, 145 insertions(+) diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index bb52b86c6..ee171ad66 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -1,10 +1,13 @@ #include "collision_constraints.hpp" #include +#include +#include // #include #include #include +#include #include #include @@ -74,8 +77,84 @@ void CollisionConstraints::build( r.end()); }); + // ------------------------------------------------------------------------- + + // Convert edge-vertex to vertex-vertex + std::vector vv_candidates; + for (const auto& [ei, vi] : candidates.ev_candidates) { + for (int j = 0; j < 2; j++) { + const int vj = mesh.edges()(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()); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), vv_candidates.size()), + [&](const tbb::blocked_range& r) { + storage.local().add_vertex_vertex_negative_constraints( + mesh, vertices, vv_candidates, is_active, r.begin(), r.end()); + }); + + // ------------------------------------------------------------------------- + + // Convert edge-vertex to vertex-vertex + // std::vector vv_candidates; + vv_candidates.clear(); + for (const auto& [fi, vi] : candidates.fv_candidates) { + for (int j = 0; j < 3; j++) { + const int vj = mesh.faces()(fi, 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()); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), vv_candidates.size()), + [&](const tbb::blocked_range& r) { + storage.local().add_vertex_vertex_positive_constraints( + mesh, vertices, vv_candidates, is_active, r.begin(), r.end()); + }); + + // ------------------------------------------------------------------------- + CollisionConstraintsBuilder::merge(storage, *this); + // logger().critical("---"); + // for (const auto& vv : vv_constraints) { + // logger().critical( + // "vv: {} {}, w: {}, d: {}", 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) { + // logger().critical( + // "ev: {}=({}, {}) {}, w: {}, d: {}", 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)))); + // } + // logger().critical("---"); + for (size_t ci = 0; ci < size(); ci++) { CollisionConstraint& constraint = (*this)[ci]; constraint.minimum_distance = dmin; diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 81b8c3105..bd2f4d13d 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -252,6 +252,58 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( // ============================================================================ +void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i) +{ + auto foo = [&](const size_t vi, const size_t vj, double& weight, + Eigen::SparseVector& weight_gradient) { + // ÷ 2 to handle double counting for correct integration + const double area_weight = + use_convergent_formulation() ? (mesh.vertex_area(vi) / 2) : 1; + + Eigen::SparseVector area_weight_gradient; + if (should_compute_weight_gradient()) { + area_weight_gradient = use_convergent_formulation() + ? (mesh.vertex_area_gradient(vi) / 2) + : Eigen::SparseVector(vertices.size()); + } + + 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) { + weight += (1 - incident_edge_amt) * area_weight; + weight_gradient += (1 - incident_edge_amt) * area_weight_gradient; + } + }; + + 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()); + } + + foo(vi, vj, weight, weight_gradient); + foo(vj, vi, weight, weight_gradient); + + if (weight != 0) { + add_vertex_vertex_constraint(vi, vj, weight, weight_gradient); + } + } +} + +// ============================================================================ + void CollisionConstraintsBuilder::add_vertex_vertex_constraint( const long v0i, const long v1i, @@ -363,6 +415,12 @@ void CollisionConstraintsBuilder::merge( fv_constraints.end(), local_constraints.fv_constraints.begin(), local_constraints.fv_constraints.end()); } + + vv_constraints.erase( + std::remove_if( + vv_constraints.begin(), vv_constraints.end(), + [&](const VertexVertexConstraint& vv) { return vv.weight == 0; }), + vv_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..cae09e183 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -39,6 +39,14 @@ class CollisionConstraintsBuilder { const size_t start_i, const size_t end_i); + void add_vertex_vertex_negative_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i); + static void merge( const tbb::enumerable_thread_specific& local_storage, From f25520e081f17c4621e8a9c93eed701be27ce219 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 04:54:48 -0400 Subject: [PATCH 02/18] Add 3D improved max op * TODO: handle mollified EE separately * TODO: add negative weight EV unclassified --- src/ipc/collisions/collision_constraints.cpp | 221 ++++++++++++++---- .../collision_constraints_builder.cpp | 157 +++++++++++-- .../collision_constraints_builder.hpp | 24 ++ 3 files changed, 339 insertions(+), 63 deletions(-) diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index ee171ad66..158581cd3 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -3,7 +3,7 @@ #include #include #include -// #include +#include #include #include @@ -15,6 +15,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, @@ -79,58 +199,65 @@ void CollisionConstraints::build( // ------------------------------------------------------------------------- - // Convert edge-vertex to vertex-vertex - std::vector vv_candidates; - for (const auto& [ei, vi] : candidates.ev_candidates) { - for (int j = 0; j < 2; j++) { - const int vj = mesh.edges()(ei, j); - if (is_active( - point_point_distance(vertices.row(vi), vertices.row(vj)))) { - vv_candidates.emplace_back(vi, vj); - } - } + if (use_convergent_formulation() && 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_vertex_vertex_negative_constraints( + mesh, vertices, vv_candidates, is_active, r.begin(), + r.end()); + }); } - // 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()); - - tbb::parallel_for( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_vertex_vertex_negative_constraints( - mesh, vertices, vv_candidates, is_active, r.begin(), r.end()); - }); - // ------------------------------------------------------------------------- - // Convert edge-vertex to vertex-vertex - // std::vector vv_candidates; - vv_candidates.clear(); - for (const auto& [fi, vi] : candidates.fv_candidates) { - for (int j = 0; j < 3; j++) { - const int vj = mesh.faces()(fi, j); - if (is_active( - point_point_distance(vertices.row(vi), vertices.row(vj)))) { - vv_candidates.emplace_back(vi, vj); - } - } + if (use_convergent_formulation() && 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_edge_vertex_negative_constraints( + mesh, vertices, ev_candidates, is_active, 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_vertex_vertex_positive_constraints( + mesh, vertices, vv_candidates, is_active, r.begin(), + r.end()); + }); } - // 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()); - - tbb::parallel_for( - tbb::blocked_range(size_t(0), vv_candidates.size()), - [&](const tbb::blocked_range& r) { - storage.local().add_vertex_vertex_positive_constraints( - mesh, vertices, vv_candidates, is_active, r.begin(), r.end()); - }); + if (use_convergent_formulation() && candidates.ee_candidates.size() > 0) { + // Convert edge-edge to edge-vertex + const std::vector 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_vertex_negative_constraints2( + mesh, vertices, ev_candidates, is_active, r.begin(), + r.end()); + }); + } // ------------------------------------------------------------------------- diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index bd2f4d13d..cac86a74c 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -260,26 +260,68 @@ void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( const size_t start_i, const size_t end_i) { - auto foo = [&](const size_t vi, const size_t vj, double& weight, - Eigen::SparseVector& weight_gradient) { - // ÷ 2 to handle double counting for correct integration - const double area_weight = - use_convergent_formulation() ? (mesh.vertex_area(vi) / 2) : 1; + 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) + * (mesh.vertex_area_gradient(vi) / 2); + } + } + }; - Eigen::SparseVector area_weight_gradient; + 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()) { - area_weight_gradient = use_convergent_formulation() - ? (mesh.vertex_area_gradient(vi) / 2) - : Eigen::SparseVector(vertices.size()); + 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_vertex_vertex_positive_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + 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 (mesh.is_vertex_on_boundary(vj) + || incident_vertices.find(vi) != incident_vertices.end()) { + return; // Skip boundary vertices and incident vertices + } - if (incident_edge_amt > 1) { - weight += (1 - incident_edge_amt) * area_weight; - weight_gradient += (1 - incident_edge_amt) * area_weight_gradient; + // ÷ 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; } }; @@ -293,8 +335,8 @@ void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( weight_gradient = Eigen::SparseVector(vertices.size()); } - foo(vi, vj, weight, weight_gradient); - foo(vj, vi, weight, weight_gradient); + add_weight(vi, vj, weight, weight_gradient); + add_weight(vj, vi, weight, weight_gradient); if (weight != 0) { add_vertex_vertex_constraint(vi, vj, weight, weight_gradient); @@ -302,6 +344,80 @@ void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( } } +void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + 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() + && use_convergent_formulation()) { + weight_gradient = (1 - incident_triangle_amt) + * (mesh.vertex_area_gradient(vi) / 4); + } + + // TODO: Add this unclassified + add_edge_vertex_constraint(ei, vi, weight, weight_gradient); + } + } +} + +void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints2( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i) +{ + for (size_t i = start_i; i < end_i; i++) { + // ÷ 4 to handle double counting and PT + EE for correct integration. + const auto& [ei, vi] = candidates[i]; + assert(vi != mesh.edges()(ei, 0) && vi != mesh.edges()(ei, 1)); + + // TODO: distinguish mollified vs non-mollified + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vi]; + const int incident_edge_amt = incident_vertices.size() + - int(incident_vertices.find(mesh.edges()(ei, 0)) + != incident_vertices.end()) + - int(incident_vertices.find(mesh.edges()(ei, 1)) + != incident_vertices.end()); + + if (incident_edge_amt > 1) { + // ÷ 4 to handle double counting and PT + EE for correct integration + const double weight = (1 - incident_edge_amt) + * (use_convergent_formulation() ? (mesh.edge_area(ei) / 4) : 1); + + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient() + && use_convergent_formulation()) { + weight_gradient = + (1 - incident_edge_amt) * (mesh.edge_area_gradient(vi) / 4); + } + + // TODO: Add this unclassified + add_edge_vertex_constraint(ei, vi, weight, weight_gradient); + } + } +} + // ============================================================================ void CollisionConstraintsBuilder::add_vertex_vertex_constraint( @@ -416,11 +532,20 @@ void CollisionConstraintsBuilder::merge( local_constraints.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()); } } // 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 cae09e183..beed64068 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -47,6 +47,30 @@ class CollisionConstraintsBuilder { const size_t start_i, const size_t end_i); + void add_vertex_vertex_positive_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i); + + void add_edge_vertex_negative_constraints( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i); + + void add_edge_vertex_negative_constraints2( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const std::function& is_active, + const size_t start_i, + const size_t end_i); + static void merge( const tbb::enumerable_thread_specific& local_storage, From f916e18ab62a293300b8bfe5f5b652f99ba09883 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 11:09:52 -0400 Subject: [PATCH 03/18] Initialize adjacencies by default --- src/ipc/collision_mesh.cpp | 2 +- src/ipc/collision_mesh.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 2960b9b47..e7b970848 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(); } diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 738bf4fa6..01a7e742e 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -169,7 +169,7 @@ class CollisionMesh { { 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; } From 2f795087b2d6711d7c0880b6a2529c07317db899 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 14:46:04 -0400 Subject: [PATCH 04/18] Test and fix improved max op for EE --- src/ipc/collision_mesh.cpp | 10 +- src/ipc/collisions/collision_constraints.cpp | 102 ++++++++++++----- src/ipc/collisions/collision_constraints.hpp | 3 + .../collision_constraints_builder.cpp | 18 +-- .../collision_constraints_builder.hpp | 4 +- tests/test_ipc.cpp | 105 +++++++++++++++++- 6 files changed, 195 insertions(+), 47 deletions(-) diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index e7b970848..b3689623d 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -250,8 +250,14 @@ 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() diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 158581cd3..1ee2779da 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include @@ -99,14 +101,24 @@ namespace { return ev_candidates; } - std::vector edge_edge_to_edge_vertex_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; + std::vector> ev_candidates; for (const EdgeEdgeCandidate& ee : ee_candidates) { + if (edge_edge_distance_type( + 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))) + == EdgeEdgeDistanceType::EA_EB) { + continue; + } + 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; @@ -119,18 +131,13 @@ namespace { if (is_active(point_edge_distance( vertices.row(vj), // vertices.row(ei0), vertices.row(ei1)))) { - ev_candidates.emplace_back(ei, vj); + ev_candidates.emplace_back( + EdgeVertexCandidate(ei, vj), mesh.edge_area(ej)); } } } } - // 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 @@ -246,14 +253,14 @@ void CollisionConstraints::build( if (use_convergent_formulation() && candidates.ee_candidates.size() > 0) { // Convert edge-edge to edge-vertex - const std::vector ev_candidates = - edge_edge_to_edge_vertex_candidates( + const std::vector> + 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_vertex_negative_constraints2( + storage.local().add_edge_vertex_negative_constraints( mesh, vertices, ev_candidates, is_active, r.begin(), r.end()); }); @@ -263,24 +270,7 @@ void CollisionConstraints::build( CollisionConstraintsBuilder::merge(storage, *this); - // logger().critical("---"); - // for (const auto& vv : vv_constraints) { - // logger().critical( - // "vv: {} {}, w: {}, d: {}", 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) { - // logger().critical( - // "ev: {}=({}, {}) {}, w: {}, d: {}", 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)))); - // } - // logger().critical("---"); + // logger().debug(to_string(mesh, vertices)); for (size_t ci = 0; ci < size(); ci++) { CollisionConstraint& constraint = (*this)[ci]; @@ -621,4 +611,56 @@ 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: {}, d: {}", 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: {}, d: {}", 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: {}, d: {}", 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, + line_line_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)))); + } + for (const auto& fv : fv_constraints) { + ss << "\n" + << fmt::format( + "fv: {}=({}, {}, {}) {}, w: {}, d: {}", 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 cac86a74c..09ed1d3b3 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -379,31 +379,33 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( } } -void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints2( +void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, - const std::vector& candidates, + const std::vector>& candidates, const std::function& is_active, const size_t start_i, const size_t end_i) { for (size_t i = start_i; i < end_i; i++) { - // ÷ 4 to handle double counting and PT + EE for correct integration. - const auto& [ei, vi] = candidates[i]; - assert(vi != mesh.edges()(ei, 0) && vi != mesh.edges()(ei, 1)); + const auto& [ei, vi] = candidates[i].first; + const int e0i = mesh.edges()(ei, 0), e1i = mesh.edges()(ei, 1); + assert(vi != e0i && vi != e1i); // TODO: distinguish mollified vs non-mollified const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vi]; const int incident_edge_amt = incident_vertices.size() - - int(incident_vertices.find(mesh.edges()(ei, 0)) + - int(incident_vertices.find(mesh.edges()(e0i, 0)) != incident_vertices.end()) - - int(incident_vertices.find(mesh.edges()(ei, 1)) + - int(incident_vertices.find(mesh.edges()(e1i, 1)) != incident_vertices.end()); if (incident_edge_amt > 1) { // ÷ 4 to handle double counting and PT + EE for correct integration const double weight = (1 - incident_edge_amt) - * (use_convergent_formulation() ? (mesh.edge_area(ei) / 4) : 1); + * (use_convergent_formulation() + ? ((mesh.edge_area(ei) + candidates[i].second) / 4) + : 1); Eigen::SparseVector weight_gradient; if (should_compute_weight_gradient() diff --git a/src/ipc/collisions/collision_constraints_builder.hpp b/src/ipc/collisions/collision_constraints_builder.hpp index beed64068..e3668d160 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -63,10 +63,10 @@ class CollisionConstraintsBuilder { const size_t start_i, const size_t end_i); - void add_edge_vertex_negative_constraints2( + void add_edge_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, - const std::vector& candidates, + const std::vector>& candidates, const std::function& is_active, const size_t start_i, const size_t end_i); diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index c822b04ed..297ea0b95 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,99 @@ 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, 0.9; + // + V.row(2) << 1e-4, 0, 0; + V.row(3) << -0.33, 0, 0; + V.row(4) << 0.5, 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); + } + + 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()); + + 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)); + // CHECK(grad_b(3 * V.cols()) == 0); + } else { + CHECK(grad_b(0) != 0); + CHECK(grad_b(2 * V.cols()) != 0); + // CHECK(grad_b(3 * V.cols()) != 0); + } + + // Compute the gradient using finite differences + auto f = [&](const Eigen::VectorXd& x) { + return collision_constraints.compute_potential( + mesh, fd::unflatten(x, V.cols()), 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 From 258d4afa660e410f29c5cd8ee1108a1e424f98dd Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 15:11:47 -0400 Subject: [PATCH 05/18] Fix edge area Jacobian --- src/ipc/collision_mesh.cpp | 79 +++++++++++-------- src/ipc/collisions/collision_constraints.cpp | 18 +++-- .../collision_constraints_builder.cpp | 13 +-- .../collision_constraints_builder.hpp | 5 +- 4 files changed, 70 insertions(+), 45 deletions(-) diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index b3689623d..402fdf02d 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -262,53 +262,66 @@ void CollisionMesh::init_areas() 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())); - const VectorMax6d edge_len_gradient = edge_length_gradient(e0, e1) / 2; + // 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)); - for (int j = 0; j < m_edges.cols(); j++) { + const Vector9d face_area_gradient = + triangle_area_gradient(f0, f1, f2) / 3.0; + + 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/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 1ee2779da..7fd7bbcb9 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -101,14 +101,17 @@ namespace { return ev_candidates; } - std::vector> + std::vector< + std::tuple>> 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; + std::vector>> + ev_candidates; for (const EdgeEdgeCandidate& ee : ee_candidates) { if (edge_edge_distance_type( vertices.row(mesh.edges()(ee.edge0_id, 0)), @@ -131,8 +134,12 @@ namespace { if (is_active(point_edge_distance( vertices.row(vj), // vertices.row(ei0), vertices.row(ei1)))) { + Eigen::SparseVector weight_gradient; + if (mesh.are_area_jacobians_initialized()) + weight_gradient = mesh.edge_area_gradient(ei); ev_candidates.emplace_back( - EdgeVertexCandidate(ei, vj), mesh.edge_area(ej)); + EdgeVertexCandidate(ei, vj), mesh.edge_area(ej), + weight_gradient); } } } @@ -253,9 +260,8 @@ void CollisionConstraints::build( if (use_convergent_formulation() && candidates.ee_candidates.size() > 0) { // Convert edge-edge to edge-vertex - const std::vector> - ev_candidates = edge_edge_to_edge_vertex_candidates( - mesh, vertices, candidates.ee_candidates, is_active); + 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()), diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 09ed1d3b3..94b956984 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -382,13 +382,15 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, - const std::vector>& candidates, + const std::vector< + std::tuple>>& + candidates, const std::function& is_active, 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].first; + const auto& [ei, vi] = std::get<0>(candidates[i]); const int e0i = mesh.edges()(ei, 0), e1i = mesh.edges()(ei, 1); assert(vi != e0i && vi != e1i); @@ -404,14 +406,15 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( // ÷ 4 to handle double counting and PT + EE for correct integration const double weight = (1 - incident_edge_amt) * (use_convergent_formulation() - ? ((mesh.edge_area(ei) + candidates[i].second) / 4) + ? ((mesh.edge_area(ei) + std::get<1>(candidates[i])) / 4) : 1); Eigen::SparseVector weight_gradient; if (should_compute_weight_gradient() && use_convergent_formulation()) { - weight_gradient = - (1 - incident_edge_amt) * (mesh.edge_area_gradient(vi) / 4); + weight_gradient = (1 - incident_edge_amt) + * (mesh.edge_area_gradient(ei) + std::get<2>(candidates[i])) + / 4; } // TODO: Add this unclassified diff --git a/src/ipc/collisions/collision_constraints_builder.hpp b/src/ipc/collisions/collision_constraints_builder.hpp index e3668d160..d7179d868 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -66,7 +66,10 @@ class CollisionConstraintsBuilder { void add_edge_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, - const std::vector>& candidates, + const std::vector>>& candidates, const std::function& is_active, const size_t start_i, const size_t end_i); From 2e9ebbfe4db42d68c20c1f429fb58157696cb0b3 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 15:41:11 -0400 Subject: [PATCH 06/18] Fix negative constraint dtypes --- src/ipc/collisions/collision_constraints.cpp | 107 +++++++++--------- .../collision_constraints_builder.cpp | 79 +++++++------ .../collision_constraints_builder.hpp | 16 ++- 3 files changed, 109 insertions(+), 93 deletions(-) diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 7fd7bbcb9..6495294d7 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -211,65 +211,60 @@ void CollisionConstraints::build( r.end()); }); - // ------------------------------------------------------------------------- - - if (use_convergent_formulation() && 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_vertex_vertex_negative_constraints( - mesh, vertices, vv_candidates, is_active, r.begin(), - 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_vertex_vertex_negative_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); + }); + } - if (use_convergent_formulation() && 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_edge_vertex_negative_constraints( - mesh, vertices, ev_candidates, is_active, 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_vertex_vertex_positive_constraints( - mesh, vertices, vv_candidates, is_active, 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_vertex_negative_constraints( + mesh, vertices, ev_candidates, r.begin(), r.end()); + }); + } - if (use_convergent_formulation() && 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_vertex_negative_constraints( - mesh, vertices, ev_candidates, is_active, 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_edge_vertex_negative_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_vertex_vertex_positive_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); + }); + } } // ------------------------------------------------------------------------- diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 94b956984..1a90871cd 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -31,8 +31,8 @@ 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; @@ -48,29 +48,38 @@ void CollisionConstraintsBuilder::add_edge_vertex_constraints( : 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; } } @@ -256,7 +265,6 @@ void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i) { @@ -304,7 +312,6 @@ void CollisionConstraintsBuilder::add_vertex_vertex_positive_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i) { @@ -348,7 +355,6 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i) { @@ -373,8 +379,12 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( * (mesh.vertex_area_gradient(vi) / 4); } - // TODO: Add this unclassified - add_edge_vertex_constraint(ei, vi, weight, weight_gradient); + 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); } } } @@ -385,7 +395,6 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( const std::vector< std::tuple>>& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i) { @@ -417,8 +426,12 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( / 4; } - // TODO: Add this unclassified - add_edge_vertex_constraint(ei, vi, weight, weight_gradient); + add_edge_vertex_constraint( + mesh, std::get<0>(candidates[i]), + point_edge_distance_type( + vertices.row(vi), vertices.row(mesh.edges()(ei, 0)), + vertices.row(mesh.edges()(ei, 1))), + weight, weight_gradient); } } } @@ -463,7 +476,7 @@ void CollisionConstraintsBuilder::add_edge_vertex_constraint( ev_constraints[found_item->second].weight += weight; ev_constraints[found_item->second].weight_gradient += 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; diff --git a/src/ipc/collisions/collision_constraints_builder.hpp b/src/ipc/collisions/collision_constraints_builder.hpp index d7179d868..c4fd31c6e 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -39,11 +39,13 @@ class CollisionConstraintsBuilder { const size_t start_i, const size_t end_i); + // ------------------------------------------------------------------------ + // Duplicate removal functions + void add_vertex_vertex_negative_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i); @@ -51,7 +53,6 @@ class CollisionConstraintsBuilder { const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i); @@ -59,7 +60,6 @@ class CollisionConstraintsBuilder { const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, const std::vector& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i); @@ -70,10 +70,11 @@ class CollisionConstraintsBuilder { EdgeVertexCandidate, double, Eigen::SparseVector>>& candidates, - const std::function& is_active, const size_t start_i, const size_t end_i); + // ------------------------------------------------------------------------ + static void merge( const tbb::enumerable_thread_specific& local_storage, @@ -118,6 +119,13 @@ class CollisionConstraintsBuilder { constraints.ev_constraints); } + void add_edge_vertex_constraint( + const CollisionMesh& mesh, + const EdgeVertexCandidate& candidate, + const PointEdgeDistanceType dtype, + const double weight, + const Eigen::SparseVector& weight_gradient); + bool use_convergent_formulation() const { return constraints.use_convergent_formulation(); From cd23484848006767c701bb055d0e96660b808e83 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 15:43:31 -0400 Subject: [PATCH 07/18] Update actions/checkout --- .github/workflows/continuous.yml | 4 ++-- .github/workflows/docs.yml | 2 +- .github/workflows/pypi.yml | 4 ++-- .github/workflows/python.yml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/continuous.yml b/.github/workflows/continuous.yml index f36f81984..a8aec4c94 100644 --- a/.github/workflows/continuous.yml +++ b/.github/workflows/continuous.yml @@ -29,7 +29,7 @@ jobs: name: Linux steps: - name: Checkout repository - uses: actions/checkout@v2.5.0 + uses: actions/checkout@v4.0.0 with: fetch-depth: 10 @@ -84,7 +84,7 @@ jobs: config: [Debug, Release] steps: - name: Checkout repository - uses: actions/checkout@v2.5.0 + uses: actions/checkout@v4.0.0 with: fetch-depth: 10 diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 429766633..d09734f9a 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -11,7 +11,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4.0.0 with: fetch-depth: 10 diff --git a/.github/workflows/pypi.yml b/.github/workflows/pypi.yml index 4075cb370..3dcd30495 100644 --- a/.github/workflows/pypi.yml +++ b/.github/workflows/pypi.yml @@ -37,7 +37,7 @@ jobs: name: macOS steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4.0.0 - name: Build wheels uses: pypa/cibuildwheel@v2.12.1 @@ -57,7 +57,7 @@ jobs: name: Build source distribution runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4.0.0 - name: Build sdist run: pipx run build --sdist diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 8d9dbedc9..f3db9cf78 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -27,7 +27,7 @@ jobs: name: macOS steps: - name: Checkout Repository - uses: actions/checkout@v3 + uses: actions/checkout@v4.0.0 with: fetch-depth: 10 From 6400d434131a1622025b7a7e4ece646460368adf Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 14 Sep 2023 16:10:57 -0400 Subject: [PATCH 08/18] Update FindSIMD --- CMakeLists.txt | 18 +- cmake/find/FindAVX.cmake | 163 ------------ cmake/find/FindFMA.cmake | 107 -------- cmake/find/FindSIMD.cmake | 514 ++++++++++++++++++++++++++++++++++++++ cmake/find/FindSSE.cmake | 300 ---------------------- 5 files changed, 521 insertions(+), 581 deletions(-) delete mode 100644 cmake/find/FindAVX.cmake delete mode 100644 cmake/find/FindFMA.cmake create mode 100644 cmake/find/FindSIMD.cmake delete mode 100644 cmake/find/FindSSE.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index c42d2b304..4aeb5267b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,18 +189,14 @@ target_link_libraries(ipc_toolkit PRIVATE ipc::toolkit::warnings) ## SIMD support if(IPC_TOOLKIT_WITH_SIMD) - # Figure out SSE level support - message(STATUS "Seaching for SSE...") - find_package(SSE) - # Figure out AVX level support - message(STATUS "Searching for AVX...") - find_package(AVX) - # Figure out FMA level support - message(STATUS "Searching for FMA...") - find_package(FMA) - # Add SSE, AVX, and FMA flags to compiler flags - string(REPLACE " " ";" SIMD_FLAGS "${SSE_FLAGS} ${AVX_FLAGS} ${FMA_FLAGS}") + # Figure out SIMD support + message(STATUS "Testing SIMD capabilities...") + find_package(SIMD) + # Add SIMD flags to compiler flags + message(STATUS "Using SIMD flags: ${SIMD_FLAGS}") target_compile_options(ipc_toolkit PRIVATE ${SIMD_FLAGS}) +else() + message(STATUS "SIMD support disabled") endif() # Use C++17 diff --git a/cmake/find/FindAVX.cmake b/cmake/find/FindAVX.cmake deleted file mode 100644 index b3d1e2c4e..000000000 --- a/cmake/find/FindAVX.cmake +++ /dev/null @@ -1,163 +0,0 @@ -################################################################################ -# Copyright (c) 2016, Project Chrono Development Team -# All rights reserved. -# This file is licensed to you under the BSD 3-Clause "New" or "Revised" License. -# See https://github.com/projectchrono/chrono/blob/develop/LICENSE for details. -################################################################################ -# -# This script checks for the highest level of AVX support on the host -# by compiling and running small C++ programs that use AVX intrinsics. -# -# You can invoke this module using the following command: -# -# FIND_PACKAGE(AVX [major[.minor]] [EXACT] [QUIET|REQUIRED]) -# -# where the version string is one of: -# -# 1.0 for AVX support -# 2.0 for AVX2 support -# -# Note that any ".0" in the above version string is optional. -# -# If any AVX support is detected, the following variables are set: -# -# AVX_FOUND = 1 -# AVX_VERSION = the requested version, if EXACT is true, or -# the highest AVX version found. -# AVX_FLAGS = compile flags for the version of AVX found -# -# If AVX is not supported on the host platform, these variables are -# not set. If QUIET is true, the module does not print a message if -# AVX if missing. If REQUIRED is true, the module produces a fatal -# error if AVX support is missing. -# -set(AVX_FLAGS) -set(AVX_FOUND) -set(DETECTED_AVX_10) -set(DETECTED_AVX_20) - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING) - if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING) - SET(AVX_FLAGS "${AVX_FLAGS} -march=native") - message(STATUS "Using CPU native flags for AVX optimization: ${AVX_FLAGS}") - endif() -endif() - -include(CheckCXXSourceRuns) -set(CMAKE_REQUIRED_FLAGS) - - -# Generate a list of AVX versions to test. -if(AVX_FIND_VERSION_EXACT) - if(AVX_FIND_VERSION VERSION_EQUAL "2.0") - set(_AVX_TEST_20 1) - elseif(AVX_FIND_VERSION VERSION_EQUAL "1.0") - set(_AVX_TEST_10 1) - endif() -else() - if(NOT AVX_FIND_VERSION VERSION_GREATER "2.0") - set(_AVX_TEST_20 1) - endif() - if(NOT AVX_FIND_VERSION VERSION_GREATER "1.0") - set(_AVX_TEST_10 1) - endif() -endif() - -# Check for AVX2 support. -if(_AVX_TEST_20) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-mavx2") - elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - set(CMAKE_REQUIRED_FLAGS "-xHost") - elseif(MSVC AND NOT CMAKE_CL_64) - set(CMAKE_REQUIRED_FLAGS "/arch:AVX2") - endif() - check_cxx_source_runs(" - #include - int main() - { - __m256i a = _mm256_set_epi32 (-1, 2, -3, 4, -1, 2, -3, 4); - __m256i result = _mm256_abs_epi32 (a); - return 0; - }" DETECTED_AVX_20) -endif() - -# Check for AVX support. -if(_AVX_TEST_10) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-mavx") - elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - set(CMAKE_REQUIRED_FLAGS "-xHost") - elseif(MSVC AND NOT CMAKE_CL_64) - set(CMAKE_REQUIRED_FLAGS "/arch:AVX") - endif() - check_cxx_source_runs(" - #include - int main() - { - __m256 a = _mm256_set_ps (-1.0f, 2.0f, -3.0f, 4.0f, -1.0f, 2.0f, -3.0f, 4.0f); - __m256 b = _mm256_set_ps (1.0f, 2.0f, 3.0f, 4.0f, 1.0f, 2.0f, 3.0f, 4.0f); - __m256 result = _mm256_add_ps (a, b); - return 0; - }" DETECTED_AVX_10) -endif() - -set(CMAKE_REQUIRED_FLAGS) - - -if(DETECTED_AVX_20) - set(AVX_VERSION "2.0") - set(AVX_STR "2_0") - set(AVX_FOUND 1) -elseif(DETECTED_AVX_10) - set(AVX_VERSION "1.0") - set(AVX_STR "1_0") - set(AVX_FOUND 1) -endif() - - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(DETECTED_AVX_20) - SET(AVX_FLAGS "${AVX_FLAGS} -mavx2") - elseif(DETECTED_AVX_10) - SET(AVX_FLAGS "${AVX_FLAGS} -mavx") - endif() - # TODO: Check for AVX512 support - SET(AVX_FLAGS "${AVX_FLAGS} -mno-avx512f -mno-avx512pf -mno-avx512er -mno-avx512cd") -elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - set(AVX_FLAGS "-xHost") -elseif(MSVC) - if(DETECTED_AVX_20) - SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX2") - elseif(DETECTED_AVX_10) - SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX") - endif() -endif() - -if(AVX_FOUND) - message(STATUS " Found AVX ${AVX_VERSION} extensions, using flags: ${AVX_FLAGS}") -else() - message(STATUS " No AVX support found") - set(AVX_FLAGS "") -endif() - -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${AVX_FLAGS}") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${AVX_FLAGS}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${AVX_FLAGS}") - -return() -#------------------------------------- - -# If no AVX support is found, print an error message. -if(AVX_FIND_VERSION) - set(_AVX_ERROR_MESSAGE "AVX ${AVX_FIND_VERSION} support is not found on this architecture") -else() - set(_AVX_ERROR_MESSAGE "AVX support is not found on this architecture") -endif() - -if(AVX_FIND_REQUIRED) - message(FATAL_ERROR "${_AVX_ERROR_MESSAGE}") -elseif(NOT AVX_FIND_QUIETLY) - message(STATUS "${_AVX_ERROR_MESSAGE}") -endif() diff --git a/cmake/find/FindFMA.cmake b/cmake/find/FindFMA.cmake deleted file mode 100644 index f435c1533..000000000 --- a/cmake/find/FindFMA.cmake +++ /dev/null @@ -1,107 +0,0 @@ -################################################################################ -# Copyright (c) 2016, Project Chrono Development Team -# All rights reserved. -# This file is licensed to you under the BSD 3-Clause "New" or "Revised" License. -# See https://github.com/projectchrono/chrono/blob/develop/LICENSE for details. -################################################################################ -# -# This script checks for the highest level of FMA support on the host -# by compiling and running small C++ programs that uses FMA intrinsics. -# -# You can invoke this module using the following command: -# -# FIND_PACKAGE(FMA [QUIET|REQUIRED]) -# -# If any FMA support is detected, the following variables are set: -# -# FMA_FOUND = 1 -# FMA_FLAGS = compile flags for the version of FMA found -# -# If FMA is not supported on the host platform, these variables are -# not set. If QUIET is true, the module does not print a message if -# FMA if missing. If REQUIRED is true, the module produces a fatal -# error if FMA support is missing. -# -set(FMA_FLAGS) -set(FMA_FOUND) -set(DETECTED_FMA) - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING) - if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING) - SET(FMA_FLAGS "${FMA_FLAGS} -march=native") - message(STATUS "Using CPU native flags for FMA optimization: ${FMA_FLAGS}") - endif() -endif() - -include(CheckCXXSourceRuns) -set(CMAKE_REQUIRED_FLAGS) - -# Generate a list of FMA versions to test. -set(_FMA_TEST 1) - -# Check for FMA support. -if(_FMA_TEST) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-mavx2 -mfma") - elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - set(CMAKE_REQUIRED_FLAGS "-fma") - elseif(MSVC AND NOT CMAKE_CL_64) - set(CMAKE_REQUIRED_FLAGS "/arch:AVX2") - endif() - check_cxx_source_runs(" - #include - int main() - { - __m256d a = _mm256_set_pd (-1, 2, -3, 4); - __m256d b = _mm256_set_pd (-2, 3, -4, 1); - __m256d c = _mm256_set_pd (-11, 6, 4, -1); - - __m256d result = _mm256_fmsub_pd (a, b, c); - return 0; - }" DETECTED_FMA) -endif() - -set(CMAKE_REQUIRED_FLAGS) - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(DETECTED_FMA) - SET(FMA_FLAGS "${FMA_FLAGS} -mfma") - SET(FMA_FOUND 1) - endif() -elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - if(DETECTED_FMA) - SET(FMA_FLAGS "${FMA_FLAGS} -fma") - SET(FMA_FOUND 1) - endif() -elseif(MSVC) - if(DETECTED_FMA) - SET(FMA_FLAGS "${FMA_FLAGS} /arch:AVX2") - SET(FMA_FOUND 1) - endif() -endif() - -if(FMA_FOUND) - message(STATUS " Found FMA extensions, using flags: ${FMA_FLAGS}") -else() - message(STATUS " No FMA support found") - set(FMA_FLAGS "") -endif() - -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${FMA_FLAGS}") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${FMA_FLAGS}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${FMA_FLAGS}") - -return() -#----------------------- - -# If no FMA support is found, print an error message. -if(FMA_FIND_REQUIRED) - set(_FMA_ERROR_MESSAGE "FMA support is not found on this architecture") -endif() - -if(FMA_FIND_REQUIRED) - message(FATAL_ERROR "${_FMA_ERROR_MESSAGE}") -elseif(NOT FMA_FIND_QUIETLY) - message(STATUS "${_FMA_ERROR_MESSAGE}") -endif() diff --git a/cmake/find/FindSIMD.cmake b/cmake/find/FindSIMD.cmake new file mode 100644 index 000000000..c3e8bf645 --- /dev/null +++ b/cmake/find/FindSIMD.cmake @@ -0,0 +1,514 @@ +################################################################################ +# Copyright (c) 2022, Project Chrono Development Team +# All rights reserved. +# This file is licensed to you under the BSD 3-Clause "New" or "Revised" License. +# See https://github.com/projectchrono/chrono/blob/main/LICENSE for details. +################################################################################ + +# SPDX-License-Identifier: BSD-3-Clause +# +# This script combines the search routines for various SIMD technologies into one place. +# + + +# This script checks for the highest level of SSE support on the host +# by compiling and running small C++ programs that uses SSE intrinsics. +# +# If any SSE support is detected, the following variables are set: +# +# SSE_FOUND = 1 +# SSE_VERSION = the highest SSE version found. +# SSE_FLAGS = compile flags for the version of SSE found +# +# If SSE is not supported on the host platform, these variables are +# not set. +# +# NOTE: 64-bit x86 architectures provide SSE 2.0 or support by default so it is not tested here. + +function (test_sse_availability) + + set(SSE_FLAGS) + set(SSE_FOUND) + set(DETECTED_SSE_41) + set(DETECTED_SSE_42) + set(DETECTED_SSE_30) + + include(CheckCXXSourceRuns) + set(CMAKE_REQUIRED_FLAGS) + + set(_SSE_TEST_42 1) + set(_SSE_TEST_41 1) + set(_SSE_TEST_30 1) + +# Check for SSE 4.2 support. + if(_SSE_TEST_42) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-msse4.2") + endif() + check_cxx_source_runs(" + #include + #include + int main() + { + long long a[2] = { 1, 2 }; + long long b[2] = { -1, 3 }; + long long c[2]; + __m128i va = _mm_loadu_si128((__m128i*)a); + __m128i vb = _mm_loadu_si128((__m128i*)b); + __m128i vc = _mm_cmpgt_epi64(va, vb); + + _mm_storeu_si128((__m128i*)c, vc); + if (c[0] == -1LL && c[1] == 0LL) + return 0; + else + return 1; + }" + DETECTED_SSE_42) + endif() + +# Check for SSE 4.1 support. + if(_SSE_TEST_41) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-msse4.1") + endif() + check_cxx_source_runs(" + #include + #include + int main() + { + long long a[2] = { 1, 2 }; + long long b[2] = { -1, 2 }; + long long c[2]; + __m128i va = _mm_loadu_si128((__m128i*)a); + __m128i vb = _mm_loadu_si128((__m128i*)b); + __m128i vc = _mm_cmpeq_epi64(va, vb); + + _mm_storeu_si128((__m128i*)c, vc); + if (c[0] == 0LL && c[1] == -1LL) + return 0; + else + return 1; + }" DETECTED_SSE_41) + endif() + +# Check for SSE 3 support. + if(_SSE_TEST_30) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-msse3") + endif() + check_cxx_source_runs(" + #include + #ifdef _WIN32 + #include + #else + #include + #endif + + int main() + { + float a[4] = { 1.0f, 2.0f, 3.0f, 4.0f }; + float b[4] = { 3.0f, 5.0f, 7.0f, 9.0f }; + float c[4]; + + __m128 va = _mm_loadu_ps(a); + __m128 vb = _mm_loadu_ps(b); + __m128 vc = _mm_hadd_ps(va, vb); + + _mm_storeu_ps(c, vc); + if (c[0] == 3.0f && c[1] == 7.0f && c[2] == 8.0f && c[3] == 16.0f) + return 0; + else + return 1; + }" DETECTED_SSE_30) + endif() + + + set(CMAKE_REQUIRED_FLAGS) + + if(DETECTED_SSE_42) + set(SSE_VERSION "4.2") + set(SSE_STR "4_2") + set(SSE_FOUND 1) + elseif(DETECTED_SSE_41) + set(SSE_VERSION "4.1") + set(SSE_STR "4_1") + set(SSE_FOUND 1) + elseif(DETECTED_SSE_30) + set(SSE_VERSION "3.0") + set(SSE_STR "3_0") + set(SSE_FOUND 1) + endif() + + + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) + if(DETECTED_SSE_42) + set(SSE_FLAGS "${SSE_FLAGS} -msse4.2 -mfpmath=sse") + endif() + if(DETECTED_SSE_41) + set(SSE_FLAGS "${SSE_FLAGS} -msse4.1 -mfpmath=sse") + endif() + if(DETECTED_SSE_30) + set(SSE_FLAGS "${SSE_FLAGS} -msse3 -mfpmath=sse") + endif() + elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # clang does not require "-mfpmath" which is automatic + if(DETECTED_SSE_42) + set(SSE_FLAGS "${SSE_FLAGS} -msse4.2") + endif() + if(DETECTED_SSE_41) + set(SSE_FLAGS "${SSE_FLAGS} -msse4.1") + endif() + if(DETECTED_SSE_30) + set(SSE_FLAGS "${SSE_FLAGS} -msse3") + endif() + elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") + set(SSE_FLAGS "-xHost") + elseif(MSVC AND CMAKE_SIZEOF_VOID_P EQUAL 4) + endif() + + # Export flags to caller scope + if(SSE_FOUND) + set(SSE_FOUND TRUE PARENT_SCOPE) + set(SSE_FLAGS "${SSE_FLAGS}" PARENT_SCOPE) + set(SSE_VERSION "${SSE_VERSION}" PARENT_SCOPE) + else() + set(SSE_FOUND FALSE PARENT_SCOPE) + set(SSE_FLAGS "") + endif() + + return() + +endfunction() + +# This script checks for the highest level of FMA support on the host +# by compiling and running small C++ programs that uses FMA intrinsics. + +# If any FMA support is detected, the following variables are set: +# +# FMA_FOUND = 1 +# FMA_FLAGS = compile flags for the version of FMA found +# +# If FMA is not supported on the host platform, these variables are +# not set. + +function (test_fma_availability) + set(FMA_FLAGS) + set(FMA_FOUND) + set(DETECTED_FMA) + + include(CheckCXXSourceRuns) + set(CMAKE_REQUIRED_FLAGS) + +# Generate a list of FMA versions to test. + set(_FMA_TEST 1) + +# Check for FMA support. + if(_FMA_TEST) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-mavx2 -mfma") + elseif(MSVC AND NOT CMAKE_CL_64) + set(CMAKE_REQUIRED_FLAGS "/arch:AVX2") + endif() + check_cxx_source_runs(" + #include + int main() + { + __m256d a = _mm256_set_pd (-1, 2, -3, 4); + __m256d b = _mm256_set_pd (-2, 3, -4, 1); + __m256d c = _mm256_set_pd (-11, 6, 4, -1); + + __m256d result = _mm256_fmsub_pd (a, b, c); + return 0; + }" DETECTED_FMA) + endif() + + set(CMAKE_REQUIRED_FLAGS) + + if(DETECTED_FMA) + SET(FMA_FOUND 1) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + SET(FMA_FLAGS "${FMA_FLAGS} -mfma") + endif() + endif() + + if(FMA_FOUND) + set(FMA_FOUND TRUE PARENT_SCOPE) + set(FMA_FLAGS "${FMA_FLAGS}" PARENT_SCOPE) + else() + set(FMA_FOUND FALSE PARENT_SCOPE) + set(FMA_FLAGS "") + endif() + + + return() +endfunction() + + +# This script checks for the highest level of AVX support on the host +# by compiling and running small C++ programs that use AVX intrinsics. +# +# If any AVX support is detected, the following variables are set: +# +# AVX_FOUND = 1 +# AVX_VERSION = the requested version, if EXACT is true, or +# the highest AVX version found. +# AVX_FLAGS = compile flags for the version of AVX found +# +# If AVX is not supported on the host platform, these variables are +# not set. + +function(test_avx_availability) + + set(AVX_FLAGS) + set(AVX_FOUND) + set(DETECTED_AVX_10) + set(DETECTED_AVX_20) + + include(CheckCXXSourceRuns) + set(CMAKE_REQUIRED_FLAGS) + + set(_AVX_TEST_20 1) + set(_AVX_TEST_10 1) + +# Check for AVX2 support. + if(_AVX_TEST_20) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-mavx2") + elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") + set(CMAKE_REQUIRED_FLAGS "-xHost") + elseif(MSVC AND NOT CMAKE_CL_64) + set(CMAKE_REQUIRED_FLAGS "/arch:AVX2") + endif() + check_cxx_source_runs(" + #include + int main() + { + __m256i a = _mm256_set_epi32 (-1, 2, -3, 4, -1, 2, -3, 4); + __m256i result = _mm256_abs_epi32 (a); + return 0; + }" DETECTED_AVX_20) + endif() + +# Check for AVX support. + if(_AVX_TEST_10) + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_REQUIRED_FLAGS "-mavx") + elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") + set(CMAKE_REQUIRED_FLAGS "-xHost") + elseif(MSVC AND NOT CMAKE_CL_64) + set(CMAKE_REQUIRED_FLAGS "/arch:AVX") + endif() + check_cxx_source_runs(" + #include + int main() + { + __m256 a = _mm256_set_ps (-1.0f, 2.0f, -3.0f, 4.0f, -1.0f, 2.0f, -3.0f, 4.0f); + __m256 b = _mm256_set_ps (1.0f, 2.0f, 3.0f, 4.0f, 1.0f, 2.0f, 3.0f, 4.0f); + __m256 result = _mm256_add_ps (a, b); + return 0; + }" DETECTED_AVX_10) + endif() + + set(CMAKE_REQUIRED_FLAGS) + + + if(DETECTED_AVX_20) + set(AVX_VERSION "2.0") + set(AVX_STR "2_0") + set(AVX_FOUND 1) + elseif(DETECTED_AVX_10) + set(AVX_VERSION "1.0") + set(AVX_STR "1_0") + set(AVX_FOUND 1) + endif() + + + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + if(DETECTED_AVX_20) + SET(AVX_FLAGS "${AVX_FLAGS} -mavx2") + endif() + if(DETECTED_AVX_10) + SET(AVX_FLAGS "${AVX_FLAGS} -mavx") + endif() + elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") + set(AVX_FLAGS "-xHost") + elseif(MSVC) + if(DETECTED_AVX_20) + SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX2") + endif() + if(DETECTED_AVX_10) + SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX") + endif() + endif() + + if(AVX_FOUND) + set(AVX_FOUND TRUE PARENT_SCOPE) + set(AVX_FLAGS "${AVX_FLAGS}" PARENT_SCOPE) + set(AVX_VERSION "${AVX_VERSION}" PARENT_SCOPE) + else() + set(AVX_FOUND FALSE PARENT_SCOPE) + set(AVX_FLAGS "") + endif() + + + return() + +endfunction() + +# This script checks for the highest level of NEON support on the host +# by compiling and running small C++ programs that uses NEON intrinsics. +# +# If any NEON support is detected, the following variables are set: +# +# NEON_FOUND = 1 +# NEON_VERSION = 2_0 (assumes Advanced SIMD 2.0) +# NEON_FLAGS = compile flags for the version of NEON found +# +# If NEON is not supported on the host platform, these variables are +# not set. +# + +function(test_neon_availability) + + set(NEON_FLAGS) + set(NEON_FOUND) + set(DETECTED_NEON) + + include(CheckCXXSourceRuns) + + set(CMAKE_REQUIRED_FLAGS "-march=armv8-a") +# Check for NEON support. + check_cxx_source_runs(" +#include + int main() + { + float64_t a[2] = { 1., 2. }; + float64_t b[2] = { -1., 3. }; + float64_t c[2]; + + float64x2_t va = vld1q_f64(&a[0]); + float64x2_t vb = vld1q_f64(&b[0]); + float64x2_t vc = vaddq_f64(va, vb); + vst1q_f64(&c[0], vc); + + if (c[0] == 0. && c[1] == 5.) + return 0; + else + return 0; + } + " DETECTED_NEON) + + set(CMAKE_REQUIRED_FLAGS) + + if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + if(DETECTED_NEON) + SET(NEON_FLAGS "-march=armv8-a") + set(NEON_STR "2_0") + SET(NEON_FOUND 1) + else() + # Setting -ffloat-store to alleviate 32bit vs 64bit discrepancies on non-SIMD platforms. + set(NEON_FLAGS "-ffloat-store") + endif() + endif() + + if(NEON_FOUND) + set(NEON_FOUND TRUE PARENT_SCOPE) + set(NEON_FLAGS "${NEON_FLAGS}" PARENT_SCOPE) + else() + set(NEON_FOUND FALSE PARENT_SCOPE) + set(NEON_FLAGS "") + endif() + + return() + +endfunction() + + + +### +# +# Perform SIMD checks as defined above +# +### + +set(SIMD_FLAGS "") + +set(SIMD_SSE "FALSE" CACHE STRING "Any detected SSE SIMD version, else FALSE") +set(SIMD_AVX "FALSE" CACHE STRING "Any detected AVX SIMD version, else FALSE") +set(SIMD_FMA ${FMA_FOUND} CACHE BOOL "Whether AVX2 FMA extensions were a detected SIMD feature") +set(SIMD_NEON ${NEON_FOUND} CACHE BOOL "Whether NEON was a detected SIMD feature") + +# Check availability of SSE instructions +test_sse_availability() +if (SSE_FOUND) + if (NOT ${SIMD_FIND_QUIETLY}) + message(STATUS "Target supports SSE instructions") + endif() + + set(SIMD_FLAGS "${SIMD_FLAGS} ${SSE_FLAGS}") + set(SIMD_SSE "${SSE_VERSION}") +endif() + +test_avx_availability() +if (AVX_FOUND) + if (NOT ${SIMD_FIND_QUIETLY}) + message(STATUS "Target supports AVX instructions") + endif() + + set(SIMD_FLAGS "${SIMD_FLAGS} ${AVX_FLAGS}") + set(SIMD_AVX "${AVX_VERSION}") +endif() + +test_fma_availability() +if (FMA_FOUND) + if (NOT ${SIMD_FIND_QUIETLY}) + message(STATUS "Target supports AVX2 FMA instructions") + endif() + + set(SIMD_FLAGS "${SIMD_FLAGS} ${FMA_FLAGS}") +endif() + +test_neon_availability() +if (NEON_FOUND) + if (NOT ${SIMD_FIND_QUIETLY}) + message(STATUS "Target supports NEON instructions") + endif() + + set(SIMD_FLAGS "${SIMD_FLAGS} ${NEON_FLAGS}") +endif() + +# Determine whether to use SIMD flags or automatic detection +if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) + execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING) + if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING) + SET(SIMD_FLAGS "-march=native") + if (NOT SIMD_FIND_QUIETLY) + message(STATUS "Using automatic native flag for SIMD optimization") + endif() + endif() +elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT CMAKE_CROSSCOMPILING) + execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE CLANG_VERSION_STRING) + if(CLANG_VERSION_STRING VERSION_GREATER_EQUAL 15.0 AND NOT CMAKE_CROSSCOMPILING) + SET(SIMD_FLAGS "-march=native") + if (NOT SIMD_FIND_QUIETLY) + message(STATUS "Using automatic native flag for SIMD optimization") + endif() + elseif(CMAKE_HOST_UNIX) + execute_process(COMMAND uname -m OUTPUT_VARIABLE UNIX_MACHINE_ARCH) + if(UNIX_MACHINE_ARCH MATCHES "x86_64|x86|amd64") + SET(SIMD_FLAGS "-march=native") + if (NOT SIMD_FIND_QUIETLY) + message(STATUS "Using automatic native flag for SIMD optimization") + endif() + endif() + endif() +endif() + +string(STRIP "${SIMD_FLAGS}" SIMD_FLAGS) +string(REPLACE " " ";" SIMD_FLAGS "${SIMD_FLAGS}") + +set(SIMD_C_FLAGS "${SIMD_FLAGS}" CACHE STRING "Flags used for compiling C programs with SIMD support") +set(SIMD_CXX_FLAGS "${SIMD_FLAGS}" CACHE STRING "Flags used for compiling C++ programs with SIMD support") + +mark_as_advanced(SIMD_SSE SIMD_AVX SIMD_FMA SIMD_NEON SIMD_C_FLAGS SIMD_CXX_FLAGS) + diff --git a/cmake/find/FindSSE.cmake b/cmake/find/FindSSE.cmake deleted file mode 100644 index 51c1ae8ab..000000000 --- a/cmake/find/FindSSE.cmake +++ /dev/null @@ -1,300 +0,0 @@ -################################################################################ -# Copyright (c) 2016, Project Chrono Development Team -# All rights reserved. -# This file is licensed to you under the BSD 3-Clause "New" or "Revised" License. -# See https://github.com/projectchrono/chrono/blob/develop/LICENSE for details. -################################################################################ -# -# This script checks for the highest level of SSE support on the host -# by compiling and running small C++ programs that uses SSE intrinsics. -# -# You can invoke this module using the following command: -# -# FIND_PACKAGE(SSE [major[.minor]] [EXACT] [QUIET|REQUIRED]) -# -# where the version string is one of: -# -# 1.0 for SSE support -# 2.0 for SSE2 support -# 3.0 for SSE3 support -# 3.1 for SSSE3 support -# 4.1 for SSE 4.1 support -# 4.2 for SSE 4.2 support -# -# Note that any ".0" in the above version string is optional. -# -# If any SSE support is detected, the following variables are set: -# -# SSE_FOUND = 1 -# SSE_VERSION = the requested version, if EXACT is true, or -# the highest SSE version found. -# SSE_FLAGS = compile flags for the version of SSE found -# -# If SSE is not supported on the host platform, these variables are -# not set. If QUIET is true, the module does not print a message if -# SSE if missing. If REQUIRED is true, the module produces a fatal -# error if SSE support is missing. -# -set(SSE_FLAGS) -set(SSE_FOUND) -set(DETECTED_SSE_41) -set(DETECTED_SSE_42) -set(DETECTED_SSE_10) -set(DETECTED_SSE_20) -set(DETECTED_SSE_30) - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING) - if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING) - SET(SSE_FLAGS "${SSE_FLAGS} -march=native") - message(STATUS "Using CPU native flags for SSE optimization: ${SSE_FLAGS}") - endif() -endif() - -include(CheckCXXSourceRuns) -set(CMAKE_REQUIRED_FLAGS) - - -# Generate a list of SSE versions to test. -if(SSE_FIND_VERSION_EXACT) - if(SSE_FIND_VERSION VERSION_EQUAL "4.2") - set(_SSE_TEST_42 1) - elseif(SSE_FIND_VERSION VERSION_EQUAL "4.1") - set(_SSE_TEST_41 1) - elseif(SSE_FIND_VERSION VERSION_EQUAL "3.0") - set(_SSE_TEST_30 1) - elseif(SSE_FIND_VERSION VERSION_EQUAL "2.0") - set(_SSE_TEST_20 1) - elseif(SSE_FIND_VERSION VERSION_EQUAL "1.0") - set(_SSE_TEST_10 1) - endif() -else() - if(NOT SSE_FIND_VERSION VERSION_GREATER "4.2") - set(_SSE_TEST_42 1) - endif() - if(NOT SSE_FIND_VERSION VERSION_GREATER "4.1") - set(_SSE_TEST_41 1) - endif() - if(NOT SSE_FIND_VERSION VERSION_GREATER "3.0") - set(_SSE_TEST_30 1) - endif() - if(NOT SSE_FIND_VERSION VERSION_GREATER "2.0") - set(_SSE_TEST_20 1) - endif() - if(NOT SSE_FIND_VERSION VERSION_GREATER "1.0") - set(_SSE_TEST_10 1) - endif() -endif() - - -# Check for SSE 4.2 support. -if(_SSE_TEST_42) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-msse4.2") - endif() - check_cxx_source_runs(" - #include - #include - int main() - { - long long a[2] = { 1, 2 }; - long long b[2] = { -1, 3 }; - long long c[2]; - __m128i va = _mm_loadu_si128((__m128i*)a); - __m128i vb = _mm_loadu_si128((__m128i*)b); - __m128i vc = _mm_cmpgt_epi64(va, vb); - - _mm_storeu_si128((__m128i*)c, vc); - if (c[0] == -1LL && c[1] == 0LL) - return 0; - else - return 1; - }" - DETECTED_SSE_42) -endif() - -# Check for SSE 4.1 support. -if(_SSE_TEST_41) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-msse4.1") - endif() - check_cxx_source_runs(" - #include - #include - int main() - { - long long a[2] = { 1, 2 }; - long long b[2] = { -1, 2 }; - long long c[2]; - __m128i va = _mm_loadu_si128((__m128i*)a); - __m128i vb = _mm_loadu_si128((__m128i*)b); - __m128i vc = _mm_cmpeq_epi64(va, vb); - - _mm_storeu_si128((__m128i*)c, vc); - if (c[0] == 0LL && c[1] == -1LL) - return 0; - else - return 1; - }" DETECTED_SSE_41) -endif() - -# Check for SSE 3 support. -if(_SSE_TEST_30) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-msse3") - endif() - check_cxx_source_runs(" - #include - #ifdef _WIN32 - #include - #else - #include - #endif - - int main() - { - float a[4] = { 1.0f, 2.0f, 3.0f, 4.0f }; - float b[4] = { 3.0f, 5.0f, 7.0f, 9.0f }; - float c[4]; - - __m128 va = _mm_loadu_ps(a); - __m128 vb = _mm_loadu_ps(b); - __m128 vc = _mm_hadd_ps(va, vb); - - _mm_storeu_ps(c, vc); - if (c[0] == 3.0f && c[1] == 7.0f && c[2] == 8.0f && c[3] == 16.0f) - return 0; - else - return 1; - }" DETECTED_SSE_30) -endif() - -# Check for SSE2 support. -if(_SSE_TEST_20) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-msse2") - elseif(MSVC AND NOT CMAKE_CL_64) - set(CMAKE_REQUIRED_FLAGS "/arch:SSE2") - endif() - check_cxx_source_runs(" - #include - int main() - { - int a[4] = { 1, 2, 3, 4 }; - int b[4] = { 3, 6, -4, -4 }; - int c[4]; - - __m128i va = _mm_loadu_si128((__m128i*)a); - __m128i vb = _mm_loadu_si128((__m128i*)b); - __m128i vc = _mm_add_epi32(va, vb); - - _mm_storeu_si128((__m128i*)c, vc); - if (c[0] == 4 && c[1] == 8 && c[2] == -1 && c[3] == 0) - return 0; - else - return 1; - }" DETECTED_SSE_20) -endif() - -# Check for SSE support. -if(_SSE_TEST_10) - if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - set(CMAKE_REQUIRED_FLAGS "-msse") - elseif(MSVC AND NOT CMAKE_CL_64) - set(CMAKE_REQUIRED_FLAGS "/arch:SSE") - endif() - check_cxx_source_runs(" - #include - int main() - { - float a[4] = { 1.0f, 2.0f, 3.0f, 4.0f }; - float b[4] = { 2.0f, 3.0f, 4.0f, 5.0f }; - float c[4]; - __m128 va = _mm_loadu_ps(a); - __m128 vb = _mm_loadu_ps(b); - __m128 vc = _mm_add_ps(va, vb); - - _mm_storeu_ps(c, vc); - if (c[0] == 3.0f && c[1] == 5.0f && c[2] == 7.0f && c[3] == 9.0f) - return 0; - else - return 1; - }" DETECTED_SSE_10) -endif() - -set(CMAKE_REQUIRED_FLAGS) - -if(DETECTED_SSE_42) - set(SSE_VERSION "4.2") - set(SSE_STR "4_2") - set(SSE_FOUND 1) -elseif(DETECTED_SSE_41) - set(SSE_VERSION "4.1") - set(SSE_STR "4_1") - set(SSE_FOUND 1) -elseif(DETECTED_SSE_30) - set(SSE_VERSION "3.0") - set(SSE_STR "3_0") - set(SSE_FOUND 1) -elseif(DETECTED_SSE_20) - set(SSE_VERSION "2.0") - set(SSE_STR "2_0") - set(SSE_FOUND 1) -elseif(DETECTED_SSE_10) - set(SSE_VERSION "1.0") - set(SSE_STR "1_0") - set(SSE_FOUND 1) -endif() - - -if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(DETECTED_SSE_42) - set(SSE_FLAGS "${SSE_FLAGS} -msse4.2 -mfpmath=sse") - elseif(DETECTED_SSE_41) - set(SSE_FLAGS "${SSE_FLAGS} -msse4.1 -mfpmath=sse") - elseif(DETECTED_SSE_30) - set(SSE_FLAGS "${SSE_FLAGS} -msse3 -mfpmath=sse") - elseif(DETECTED_SSE_20) - set(SSE_FLAGS "${SSE_FLAGS} -msse2 -mfpmath=sse") - elseif(DETECTED_SSE_10) - set(SSE_FLAGS "${SSE_FLAGS} -msse -mfpmath=sse") - else() - # Setting -ffloat-store to alleviate 32bit vs 64bit discrepancies on non-SSE platforms. - set(SSE_FLAGS "-ffloat-store") - endif() -elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") - set(SSE_FLAGS "-xHost") -elseif(MSVC) - if(DETECTED_SSE_20) - set(SSE_FLAGS "${SSE_FLAGS} /arch:SSE2") - elseif(DETECTED_SSE_10) - set(SSE_FLAGS "${SSE_FLAGS} /arch:SSE") - endif() -endif() - -if(SSE_FOUND) - message(STATUS " Found SSE ${SSE_VERSION} extensions, using flags: ${SSE_FLAGS}") -else() - message(STATUS " No SSE support found") - set(SSE_FLAGS "") -endif() - -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${SSE_FLAGS}") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${SSE_FLAGS}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${SSE_FLAGS}") - -return() -#------------------------------------- - -# If no SSE support is found, print an error message. -if(SSE_FIND_VERSION) - set(_SSE_ERROR_MESSAGE "SSE ${SSE_FIND_VERSION} support is not found on this architecture") -else() - set(_SSE_ERROR_MESSAGE "SSE support is not found on this architecture") -endif() - -if(SSE_FIND_REQUIRED) - message(FATAL_ERROR "${_SSE_ERROR_MESSAGE}") -elseif(NOT SSE_FIND_QUIETLY) - message(STATUS "${_SSE_ERROR_MESSAGE}") -endif() From afb9062746d15f931a499adad413291f4438ec7a Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 18 Sep 2023 10:50:37 -0400 Subject: [PATCH 09/18] Do not improve mollified EE constraints for now --- src/ipc/collisions/collision_constraints.cpp | 17 +++++----- .../collision_constraints_builder.cpp | 33 ++++++++++++++----- tests/test_ipc.cpp | 21 +++++++++++- 3 files changed, 52 insertions(+), 19 deletions(-) diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 6495294d7..ccbbfb428 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -113,14 +113,14 @@ namespace { EdgeVertexCandidate, double, Eigen::SparseVector>> ev_candidates; for (const EdgeEdgeCandidate& ee : ee_candidates) { - if (edge_edge_distance_type( - 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))) - == EdgeEdgeDistanceType::EA_EB) { - continue; - } + // if (edge_edge_distance_type( + // 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))) + // == EdgeEdgeDistanceType::EA_EB) { + // continue; + // } for (int i = 0; i < 2; i++) { const int ei = i == 0 ? ee.edge0_id : ee.edge1_id; @@ -212,7 +212,6 @@ void CollisionConstraints::build( }); if (use_convergent_formulation()) { - if (candidates.ev_candidates.size() > 0) { // Convert edge-vertex to vertex-vertex const std::vector vv_candidates = diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 1a90871cd..e5d557aa0 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -403,17 +403,32 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( const int e0i = mesh.edges()(ei, 0), e1i = mesh.edges()(ei, 1); assert(vi != e0i && vi != e1i); - // TODO: distinguish mollified vs non-mollified + int nonmollified_incident_edge_amt = 0; + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vi]; - const int incident_edge_amt = incident_vertices.size() - - int(incident_vertices.find(mesh.edges()(e0i, 0)) - != incident_vertices.end()) - - int(incident_vertices.find(mesh.edges()(e1i, 1)) - != incident_vertices.end()); + for (const int vj : incident_vertices) { + if (vj == e0i || vj == e1i) { + continue; + } - if (incident_edge_amt > 1) { + const double eps_x = edge_edge_mollifier_threshold( + mesh.rest_positions().row(vi), mesh.rest_positions().row(vj), + mesh.rest_positions().row(e0i), mesh.rest_positions().row(e1i)); + + const double ee_cross_norm_sqr = edge_edge_cross_squarednorm( + vertices.row(vi), vertices.row(vj), vertices.row(e0i), + vertices.row(e1i)); + + if (ee_cross_norm_sqr < eps_x) { + // TODO: add EE mollified constraint with weight of -area_weight + } else { + nonmollified_incident_edge_amt++; + } + } + + if (nonmollified_incident_edge_amt > 1) { // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = (1 - incident_edge_amt) + const double weight = (1 - nonmollified_incident_edge_amt) * (use_convergent_formulation() ? ((mesh.edge_area(ei) + std::get<1>(candidates[i])) / 4) : 1); @@ -421,7 +436,7 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( Eigen::SparseVector weight_gradient; if (should_compute_weight_gradient() && use_convergent_formulation()) { - weight_gradient = (1 - incident_edge_amt) + weight_gradient = (1 - nonmollified_incident_edge_amt) * (mesh.edge_area_gradient(ei) + std::get<2>(candidates[i])) / 4; } diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index 297ea0b95..89cd13d4f 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -370,6 +370,24 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") CHECK(point_edge_distance(V.row(2), V.row(0), V.row(1)) < dhat * dhat); } + SECTION("3D Edge-Edge 2") + { + V.resize(5, 3); + // + V.row(0) << 0, 1e-4, -1e-4; + V.row(1) << 0, 1e-4, -1; + // + V.row(2) << 1e-4, 0, 0; + V.row(3) << -0.33, 0, 0; + V.row(4) << 0.5, 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); + } const CollisionMesh mesh(V, E, F); @@ -383,7 +401,8 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") const Eigen::VectorXd grad_b = collision_constraints.compute_potential_gradient(mesh, V, dhat); - const Eigen::MatrixXd force = -fd::unflatten(grad_b, V.cols()); + // const Eigen::MatrixXd force = -fd::unflatten(grad_b, V.cols()); + // std::cout << force << std::endl; if (use_convergent_formulation) { constexpr double eps = std::numeric_limits::epsilon(); From 0457f808179cfa0ffc4810f4c862be497c5989cb Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 18 Sep 2023 22:30:25 -0400 Subject: [PATCH 10/18] Print CCACHE_PROGRAM --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3515752f..f70473ffe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,7 @@ else() option(IPC_TOOLKIT_WITH_CCACHE "Enable ccache when building IPC Toolkit" OFF) endif() if(IPC_TOOLKIT_WITH_CCACHE AND CCACHE_PROGRAM) - message(STATUS "Enabling Ccache support") + message(STATUS "Enabling Ccache support (${CCACHE_PROGRAM})") set(ccacheEnv CCACHE_BASEDIR=${CMAKE_BINARY_DIR} CCACHE_SLOPPINESS=clang_index_store,include_file_ctime,include_file_mtime,locale,pch_defines,time_macros From ba94958651fcdf02643cdc4954349d5d26f43e17 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 21 Sep 2023 23:31:05 -0400 Subject: [PATCH 11/18] Fix EE improved max approx --- src/ipc/collisions/collision_constraints.cpp | 50 +++++------- .../collision_constraints_builder.cpp | 65 ++++++++-------- .../collision_constraints_builder.hpp | 13 ++-- tests/test_ipc.cpp | 76 ++++++++++--------- 4 files changed, 97 insertions(+), 107 deletions(-) diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index ccbbfb428..1f7b9dd8d 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -101,27 +101,14 @@ namespace { return ev_candidates; } - std::vector< - std::tuple>> - edge_edge_to_edge_vertex_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; + std::vector ev_candidates; for (const EdgeEdgeCandidate& ee : ee_candidates) { - // if (edge_edge_distance_type( - // 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))) - // == EdgeEdgeDistanceType::EA_EB) { - // continue; - // } - 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; @@ -134,17 +121,18 @@ namespace { if (is_active(point_edge_distance( vertices.row(vj), // vertices.row(ei0), vertices.row(ei1)))) { - Eigen::SparseVector weight_gradient; - if (mesh.are_area_jacobians_initialized()) - weight_gradient = mesh.edge_area_gradient(ei); - ev_candidates.emplace_back( - EdgeVertexCandidate(ei, vj), mesh.edge_area(ej), - weight_gradient); + 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 @@ -221,8 +209,9 @@ void CollisionConstraints::build( tbb::parallel_for( tbb::blocked_range(size_t(0), vv_candidates.size()), [&](const tbb::blocked_range& r) { - storage.local().add_vertex_vertex_negative_constraints( - mesh, vertices, vv_candidates, r.begin(), r.end()); + storage.local() + .add_edge_vertex_negative_vertex_vertex_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); }); } @@ -234,8 +223,9 @@ void CollisionConstraints::build( tbb::parallel_for( tbb::blocked_range(size_t(0), ev_candidates.size()), [&](const tbb::blocked_range& r) { - storage.local().add_edge_vertex_negative_constraints( - mesh, vertices, ev_candidates, r.begin(), r.end()); + storage.local() + .add_edge_edge_negative_edge_vertex_constraints( + mesh, vertices, ev_candidates, r.begin(), r.end()); }); } @@ -248,8 +238,9 @@ void CollisionConstraints::build( tbb::parallel_for( tbb::blocked_range(size_t(0), ev_candidates.size()), [&](const tbb::blocked_range& r) { - storage.local().add_edge_vertex_negative_constraints( - mesh, vertices, ev_candidates, r.begin(), r.end()); + storage.local() + .add_face_vertex_negative_edge_vertex_constraints( + mesh, vertices, ev_candidates, r.begin(), r.end()); }); // Convert face-vertex to vertex-vertex @@ -260,8 +251,9 @@ void CollisionConstraints::build( tbb::parallel_for( tbb::blocked_range(size_t(0), vv_candidates.size()), [&](const tbb::blocked_range& r) { - storage.local().add_vertex_vertex_positive_constraints( - mesh, vertices, vv_candidates, r.begin(), r.end()); + storage.local() + .add_face_vertex_positive_vertex_vertex_constraints( + mesh, vertices, vv_candidates, r.begin(), r.end()); }); } } diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index e5d557aa0..a708b567d 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -261,12 +261,13 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( // ============================================================================ -void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( - const CollisionMesh& mesh, - const Eigen::MatrixXd& vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) +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, @@ -308,12 +309,13 @@ void CollisionConstraintsBuilder::add_vertex_vertex_negative_constraints( } } -void CollisionConstraintsBuilder::add_vertex_vertex_positive_constraints( - const CollisionMesh& mesh, - const Eigen::MatrixXd& vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) +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, @@ -351,12 +353,13 @@ void CollisionConstraintsBuilder::add_vertex_vertex_positive_constraints( } } -void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( - const CollisionMesh& mesh, - const Eigen::MatrixXd& vertices, - const std::vector& candidates, - const size_t start_i, - const size_t end_i) +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]; @@ -389,17 +392,16 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( } } -void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( - const CollisionMesh& mesh, - const Eigen::MatrixXd& vertices, - const std::vector< - std::tuple>>& - candidates, - const size_t start_i, - const size_t end_i) +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) { for (size_t i = start_i; i < end_i; i++) { - const auto& [ei, vi] = std::get<0>(candidates[i]); + const auto& [ei, vi] = candidates[i]; const int e0i = mesh.edges()(ei, 0), e1i = mesh.edges()(ei, 1); assert(vi != e0i && vi != e1i); @@ -429,20 +431,17 @@ void CollisionConstraintsBuilder::add_edge_vertex_negative_constraints( if (nonmollified_incident_edge_amt > 1) { // ÷ 4 to handle double counting and PT + EE for correct integration const double weight = (1 - nonmollified_incident_edge_amt) - * (use_convergent_formulation() - ? ((mesh.edge_area(ei) + std::get<1>(candidates[i])) / 4) - : 1); + * (use_convergent_formulation() ? (mesh.edge_area(ei) / 4) : 1); Eigen::SparseVector weight_gradient; if (should_compute_weight_gradient() && use_convergent_formulation()) { weight_gradient = (1 - nonmollified_incident_edge_amt) - * (mesh.edge_area_gradient(ei) + std::get<2>(candidates[i])) - / 4; + * mesh.edge_area_gradient(ei) / 4; } add_edge_vertex_constraint( - mesh, std::get<0>(candidates[i]), + mesh, candidates[i], point_edge_distance_type( vertices.row(vi), vertices.row(mesh.edges()(ei, 0)), vertices.row(mesh.edges()(ei, 1))), diff --git a/src/ipc/collisions/collision_constraints_builder.hpp b/src/ipc/collisions/collision_constraints_builder.hpp index c4fd31c6e..3b4032c5a 100644 --- a/src/ipc/collisions/collision_constraints_builder.hpp +++ b/src/ipc/collisions/collision_constraints_builder.hpp @@ -42,34 +42,31 @@ class CollisionConstraintsBuilder { // ------------------------------------------------------------------------ // Duplicate removal functions - void add_vertex_vertex_negative_constraints( + 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_vertex_vertex_positive_constraints( + 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_edge_vertex_negative_constraints( + 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_vertex_negative_constraints( + void add_edge_edge_negative_edge_vertex_constraints( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices, - const std::vector>>& candidates, + const std::vector& candidates, const size_t start_i, const size_t end_i); diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index 89cd13d4f..293c777fb 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -352,42 +352,44 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") 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, 0.9; - // - V.row(2) << 1e-4, 0, 0; - V.row(3) << -0.33, 0, 0; - V.row(4) << 0.5, 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 2") - { - V.resize(5, 3); - // - V.row(0) << 0, 1e-4, -1e-4; - V.row(1) << 0, 1e-4, -1; - // - V.row(2) << 1e-4, 0, 0; - V.row(3) << -0.33, 0, 0; - V.row(4) << 0.5, 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") + // { + // V.resize(5, 3); + // // + // V.row(0) << 0, 1e-4, -1; + // V.row(1) << 0, 1e-4, 0.9; + // // + // V.row(2) << 1e-4, 0, 0; + // V.row(3) << -0.33, 0, 0; + // V.row(4) << 0.5, 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 2") + // { + // V.resize(5, 3); + // // + // V.row(0) << 0, 1e-4, -1e-4; + // V.row(1) << 0, 1e-4, -1; + // // + // V.row(2) << 1e-4, 0, 0; + // V.row(3) << -0.33, 0, 0; + // V.row(4) << 0.5, 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); + // } const CollisionMesh mesh(V, E, F); @@ -402,7 +404,7 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") collision_constraints.compute_potential_gradient(mesh, V, dhat); // const Eigen::MatrixXd force = -fd::unflatten(grad_b, V.cols()); - // std::cout << force << std::endl; + // std::cout << "force:\n" << force << std::endl; if (use_convergent_formulation) { constexpr double eps = std::numeric_limits::epsilon(); From 8f0f5340648056bfa27e27dbf828a70d86eafbdb Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 21 Sep 2023 23:53:43 -0400 Subject: [PATCH 12/18] add lines --- tests/test_ipc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index 293c777fb..3bfd24900 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -403,8 +403,8 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") 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; + 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(); From 8d756efc1b4d752ce593ab0d3674114288b3b8e9 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 21 Sep 2023 23:53:55 -0400 Subject: [PATCH 13/18] remove log --- tests/test_ipc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_ipc.cpp b/tests/test_ipc.cpp index 3bfd24900..293c777fb 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -403,8 +403,8 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") 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; + // 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(); From 219903a98a76df82c38eacc2f0d42d2abb08f7a3 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 22 Sep 2023 00:28:45 -0400 Subject: [PATCH 14/18] show ccache config --- .github/workflows/continuous.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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' From 9a191454dc1e0e8d7cfe0fc1ab7e46d638014b42 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 22 Sep 2023 17:29:02 -0400 Subject: [PATCH 15/18] Handle mollified EE in the improved max approx --- python/src/collisions/edge_edge.cpp | 11 +- src/ipc/collision_mesh.cpp | 3 + src/ipc/collision_mesh.hpp | 15 +- .../collision_constraints_builder.cpp | 130 ++++++++++++------ src/ipc/collisions/edge_edge.cpp | 108 ++++++--------- src/ipc/collisions/edge_edge.hpp | 21 ++- 6 files changed, 175 insertions(+), 113 deletions(-) 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 402fdf02d..2b77d591c 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -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()); diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 01a7e742e..809852b84 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -164,6 +164,16 @@ 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 { @@ -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_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index a708b567d..b3de52418 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -100,11 +100,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; @@ -116,11 +116,11 @@ 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. @@ -170,7 +170,8 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( break; case EdgeEdgeDistanceType::EA_EB: - constraints.ee_constraints.emplace_back(eai, ebi, eps_x); + constraints.ee_constraints.emplace_back( + eai, ebi, eps_x, actual_dtype); constraints.ee_constraints.back().weight = weight; constraints.ee_constraints.back().weight_gradient = weight_gradient; break; @@ -284,8 +285,8 @@ void CollisionConstraintsBuilder:: if (should_compute_weight_gradient() && use_convergent_formulation()) { - weight_gradient += (1 - incident_edge_amt) - * (mesh.vertex_area_gradient(vi) / 2); + weight_gradient += (1 - incident_edge_amt) / 2.0 + * mesh.vertex_area_gradient(vi); } } }; @@ -376,10 +377,11 @@ void CollisionConstraintsBuilder:: : 1); Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient() - && use_convergent_formulation()) { - weight_gradient = (1 - incident_triangle_amt) - * (mesh.vertex_area_gradient(vi) / 4); + 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( @@ -400,53 +402,95 @@ void CollisionConstraintsBuilder:: 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& [ei, vi] = candidates[i]; - const int e0i = mesh.edges()(ei, 0), e1i = mesh.edges()(ei, 1); - assert(vi != e0i && vi != e1i); + const auto& [ea, p] = candidates[i]; + const int ea0 = mesh.edges()(ea, 0), ea1 = mesh.edges()(ea, 1); + assert(p != ea0 && p != ea1); + + 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_vertices = mesh.vertex_vertex_adjacencies()[vi]; - for (const int vj : incident_vertices) { - if (vj == e0i || vj == e1i) { + 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 == eb1) { continue; } const double eps_x = edge_edge_mollifier_threshold( - mesh.rest_positions().row(vi), mesh.rest_positions().row(vj), - mesh.rest_positions().row(e0i), mesh.rest_positions().row(e1i)); + 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(vi), vertices.row(vj), vertices.row(e0i), - vertices.row(e1i)); + vertices.row(ea0), vertices.row(ea1), vertices.row(eb0), + vertices.row(eb1)); - if (ee_cross_norm_sqr < eps_x) { - // TODO: add EE mollified constraint with weight of -area_weight - } else { + if (ee_cross_norm_sqr >= eps_x) { nonmollified_incident_edge_amt++; + continue; } - } - if (nonmollified_incident_edge_amt > 1) { - // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = (1 - nonmollified_incident_edge_amt) - * (use_convergent_formulation() ? (mesh.edge_area(ei) / 4) : 1); + // Add mollified EE constraint with weight of -¼w and specified + // distance type + + // Convert the PE distance type to an EE distance type + EdgeEdgeDistanceType ee_dtype; + 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; + } - Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient() - && use_convergent_formulation()) { - weight_gradient = (1 - nonmollified_incident_edge_amt) - * mesh.edge_area_gradient(ei) / 4; + constraints.ee_constraints.emplace_back(ea, eb, eps_x, ee_dtype); + constraints.ee_constraints.back().weight = + use_convergent_formulation() ? (-0.25 * mesh.edge_area(ea)) + : -1; + if (should_compute_weight_gradient()) { + constraints.ee_constraints.back().weight_gradient = + use_convergent_formulation() + ? (-0.25 * mesh.edge_area_gradient(ea)) + : 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); + if (nonmollified_incident_edge_amt == 1) { + continue; // no constraint to add because (1 - ρ(x)) = 0 } + // if nonmollified_incident_edge_amt == 0, then we need to explicitly + // add a positive constraint. + + // ÷ 4 to handle double counting and PT + EE for correct integration + const double weight = (1 - nonmollified_incident_edge_amt) + * (use_convergent_formulation() ? (mesh.edge_area(ea) / 4) : 1); + + Eigen::SparseVector weight_gradient; + if (should_compute_weight_gradient()) { + weight_gradient = use_convergent_formulation() + ? ((1 - nonmollified_incident_edge_amt) / 4.0 + * mesh.edge_area_gradient(ea)) + : Eigen::SparseVector(vertices.size()); + } + + add_edge_vertex_constraint( + mesh, candidates[i], dtype, weight, weight_gradient); } } diff --git a/src/ipc/collisions/edge_edge.cpp b/src/ipc/collisions/edge_edge.cpp index 18075826c..e7c4fe6d6 100644 --- a/src/ipc/collisions/edge_edge.cpp +++ b/src/ipc/collisions/edge_edge.cpp @@ -7,16 +7,23 @@ namespace ipc { EdgeEdgeConstraint::EdgeEdgeConstraint( - long edge0_id, long edge1_id, double eps_x) + long edge0_id, + long edge1_id, + 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, + double eps_x, + const EdgeEdgeDistanceType dtype) : EdgeEdgeCandidate(candidate) , eps_x(eps_x) + , dtype(dtype) { } @@ -40,33 +47,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 +74,38 @@ 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); - } - - return weight * 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(); + + 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; } } // namespace ipc diff --git a/src/ipc/collisions/edge_edge.hpp b/src/ipc/collisions/edge_edge.hpp index f33cee74d..261ea67d4 100644 --- a/src/ipc/collisions/edge_edge.hpp +++ b/src/ipc/collisions/edge_edge.hpp @@ -9,8 +9,16 @@ 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( + long edge0_id, + long edge1_id, + double eps_x, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); + + EdgeEdgeConstraint( + const EdgeEdgeCandidate& candidate, + double eps_x, + const EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); double compute_potential( const Eigen::MatrixXd& vertices, @@ -38,7 +46,16 @@ class EdgeEdgeConstraint : public EdgeEdgeCandidate, std::move(h), static_cast(ee)); } + /// @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 From b57d7164b922cf9109f856b81013320ba2842aad Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 22 Sep 2023 17:37:36 -0400 Subject: [PATCH 16/18] Fix Python bindings --- python/src/bindings.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); From 9aa24643fea94bd110616c4bfdbb77a40f9831ee Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 24 Sep 2023 14:46:56 -0400 Subject: [PATCH 17/18] Remove duplicate EE constraints --- src/ipc/collisions/collision_constraint.cpp | 7 + src/ipc/collisions/collision_constraint.hpp | 6 + src/ipc/collisions/collision_constraints.cpp | 23 +- .../collision_constraints_builder.cpp | 204 +++++++++--------- .../collision_constraints_builder.hpp | 83 ++++--- src/ipc/collisions/edge_edge.cpp | 40 +++- src/ipc/collisions/edge_edge.hpp | 28 ++- src/ipc/collisions/edge_vertex.hpp | 10 + src/ipc/collisions/face_vertex.hpp | 10 + src/ipc/collisions/vertex_vertex.hpp | 10 + tests/test_ipc.cpp | 103 ++++----- 11 files changed, 318 insertions(+), 206 deletions(-) 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 1f7b9dd8d..fc5dd7fdf 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include @@ -173,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()), @@ -610,7 +610,7 @@ std::string CollisionConstraints::to_string( for (const auto& vv : vv_constraints) { ss << "\n" << fmt::format( - "vv: {} {}, w: {}, d: {}", vv.vertex0_id, vv.vertex1_id, + "vv: {} {}, w: {:g}, d: {:g}", vv.vertex0_id, vv.vertex1_id, vv.weight, point_point_distance( vertices.row(vv.vertex0_id), @@ -619,7 +619,7 @@ std::string CollisionConstraints::to_string( for (const auto& ev : ev_constraints) { ss << "\n" << fmt::format( - "ev: {}=({}, {}) {}, w: {}, d: {}", ev.edge_id, + "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( @@ -630,20 +630,21 @@ std::string CollisionConstraints::to_string( for (const auto& ee : ee_constraints) { ss << "\n" << fmt::format( - "ee: {}=({}, {}) {}=({}, {}), w: {}, d: {}", 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, - line_line_distance( + "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)))); + vertices.row(mesh.edges()(ee.edge1_id, 1)), ee.dtype)); } for (const auto& fv : fv_constraints) { ss << "\n" << fmt::format( - "fv: {}=({}, {}, {}) {}, w: {}, d: {}", fv.face_id, + "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( diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index b3de52418..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; } // ============================================================================ @@ -39,11 +40,11 @@ void CollisionConstraintsBuilder::add_edge_vertex_constraints( // ÷ 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()); } @@ -124,13 +125,13 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( // ÷ 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()); @@ -170,10 +171,9 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( break; case EdgeEdgeDistanceType::EA_EB: - constraints.ee_constraints.emplace_back( - eai, ebi, eps_x, actual_dtype); - 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: @@ -210,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()); } @@ -248,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: @@ -280,11 +278,9 @@ void CollisionConstraintsBuilder:: 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); + * (use_convergent_formulation ? (mesh.vertex_area(vi) / 2) : 1); - if (should_compute_weight_gradient() - && use_convergent_formulation()) { + if (should_compute_weight_gradient && use_convergent_formulation) { weight_gradient += (1 - incident_edge_amt) / 2.0 * mesh.vertex_area_gradient(vi); } @@ -297,7 +293,7 @@ void CollisionConstraintsBuilder:: double weight = 0; Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { + if (should_compute_weight_gradient) { weight_gradient = Eigen::SparseVector(vertices.size()); } @@ -328,9 +324,9 @@ void CollisionConstraintsBuilder:: } // ÷ 4 to handle double counting and PT + EE for correct integration. - weight += use_convergent_formulation() ? (mesh.vertex_area(vi) / 4) : 1; + weight += use_convergent_formulation ? (mesh.vertex_area(vi) / 4) : 1; - if (should_compute_weight_gradient() && use_convergent_formulation()) { + if (should_compute_weight_gradient && use_convergent_formulation) { weight_gradient += mesh.vertex_area_gradient(vi) / 4; } }; @@ -341,7 +337,7 @@ void CollisionConstraintsBuilder:: double weight = 0; Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { + if (should_compute_weight_gradient) { weight_gradient = Eigen::SparseVector(vertices.size()); } @@ -373,12 +369,11 @@ void CollisionConstraintsBuilder:: 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); + * (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 ? ((1 - incident_triangle_amt) / 4.0 * mesh.vertex_area_gradient(vi)) : Eigen::SparseVector(vertices.size()); @@ -409,6 +404,16 @@ void CollisionConstraintsBuilder:: 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)); @@ -419,7 +424,7 @@ void CollisionConstraintsBuilder:: 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 == eb1) { + if (q == ea0 || q == ea1) { continue; } @@ -436,11 +441,9 @@ void CollisionConstraintsBuilder:: continue; } - // Add mollified EE constraint with weight of -¼w and specified - // distance type - + // Add mollified EE constraint with specified distance type // Convert the PE distance type to an EE distance type - EdgeEdgeDistanceType ee_dtype; + EdgeEdgeDistanceType ee_dtype = EdgeEdgeDistanceType::AUTO; switch (dtype) { case PointEdgeDistanceType::P_E0: ee_dtype = p == eb0 ? EdgeEdgeDistanceType::EA0_EB0 @@ -459,86 +462,76 @@ void CollisionConstraintsBuilder:: break; } - constraints.ee_constraints.emplace_back(ea, eb, eps_x, ee_dtype); - constraints.ee_constraints.back().weight = - use_convergent_formulation() ? (-0.25 * mesh.edge_area(ea)) - : -1; - if (should_compute_weight_gradient()) { - constraints.ee_constraints.back().weight_gradient = - use_convergent_formulation() - ? (-0.25 * mesh.edge_area_gradient(ea)) - : Eigen::SparseVector(vertices.size()); - } + 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 (1 - ρ(x)) = 0 + continue; // no constraint to add because (ρ(x) - 1) = 0 } // if nonmollified_incident_edge_amt == 0, then we need to explicitly // add a positive constraint. - - // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = (1 - nonmollified_incident_edge_amt) - * (use_convergent_formulation() ? (mesh.edge_area(ea) / 4) : 1); - - Eigen::SparseVector weight_gradient; - if (should_compute_weight_gradient()) { - weight_gradient = use_convergent_formulation() - ? ((1 - nonmollified_incident_edge_amt) / 4.0 - * mesh.edge_area_gradient(ea)) - : Eigen::SparseVector(vertices.size()); - } - add_edge_vertex_constraint( - mesh, candidates[i], dtype, weight, weight_gradient); + 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 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); } } @@ -551,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; @@ -560,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); @@ -572,40 +566,36 @@ 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 : 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& 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& 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 @@ -622,6 +612,12 @@ void CollisionConstraintsBuilder::merge( 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 3b4032c5a..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, @@ -77,43 +77,41 @@ class CollisionConstraintsBuilder { 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); } void add_edge_vertex_constraint( @@ -123,20 +121,43 @@ class CollisionConstraintsBuilder { const double weight, const Eigen::SparseVector& weight_gradient); - bool use_convergent_formulation() const - { - return constraints.use_convergent_formulation(); - } + // ------------------------------------------------------------------------- + + static void add_edge_edge_constraint( + const EdgeEdgeConstraint& ee_constraint, + unordered_map& ee_to_id, + std::vector& ee_constraints); - bool should_compute_weight_gradient() const + 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 e7c4fe6d6..17ebab7d0 100644 --- a/src/ipc/collisions/edge_edge.cpp +++ b/src/ipc/collisions/edge_edge.cpp @@ -7,9 +7,9 @@ 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) @@ -19,7 +19,7 @@ EdgeEdgeConstraint::EdgeEdgeConstraint( EdgeEdgeConstraint::EdgeEdgeConstraint( const EdgeEdgeCandidate& candidate, - double eps_x, + const double eps_x, const EdgeEdgeDistanceType dtype) : EdgeEdgeCandidate(candidate) , eps_x(eps_x) @@ -27,6 +27,20 @@ EdgeEdgeConstraint::EdgeEdgeConstraint( { } +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) +{ +} + double EdgeEdgeConstraint::compute_potential( const Eigen::MatrixXd& vertices, const Eigen::MatrixXi& edges, @@ -108,4 +122,22 @@ MatrixMax12d EdgeEdgeConstraint::compute_potential_hessian( 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 261ea67d4..5e68d20f8 100644 --- a/src/ipc/collisions/edge_edge.hpp +++ b/src/ipc/collisions/edge_edge.hpp @@ -10,14 +10,22 @@ class EdgeEdgeConstraint : public EdgeEdgeCandidate, public CollisionConstraint { public: 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 = EdgeEdgeDistanceType::AUTO); EdgeEdgeConstraint( const EdgeEdgeCandidate& candidate, - double eps_x, + 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( @@ -39,13 +47,21 @@ 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; 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/tests/test_ipc.cpp b/tests/test_ipc.cpp index 293c777fb..1828c565a 100644 --- a/tests/test_ipc.cpp +++ b/tests/test_ipc.cpp @@ -352,44 +352,42 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") 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, 0.9; - // // - // V.row(2) << 1e-4, 0, 0; - // V.row(3) << -0.33, 0, 0; - // V.row(4) << 0.5, 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 2") - // { - // V.resize(5, 3); - // // - // V.row(0) << 0, 1e-4, -1e-4; - // V.row(1) << 0, 1e-4, -1; - // // - // V.row(2) << 1e-4, 0, 0; - // V.row(3) << -0.33, 0, 0; - // V.row(4) << 0.5, 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") + { + 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); @@ -406,21 +404,26 @@ TEST_CASE("Test convergent formulation", "[ipc][convergent]") // 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)); - // CHECK(grad_b(3 * V.cols()) == 0); - } else { - CHECK(grad_b(0) != 0); - CHECK(grad_b(2 * V.cols()) != 0); - // CHECK(grad_b(3 * V.cols()) != 0); - } + // 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) { - return collision_constraints.compute_potential( - mesh, fd::unflatten(x, V.cols()), dhat); + 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); From 0f0c1569e790d51012d0adddfb1ca973d6ed1f22 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 24 Sep 2023 20:14:15 -0400 Subject: [PATCH 18/18] Fix non-Abseil build --- src/ipc/utils/unordered_map_and_set.hpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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