self-contact (fric wip)

Author: liminchen

self_contact/BarrierEnergy.py

@@ -1,10 +1,15 @@
 import math
 import numpy as np
 
+import distance.PointEdgeDistance as PE
+import distance.CCD as CCD
+
+import utils
+
 dhat = 0.01
 kappa = 1e5
 
-def val(x, n, o, contact_area):
+def val(x, n, o, bp, be, contact_area):
     sum = 0.0
     # floor:
     for i in range(0, len(x)):
@@ -19,9 +24,19 @@ def val(x, n, o, contact_area):
         if d < dhat:
             s = d / dhat
             sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
+    # self-contact
+    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:
+                    sum += contact_area[xI] * dhat * kappa / 8 * (s - 1) * math.log(s)
     return sum
 
-def grad(x, n, o, contact_area):
+def grad(x, n, o, bp, be, contact_area):
     g = np.array([[0.0, 0.0]] * len(x))
     # floor:
     for i in range(0, len(x)):
@@ -38,9 +53,22 @@ def grad(x, n, o, contact_area):
             local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n
             g[i] += local_grad
             g[-1] -= local_grad
+    # self-contact
+    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:
+                    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]])
+                    g[xI] += local_grad[0:2]
+                    g[eI[0]] += local_grad[2:4]
+                    g[eI[1]] += local_grad[4:6]
     return g
 
-def hess(x, n, o, contact_area):
+def hess(x, n, o, bp, be, contact_area):
     IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
     # floor:
     for i in range(0, len(x)):
@@ -66,9 +94,29 @@ def hess(x, n, o, contact_area):
                             IJV[0].append(index[nI] * 2 + r)
                             IJV[1].append(index[nJ] * 2 + c)
                             IJV[2] = np.append(IJV[2], ((-1) ** (nI != nJ)) * local_hess[r, c])
+    # self-contact
+    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:
+                    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) \
+                        + (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):
+                        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], local_hess[nI * 2 + r, nJ * 2 + c])
     return IJV
 
-def init_step_size(x, n, o, p):
+def init_step_size(x, n, o, bp, be, p):
     alpha = 1
     # floor:
     for i in range(0, len(x)):
@@ -81,6 +129,14 @@ def init_step_size(x, n, o, p):
         p_n = (p[i] - p[-1]).dot(n)
         if p_n < 0:
             alpha = min(alpha, 0.9 * n.dot(x[i] - x[-1]) / -p_n)
+    # self-contact
+    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
+                if CCD.bbox_overlap(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha):
+                    toc = CCD.narrow_phase_CCD(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha)
+                    if alpha > toc:
+                        alpha = toc
     return alpha
 
 def compute_mu_lambda(x, n, o, contact_area, mu):

self_contact/distance/CCD.py

@@ -0,0 +1,59 @@
+from copy import deepcopy
+import numpy as np
+import math
+
+import distance.PointEdgeDistance as PE
+
+# check whether the bounding box of the trajectory of the point and the edge overlap
+def bbox_overlap(p, e0, e1, dp, de0, de1, toc_upperbound):
+    max_p = np.maximum(p, p + toc_upperbound * dp) # point trajectory bbox top-right
+    min_p = np.minimum(p, p + toc_upperbound * dp) # point trajectory bbox bottom-left
+    max_e = np.maximum(np.maximum(e0, e0 + toc_upperbound * de0), np.maximum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox top-right
+    min_e = np.minimum(np.minimum(e0, e0 + toc_upperbound * de0), np.minimum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox bottom-left
+    if np.any(np.greater(min_p, max_e)) or np.any(np.greater(min_e, max_p)):
+        return False
+    else:
+        return True
+
+# compute the first "time" of contact, or toc,
+# return the computed toc only if it is smaller than the previously computed toc_upperbound
+def narrow_phase_CCD(_p, _e0, _e1, _dp, _de0, _de1, toc_upperbound):
+    p = deepcopy(_p)
+    e0 = deepcopy(_e0)
+    e1 = deepcopy(_e1)
+    dp = deepcopy(_dp)
+    de0 = deepcopy(_de0)
+    de1 = deepcopy(_de1)
+
+    # use relative displacement for faster convergence
+    mov = (dp + de0 + de1) / 3 
+    de0 -= mov
+    de1 -= mov
+    dp -= mov
+    maxDispMag = np.linalg.norm(dp) + math.sqrt(max(np.dot(de0, de0), np.dot(de1, de1)))
+    if maxDispMag == 0:
+        return toc_upperbound
+
+    eta = 0.1 # calculate the toc that first brings the distance to 0.1x the current distance
+    dist2_cur = PE.val(p, e0, e1)
+    dist_cur = math.sqrt(dist2_cur)
+    gap = eta * dist_cur
+    # iteratively move the point and edge towards each other and
+    # grow the toc estimate without numerical errors
+    toc = 0
+    while True:
+        tocLowerBound = (1 - eta) * dist_cur / maxDispMag
+
+        p += tocLowerBound * dp
+        e0 += tocLowerBound * de0
+        e1 += tocLowerBound * de1
+        dist2_cur = PE.val(p, e0, e1)
+        dist_cur = math.sqrt(dist2_cur)
+        if toc != 0 and dist_cur < gap:
+            break
+
+        toc += tocLowerBound
+        if toc > toc_upperbound:
+            return toc_upperbound
+
+    return toc

self_contact/distance/PointEdgeDistance.py

@@ -1,7 +1,7 @@
 import numpy as np
 
-import PointPointDistance as PP
-import PointLineDistance as PL
+import distance.PointPointDistance as PP
+import distance.PointLineDistance as PL
 
 def val(p, e0, e1):
     e = e1 - e0

self_contact/simulator.py

@@ -80,7 +80,7 @@ def screen_projection(x):
     pygame.display.flip()   # flip the display
 
     # step forward simulation and wait for screen refresh
-    [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, ground_n, ground_o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, h, 1e-2)
+    [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, ground_n, ground_o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, h, 1e-2)
     time_step += 1
     pygame.time.wait(int(h * 1000))
 

self_contact/time_integrator.py

@@ -13,7 +13,7 @@
 import FrictionEnergy
 import SpringEnergy
 
-def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, h, tol):
+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
@@ -27,51 +27,51 @@ def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, contact_area, mu, is_D
 
     # Newton loop
     iter = 0
-    E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, 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, 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, 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)
     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, 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, DBC, DBC_target, DBC_stiff, h)
 
         # filter line search
-        alpha = min(BarrierEnergy.init_step_size(x, n, o, 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, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff, h) > E_last:
+        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:
             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, 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, 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, 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)
         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, 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, 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, contact_area) + 
+        BarrierEnergy.val(x, n, o, bp, be, contact_area) + 
         FrictionEnergy.val(v, mu_lambda, 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, 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, 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, contact_area) + 
+        BarrierEnergy.grad(x, n, o, bp, be, contact_area) + 
         FrictionEnergy.grad(v, mu_lambda, 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, 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, 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, contact_area)
+    IJV_B = BarrierEnergy.hess(x, n, o, bp, be, contact_area)
     IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n)
     IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff)
     IJV_MS[2] *= h * h    # implicit Euler
@@ -84,9 +84,9 @@ def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_l
     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, 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, 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, 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, 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)
     # check whether each DBC is satisfied
     DBC_satisfied = [False] * len(x)
     for i in range(0, len(DBC)):