frictional self-contact

Author: liminchen

self_contact/BarrierEnergy.py

@@ -62,7 +62,7 @@ def grad(x, n, o, bp, be, contact_area):
                 if d_sqr < dhat_sqr:
                     s = d_sqr / dhat_sqr
                     # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity:
-                    local_grad = contact_area[i] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.grad(x[xI], x[eI[0]], x[eI[1]])
+                    local_grad = contact_area[xI] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.grad(x[xI], x[eI[0]], x[eI[1]])
                     g[xI] += local_grad[0:2]
                     g[eI[0]] += local_grad[2:4]
                     g[eI[1]] += local_grad[4:6]
@@ -104,7 +104,7 @@ def hess(x, n, o, bp, be, contact_area):
                     d_sqr_grad = PE.grad(x[xI], x[eI[0]], x[eI[1]])
                     s = d_sqr / dhat_sqr
                     # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity:
-                    local_hess = contact_area[i] * dhat * utils.make_PD(kappa / (8 * d_sqr * d_sqr * dhat_sqr) * (d_sqr + dhat_sqr) * np.outer(d_sqr_grad, d_sqr_grad) \
+                    local_hess = contact_area[xI] * dhat * utils.make_PD(kappa / (8 * d_sqr * d_sqr * dhat_sqr) * (d_sqr + dhat_sqr) * np.outer(d_sqr_grad, d_sqr_grad) \
                         + (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.hess(x[xI], x[eI[0]], x[eI[1]]))
                     index = [xI, eI[0], eI[1]]
                     for nI in range(0, 3):
@@ -139,11 +139,26 @@ def init_step_size(x, n, o, bp, be, p):
                         alpha = toc
     return alpha
 
-def compute_mu_lambda(x, n, o, contact_area, mu):
+def compute_mu_lambda(x, n, o, bp, be, contact_area, mu):
+    # floor:
     mu_lambda = np.array([0.0] * len(x))
     for i in range(0, len(x)):
         d = n.dot(x[i] - o)
         if d < dhat:
             s = d / dhat
             mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d))
-    return mu_lambda
+    # self-contact
+    mu_lambda_self = []
+    dhat_sqr = dhat * dhat
+    for xI in bp:
+        for eI in be:
+            if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge
+                d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]])
+                if d_sqr < dhat_sqr:
+                    s = d_sqr / dhat_sqr
+                    # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity
+                    # also, lambda = -\partial b / \partial d = -(\partial b / \partial d^2) * (\partial d^2 / \partial d)
+                    mu_lam = mu * -contact_area[xI] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * 2 * math.sqrt(d_sqr)
+                    [n, r] = PE.tangent(x[xI], x[eI[0]], x[eI[1]]) # normal and closest point parameterization on the edge
+                    mu_lambda_self.append([xI, eI[0], eI[1], mu_lam, n, r])
+    return [mu_lambda, mu_lambda_self]

self_contact/FrictionEnergy.py

@@ -23,26 +23,46 @@ def f_hess_term(vbarnorm, epsv):
     else:
         return -1.0 / (epsv * epsv)
 
-def val(v, mu_lambda, hhat, n):
+def val(v, mu_lambda, mu_lambda_self, hhat, n):
     sum = 0.0
+    # floor:
     T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
     for i in range(0, len(v)):
         if mu_lambda[i] > 0:
             vbar = np.transpose(T).dot(v[i])
             sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat)
+    # self-contact:
+    for i in range(0, len(mu_lambda_self)):
+        [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i]
+        T = np.identity(2) - np.outer(n, n)
+        rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1])
+        vbar = np.transpose(T).dot(rel_v)
+        sum += mu_lam * f0(np.linalg.norm(vbar), epsv, hhat)
     return sum
 
-def grad(v, mu_lambda, hhat, n):
+def grad(v, mu_lambda, mu_lambda_self, hhat, n):
     g = np.array([[0.0, 0.0]] * len(v))
+    # floor:
     T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
     for i in range(0, len(v)):
         if mu_lambda[i] > 0:
             vbar = np.transpose(T).dot(v[i])
             g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar)
+    # self-contact:
+    for i in range(0, len(mu_lambda_self)):
+        [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i]
+        T = np.identity(2) - np.outer(n, n)
+        rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1])
+        vbar = np.transpose(T).dot(rel_v)
+        g_rel_v = mu_lam * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar)
+        g[xI] += g_rel_v
+        g[eI0] += g_rel_v * -(1 - r)
+        g[eI1] += g_rel_v * -r
     return g
 
-def hess(v, mu_lambda, hhat, n):
+def hess(v, mu_lambda, mu_lambda_self, hhat, n):
     IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
+    # floor:
     T = np.identity(2) - np.outer(n, n) # tangent of slope is constant
     for i in range(0, len(v)):
         if mu_lambda[i] > 0:
@@ -57,4 +77,24 @@ def hess(v, mu_lambda, hhat, n):
                     IJV[0].append(i * 2 + r)
                     IJV[1].append(i * 2 + c)
                     IJV[2] = np.append(IJV[2], local_hess[r, c])
