diff --git a/CHANGELOG.md b/CHANGELOG.md index 7e866b214..f417e3f3e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,10 +10,15 @@ All notable changes to this project will be documented in this file. #### Fixed --> +### 2021-02-01 +#### Added +* Added minimum seperation distance (thickness) to distance constraints + * Based on [Codimensional Incremental Potential Contact [Li et al. 2020]](https://arxiv.org/abs/2012.04457) + ### 2021-02-01 ([a395175](https://github.com/ipc-sim/ipc-toolkit/commit/a3951750ca5f167ab1d546ae1dadd87d0a9e2497)) #### Added * Added 2D friction model based on the 3D formulation. - * TODO: tTest this further + * TODO: Test this further ### 2021-01-12 ([deee6d0](https://github.com/ipc-sim/ipc-toolkit/commit/deee6d0f9802910c5565f800492f9a995e65cf7e)) #### Added diff --git a/src/barrier/adaptive_stiffness.cpp b/src/barrier/adaptive_stiffness.cpp index d2cc8cee8..53bdb8cdc 100644 --- a/src/barrier/adaptive_stiffness.cpp +++ b/src/barrier/adaptive_stiffness.cpp @@ -21,20 +21,24 @@ double initial_barrier_stiffness( const Eigen::VectorXd& grad_energy, const Eigen::VectorXd& grad_barrier, double& max_barrier_stiffness, - double min_barrier_stiffness_scale) + double min_barrier_stiffness_scale, + double dmin) { assert(average_mass > 0 && min_barrier_stiffness_scale > 0); assert(bbox_diagonal > 0); double dhat_squared = dhat * dhat; + double dmin_squared = dmin * dmin; // Find a good initial value for κ - double d0 = 1e-8 * bbox_diagonal; + double d0 = 1e-8 * bbox_diagonal + dmin; d0 *= d0; - if (d0 >= dhat_squared) { - d0 = 0.5 * dhat_squared; // TODO: this is untested + if (d0 - dmin_squared >= 2 * dmin * dhat + dhat_squared) { + d0 = dmin * dhat + 0.5 * dhat_squared; // TODO: this is untested } - double min_barrier_stiffness = barrier_hessian(d0, dhat_squared) * 4 * d0; + double min_barrier_stiffness = + barrier_hessian(d0 - dmin_squared, 2 * dmin * dhat + dhat_squared) * 4 + * d0; min_barrier_stiffness = min_barrier_stiffness_scale * average_mass / min_barrier_stiffness; assert(std::isfinite(min_barrier_stiffness)); @@ -62,18 +66,19 @@ double initial_barrier_stiffness( double average_mass, const Eigen::VectorXd& grad_energy, double& max_barrier_stiffness, - double min_barrier_stiffness_scale) + double min_barrier_stiffness_scale, + double dmin) { double diag = world_bbox_diagonal(V); Constraints constraint_set; - construct_constraint_set(V_rest, V, E, F, dhat, constraint_set); + construct_constraint_set(V_rest, V, E, F, dhat, constraint_set, dmin); Eigen::VectorXd grad_barrier = compute_barrier_potential_gradient(V, E, F, constraint_set, dhat); return initial_barrier_stiffness( diag, dhat, average_mass, grad_energy, grad_barrier, - max_barrier_stiffness, min_barrier_stiffness_scale); + max_barrier_stiffness, min_barrier_stiffness_scale, dmin); } // Adaptive κ @@ -83,10 +88,12 @@ void update_barrier_stiffness( double max_barrier_stiffness, double& barrier_stiffness, double dhat_epsilon_scale, - double bbox_diagonal) + double bbox_diagonal, + double dmin) { // Is the barrier having a difficulty pushing the bodies apart? - double dhat_epsilon = dhat_epsilon_scale * bbox_diagonal; + double dhat_epsilon = dhat_epsilon_scale * (bbox_diagonal + dmin); + dhat_epsilon *= dhat_epsilon; if (prev_min_distance < dhat_epsilon && min_distance < dhat_epsilon && min_distance < prev_min_distance) { // Then increase the barrier stiffness. @@ -105,17 +112,18 @@ void update_barrier_stiffness( double& min_distance, double max_barrier_stiffness, double& barrier_stiffness, - double dhat_epsilon_scale) + double dhat_epsilon_scale, + double dmin) { Constraints constraint_set; - construct_constraint_set(/*V_rest=*/V, V, E, F, dhat, constraint_set); + construct_constraint_set(/*V_rest=*/V, V, E, F, dhat, constraint_set, dmin); // Use a temporay variable in case &prev_min_distance == &min_distance double current_min_distance = compute_minimum_distance(V, E, F, constraint_set); return update_barrier_stiffness( prev_min_distance, current_min_distance, max_barrier_stiffness, - barrier_stiffness, dhat_epsilon_scale, world_bbox_diagonal(V)); + barrier_stiffness, dhat_epsilon_scale, world_bbox_diagonal(V), dmin); min_distance = current_min_distance; } diff --git a/src/barrier/adaptive_stiffness.hpp b/src/barrier/adaptive_stiffness.hpp index 13be16e2a..48016a2f0 100644 --- a/src/barrier/adaptive_stiffness.hpp +++ b/src/barrier/adaptive_stiffness.hpp @@ -14,7 +14,8 @@ double initial_barrier_stiffness( const Eigen::VectorXd& grad_energy, const Eigen::VectorXd& grad_barrier, double& max_barrier_stiffness, - double min_barrier_stiffness_scale = 1e11); + double min_barrier_stiffness_scale = 1e11, + double dmin = 0); /// Compute an inital barrier stiffness using the mesh to compute the barrier /// potential gradient. @@ -27,7 +28,8 @@ double initial_barrier_stiffness( double average_mass, const Eigen::VectorXd& grad_energy, double& max_barrier_stiffness, - double min_barrier_stiffness_scale = 1e11); + double min_barrier_stiffness_scale = 1e11, + double dmin = 0); /// Update the barrier stiffness if the distance is decreasing and less than /// dhat_epsilon_scale * diag. @@ -37,7 +39,8 @@ void update_barrier_stiffness( double max_barrier_stiffness, double& barrier_stiffness, double dhat_epsilon_scale, - double bbox_diagonal); + double bbox_diagonal, + double dmin = 0); /// Update the barrier stiffness if the distance is decreasing and less than /// dhat_epsilon_scale * diag. @@ -50,6 +53,7 @@ void update_barrier_stiffness( double& min_distance, double max_barrier_stiffness, double& barrier_stiffness, - double dhat_epsilon_scale); + double dhat_epsilon_scale, + double dmin = 0); } // namespace ipc diff --git a/src/collision_constraint.cpp b/src/collision_constraint.cpp index ec239951c..472476b83 100644 --- a/src/collision_constraint.cpp +++ b/src/collision_constraint.cpp @@ -29,7 +29,10 @@ double VertexVertexConstraint::compute_potential( double dhat_squared = dhat * dhat; double distance_sqr = point_point_distance(V.row(vertex0_index), V.row(vertex1_index)); - return multiplicity * barrier(distance_sqr, dhat_squared); + return multiplicity + * barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); } Eigen::VectorX12d VertexVertexConstraint::compute_potential_gradient( @@ -48,7 +51,9 @@ Eigen::VectorX12d VertexVertexConstraint::compute_potential_gradient( point_point_distance_gradient(p0, p1, local_grad); double distance_sqr = point_point_distance(p0, p1); - local_grad *= barrier_gradient(distance_sqr, dhat_squared); + local_grad *= barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); return multiplicity * local_grad; } @@ -73,9 +78,13 @@ Eigen::MatrixXX12d VertexVertexConstraint::compute_potential_hessian( Eigen::MatrixXX6d local_hess; point_point_distance_hessian(p0, p1, local_hess); - local_hess *= barrier_gradient(distance_sqr, dhat_squared); - local_hess += barrier_hessian(distance_sqr, dhat_squared) * local_grad - * local_grad.transpose(); + local_hess *= barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); + local_hess += barrier_hessian( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared) + * local_grad * local_grad.transpose(); local_hess *= multiplicity; @@ -109,7 +118,10 @@ double EdgeVertexConstraint::compute_potential( double distance_sqr = point_edge_distance( V.row(vertex_index), V.row(E(edge_index, 0)), V.row(E(edge_index, 1)), PointEdgeDistanceType::P_E); - return multiplicity * barrier(distance_sqr, dhat_squared); + return multiplicity + * barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); } Eigen::VectorX12d EdgeVertexConstraint::compute_potential_gradient( @@ -131,7 +143,9 @@ Eigen::VectorX12d EdgeVertexConstraint::compute_potential_gradient( double distance_sqr = point_edge_distance(p, e0, e1, PointEdgeDistanceType::P_E); - local_grad *= barrier_gradient(distance_sqr, dhat_squared); + local_grad *= barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); return multiplicity * local_grad; } @@ -160,9 +174,13 @@ Eigen::MatrixXX12d EdgeVertexConstraint::compute_potential_hessian( point_edge_distance_hessian( p, e0, e1, PointEdgeDistanceType::P_E, local_hess); - local_hess *= barrier_gradient(distance_sqr, dhat_squared); - local_hess += barrier_hessian(distance_sqr, dhat_squared) * local_grad - * local_grad.transpose(); + local_hess *= barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); + local_hess += barrier_hessian( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared) + * local_grad * local_grad.transpose(); local_hess *= multiplicity; @@ -206,7 +224,9 @@ double EdgeEdgeConstraint::compute_potential( // constraints where also added as EE constraints. double distance_sqr = edge_edge_distance(ea0, ea1, eb0, eb1); return edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x) - * barrier(distance_sqr, dhat_squared); + * barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); } Eigen::VectorX12d EdgeEdgeConstraint::compute_potential_gradient( @@ -235,8 +255,14 @@ Eigen::VectorX12d EdgeEdgeConstraint::compute_potential_gradient( edge_edge_mollifier_gradient( ea0, ea1, eb0, eb1, eps_x, local_mollifier_grad); - return local_mollifier_grad * barrier(distance_sqr, dhat_squared) - + mollifier * barrier_gradient(distance_sqr, dhat_squared) + return local_mollifier_grad + * barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared) + + mollifier + * barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared) * local_distance_grad; } @@ -277,9 +303,15 @@ Eigen::MatrixXX12d EdgeEdgeConstraint::compute_potential_hessian( edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x, mollifier_hess); // Compute_barrier_derivatives - double b = barrier(distance_sqr, dhat_squared); - double grad_b = barrier_gradient(distance_sqr, dhat_squared); - double hess_b = barrier_hessian(distance_sqr, dhat_squared); + double b = barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); + double grad_b = barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); + double hess_b = barrier_hessian( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); Eigen::MatrixXX12d local_hess = mollifier_hess * b + grad_b @@ -324,7 +356,9 @@ double FaceVertexConstraint::compute_potential( // The distance type is known because of construct_constraint_set() double distance_sqr = point_triangle_distance(p, t0, t1, t2, PointTriangleDistanceType::P_T); - return barrier(distance_sqr, dhat_squared); + return barrier( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); } Eigen::VectorX12d FaceVertexConstraint::compute_potential_gradient( @@ -348,7 +382,10 @@ Eigen::VectorX12d FaceVertexConstraint::compute_potential_gradient( double distance_sqr = point_triangle_distance(p, t0, t1, t2, PointTriangleDistanceType::P_T); - return local_grad * barrier_gradient(distance_sqr, dhat_squared); + return local_grad + * barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); } Eigen::MatrixXX12d FaceVertexConstraint::compute_potential_hessian( @@ -376,9 +413,13 @@ Eigen::MatrixXX12d FaceVertexConstraint::compute_potential_hessian( point_triangle_distance_hessian( p, t0, t1, t2, PointTriangleDistanceType::P_T, local_hess); - local_hess *= barrier_gradient(distance_sqr, dhat_squared); - local_hess += barrier_hessian(distance_sqr, dhat_squared) * local_grad - * local_grad.transpose(); + local_hess *= barrier_gradient( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared); + local_hess += barrier_hessian( + distance_sqr - minimum_distance * minimum_distance, + 2 * minimum_distance * dhat + dhat_squared) + * local_grad * local_grad.transpose(); if (project_to_psd) { local_hess = Eigen::project_to_psd(local_hess); diff --git a/src/collision_constraint.hpp b/src/collision_constraint.hpp index af9b90998..e9c9bf5db 100644 --- a/src/collision_constraint.hpp +++ b/src/collision_constraint.hpp @@ -31,6 +31,8 @@ struct CollisionConstraint { const Eigen::MatrixXi& F, const double dhat, const bool project_to_psd) const = 0; + + double minimum_distance = 0; }; /////////////////////////////////////////////////////////////////////////////// diff --git a/src/friction/friction.cpp b/src/friction/friction.cpp index 145df0017..a5ecffa68 100644 --- a/src/friction/friction.cpp +++ b/src/friction/friction.cpp @@ -16,9 +16,13 @@ namespace ipc { double compute_normal_force_magnitude( - double distance_squared, double dhat_squared, double barrier_stiffness) + double distance_squared, + double dhat, + double barrier_stiffness, + double dmin = 0) { - double grad_b = barrier_gradient(distance_squared, dhat_squared); + double grad_b = barrier_gradient( + distance_squared - dmin * dmin, 2 * dmin * dhat + dhat * dhat); grad_b *= barrier_stiffness; return -grad_b * 2 * sqrt(distance_squared); // / (h * h) eliminated here } @@ -33,8 +37,6 @@ void construct_friction_constraint_set( double mu, FrictionConstraints& friction_constraint_set) { - double dhat_squared = dhat * dhat; - friction_constraint_set.vv_constraints.reserve( contact_constraint_set.vv_constraints.size()); for (const auto& vv_constraint : contact_constraint_set.vv_constraints) { @@ -47,7 +49,8 @@ void construct_friction_constraint_set( point_point_tangent_basis(p0, p1); friction_constraint_set.vv_constraints.back().normal_force_magnitude = compute_normal_force_magnitude( - point_point_distance(p0, p1), dhat_squared, barrier_stiffness); + point_point_distance(p0, p1), dhat, barrier_stiffness, + vv_constraint.minimum_distance); friction_constraint_set.vv_constraints.back().mu = mu; } @@ -68,7 +71,7 @@ void construct_friction_constraint_set( friction_constraint_set.ev_constraints.back().normal_force_magnitude = compute_normal_force_magnitude( point_edge_distance(p, e0, e1, PointEdgeDistanceType::P_E), - dhat_squared, barrier_stiffness); + dhat, barrier_stiffness, ev_constraint.minimum_distance); friction_constraint_set.ev_constraints.back().mu = mu; } @@ -98,7 +101,7 @@ void construct_friction_constraint_set( // skipped above. edge_edge_distance( ea0, ea1, eb0, eb1, EdgeEdgeDistanceType::EA_EB), - dhat_squared, barrier_stiffness); + dhat, barrier_stiffness, ee_constraint.minimum_distance); friction_constraint_set.ee_constraints.back().mu = mu; } @@ -120,7 +123,7 @@ void construct_friction_constraint_set( compute_normal_force_magnitude( point_triangle_distance( p, t0, t1, t2, PointTriangleDistanceType::P_T), - dhat_squared, barrier_stiffness); + dhat, barrier_stiffness, fv_constraint.minimum_distance); friction_constraint_set.fv_constraints.back().mu = mu; } } diff --git a/src/ipc.cpp b/src/ipc.cpp index b43debdd3..934fb685f 100644 --- a/src/ipc.cpp +++ b/src/ipc.cpp @@ -71,11 +71,14 @@ void construct_constraint_set( Constraints& constraint_set, bool ignore_codimensional_vertices, const Eigen::VectorXi& vertex_group_ids, - const Eigen::MatrixXi& F2E) + const Eigen::MatrixXi& F2E, + double dmin) { + double inflation_radius = (dhat + dmin) / 2; + Candidates candidates; HashGrid hash_grid; - hash_grid.resize(V, V, E, /*inflation_radius=*/dhat); + hash_grid.resize(V, V, E, inflation_radius); // Assumes the edges connect to all boundary vertices if (ignore_codimensional_vertices) { @@ -83,18 +86,20 @@ void construct_constraint_set( const int e0 = E(e, 0); const int e1 = E(e, 1); hash_grid.addVertex( - V.row(e0), V.row(e0), e0, /*inflation_radius=*/dhat); + V.row(e0), V.row(e0), e0, + /*inflation_radius=*/dhat + dmin); hash_grid.addVertex( - V.row(e1), V.row(e1), e1, /*inflation_radius=*/dhat); + V.row(e1), V.row(e1), e1, + /*inflation_radius=*/dhat + dmin); } } else { - hash_grid.addVertices(V, V, /*inflation_radius=*/dhat); + hash_grid.addVertices(V, V, inflation_radius); } - hash_grid.addEdges(V, V, E, /*inflation_radius=*/dhat); + hash_grid.addEdges(V, V, E, inflation_radius); if (V.cols() == 3) { // These are not needed for 2D - hash_grid.addFaces(V, V, F, /*inflation_radius=*/dhat); + hash_grid.addFaces(V, V, F, inflation_radius); } if (V.cols() == 2) { @@ -110,7 +115,7 @@ void construct_constraint_set( } construct_constraint_set( - candidates, V_rest, V, E, F, dhat, constraint_set, F2E); + candidates, V_rest, V, E, F, dhat, constraint_set, F2E, dmin); } void construct_constraint_set( @@ -121,9 +126,11 @@ void construct_constraint_set( const Eigen::MatrixXi& F, double dhat, Constraints& constraint_set, - const Eigen::MatrixXi& F2E) + const Eigen::MatrixXi& F2E, + double dmin) { double dhat_squared = dhat * dhat; + double dmin_squared = dmin * dmin; // Cull the candidates by measuring the distance and dropping those that are // greater than dhat. @@ -159,7 +166,7 @@ void construct_constraint_set( double distance_sqr = point_edge_distance(V.row(vi), V.row(e0i), V.row(e1i), dtype); - if (distance_sqr < dhat_squared) { + if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) { switch (dtype) { case PointEdgeDistanceType::P_E0: add_vertex_vertex_constraint( @@ -193,7 +200,7 @@ void construct_constraint_set( double distance_sqr = edge_edge_distance( V.row(ea0i), V.row(ea1i), V.row(eb0i), V.row(eb1i), dtype); - if (distance_sqr < dhat_squared) { + if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) { double eps_x = edge_edge_mollifier_threshold( V_rest.row(ea0i), V_rest.row(ea1i), // V_rest.row(eb0i), V_rest.row(eb1i)); @@ -269,7 +276,7 @@ void construct_constraint_set( V.row(fv_candidate.vertex_index), // V.row(f0i), V.row(f1i), V.row(f2i), dtype); - if (distance_sqr < dhat_squared) { + if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) { switch (dtype) { case PointTriangleDistanceType::P_T0: add_vertex_vertex_constraint( @@ -310,6 +317,10 @@ void construct_constraint_set( } } } + + for (int ci = 0; ci < constraint_set.size(); ci++) { + constraint_set[ci].minimum_distance = dmin; + } } /////////////////////////////////////////////////////////////////////////////// @@ -335,8 +346,6 @@ Eigen::VectorXd compute_barrier_potential_gradient( const Constraints& constraint_set, double dhat) { - double dhat_squared = dhat * dhat; - Eigen::VectorXd grad = Eigen::VectorXd::Zero(V.size()); int dim = V.cols(); @@ -357,7 +366,6 @@ Eigen::SparseMatrix compute_barrier_potential_hessian( double dhat, bool project_to_psd) { - double dhat_squared = dhat * dhat; int dim = V.cols(); int dim_sq = dim * dim; diff --git a/src/ipc.hpp b/src/ipc.hpp index f8f05ce45..254522ed5 100644 --- a/src/ipc.hpp +++ b/src/ipc.hpp @@ -35,7 +35,8 @@ void construct_constraint_set( Constraints& constraint_set, bool ignore_codimensional_vertices = true, const Eigen::VectorXi& vertex_group_ids = Eigen::VectorXi(), - const Eigen::MatrixXi& F2E = Eigen::MatrixXi()); + const Eigen::MatrixXi& F2E = Eigen::MatrixXi(), + double dmin = 0); /// @brief Construct a set of constraints used to compute the barrier potential. /// @@ -57,7 +58,8 @@ void construct_constraint_set( const Eigen::MatrixXi& F, double dhat, Constraints& constraint_set, - const Eigen::MatrixXi& F2E = Eigen::MatrixXi()); + const Eigen::MatrixXi& F2E = Eigen::MatrixXi(), + double dmin = 0); /// @brief Compute the barrier potential for a given constraint set. ///