Added minimum seperation distance (thickness) to distance constraints

CHANGELOG.md

@@ -10,10 +10,15 @@ All notable changes to this project will be documented in this file.
 #### Fixed
 -->
 
+### 2021-02-01 <!--([XXXXXXX](https://github.com/ipc-sim/ipc-toolkit/commit/XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX))-->
+#### 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

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;
 }

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

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);

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;
 };
 
 ///////////////////////////////////////////////////////////////////////////////

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;
     }
 }