+    # self-contact:
+    for i in range(0, len(mu_lambda_self)):
+        [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i]
+        T = np.identity(2) - np.outer(n, n)
+        rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1])
+        vbar = np.transpose(T).dot(rel_v)
+        vbarnorm = np.linalg.norm(vbar)
+        inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2)
+        if vbarnorm != 0:
+            inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar)
+        hess_rel_v = mu_lam * T.dot(utils.make_PD(inner_term)).dot(np.transpose(T)) / hhat
+        index = [xI, eI0, eI1]
+        d_rel_v_dv = [1, -(1 - r), -r]
+        for nI in range(0, 3):
+            for nJ in range(0, 3):
+                for c in range(0, 2):
+                    for r in range(0, 2):
+                        IJV[0].append(index[nI] * 2 + r)
+                        IJV[1].append(index[nJ] * 2 + c)
+                        IJV[2] = np.append(IJV[2], d_rel_v_dv[nI] * d_rel_v_dv[nJ] * hess_rel_v[r, c])
     return IJV

self_contact/distance/PointEdgeDistance.py

@@ -45,4 +45,16 @@ def hess(p, e0, e1):
             np.reshape([H_PP[2, 0:2], np.array([0.0, 0.0]), H_PP[2, 2:4]], (1, 6))[0], \
             np.reshape([H_PP[3, 0:2], np.array([0.0, 0.0]), H_PP[3, 2:4]], (1, 6))[0]])
     else:            # point(p)-line(e0e1) expression
-        return PL.hess(p, e0, e1)
+        return PL.hess(p, e0, e1)
+
+# compute normal and the parameterization of the closest point on the edge
+def tangent(p, e0, e1):
+    e = e1 - e0
+    ratio = np.dot(e, p - e0) / np.dot(e, e)
+    if ratio < 0:    # point(p)-point(e0) expression
+        n = p - e0
+    elif ratio > 1:  # point(p)-point(e1) expression
+        n = p - e1
+    else:            # point(p)-line(e0e1) expression
+        n = p - ((1 - ratio) * e0 + ratio * e1)
+    return [n / np.linalg.norm(n), ratio]

self_contact/time_integrator.py

@@ -16,7 +16,7 @@
 def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, h, tol):
     x_tilde = x + v * h     # implicit Euler predictive position
     x_n = copy.deepcopy(x)
-    mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, contact_area, mu)  # compute mu * lambda for each node using x^n
+    [mu_lambda, mu_lambda_self] = BarrierEnergy.compute_mu_lambda(x, n, o, bp, be, contact_area, mu)  # compute mu * lambda for each node using x^n
     DBC_target = [] # target position of each DBC in the current time step
     for i in range(0, len(DBC)):
         if (DBC_limit[i] - x_n[DBC[i]]).dot(DBC_v[i]) > 0:
@@ -27,52 +27,52 @@ def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area,
 
     # Newton loop
     iter = 0
-    E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff, h)
-    [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h)
+    E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h)
+    [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff, tol, h)
     while (LA.norm(p, inf) / h > tol) | (sum(DBC_satisfied) != len(DBC)):   # also check whether all DBCs are satisfied
         print('Iteration', iter, ':')
         print('residual =', LA.norm(p, inf) / h)
 
         if (LA.norm(p, inf) / h <= tol) & (sum(DBC_satisfied) != len(DBC)):
             # increase DBC stiffness and recompute energy value record
             DBC_stiff *= 2
-            E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff, h)
+            E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h)
 
         # filter line search
         alpha = min(BarrierEnergy.init_step_size(x, n, o, bp, be, p), NeoHookeanEnergy.init_step_size(x, e, p))  # avoid interpenetration, tunneling, and inversion
-        while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff, h) > E_last:
+        while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x + alpha * p - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h) > E_last:
             alpha /= 2
         print('step size =', alpha)
 
         x += alpha * p
-        E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff, h)
-        [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h)
+        E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h)
+        [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff, tol, h)
         iter += 1
 
     v = (x - x_n) / h   # implicit Euler velocity update
     return [x, v]
 
-def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h):
+def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h):
     return InertiaEnergy.val(x, x_tilde, m) + h * h * (     # implicit Euler
         NeoHookeanEnergy.val(x, e, vol, IB, mu_lame, lam) + 
         GravityEnergy.val(x, m) + 
         BarrierEnergy.val(x, n, o, bp, be, contact_area) + 
-        FrictionEnergy.val(v, mu_lambda, h, n)
+        FrictionEnergy.val(v, mu_lambda, mu_lambda_self, h, n)
     ) + SpringEnergy.val(x, m, DBC, DBC_target, DBC_stiff)
 
-def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h):
+def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h):
     return InertiaEnergy.grad(x, x_tilde, m) + h * h * (    # implicit Euler
         NeoHookeanEnergy.grad(x, e, vol, IB, mu_lame, lam) + 
         GravityEnergy.grad(x, m) + 
         BarrierEnergy.grad(x, n, o, bp, be, contact_area) + 
-        FrictionEnergy.grad(v, mu_lambda, h, n)
+        FrictionEnergy.grad(v, mu_lambda, mu_lambda_self, h, n)
     ) + SpringEnergy.grad(x, m, DBC, DBC_target, DBC_stiff)
 
-def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h):
+def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h):
     IJV_In = InertiaEnergy.hess(x, x_tilde, m)
     IJV_MS = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam)
     IJV_B = BarrierEnergy.hess(x, n, o, bp, be, contact_area)
-    IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n)
+    IJV_F = FrictionEnergy.hess(v, mu_lambda, mu_lambda_self, h, n)
     IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff)
     IJV_MS[2] *= h * h    # implicit Euler
     IJV_B[2] *= h * h     # implicit Euler
@@ -84,9 +84,9 @@ def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area,
     H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr()
     return H
 
-def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h):
-    projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h)
-    reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1)
+def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff, tol, h):
+    projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h)
+    reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1)
     # check whether each DBC is satisfied
     DBC_satisfied = [False] * len(x)
     for i in range(0, len(DBC)):