self_contact init

Author: liminchen

self_contact/BarrierEnergy.py

@@ -0,0 +1,93 @@
+import math
+import numpy as np
+
+dhat = 0.01
+kappa = 1e5
+
+def val(x, n, o, contact_area):
+    sum = 0.0
+    # floor:
+    for i in range(0, len(x)):
+        d = n.dot(x[i] - o)
+        if d < dhat:
+            s = d / dhat
+            sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
+    # ceil:
+    n = np.array([0.0, -1.0])
+    for i in range(0, len(x) - 1):
+        d = n.dot(x[i] - x[-1])
+        if d < dhat:
+            s = d / dhat
+            sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s)
+    return sum
+
+def grad(x, n, o, contact_area):
+    g = np.array([[0.0, 0.0]] * len(x))
+    # floor:
+    for i in range(0, len(x)):
+        d = n.dot(x[i] - o)
+        if d < dhat:
+            s = d / dhat
+            g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n
+    # ceil:
+    n = np.array([0.0, -1.0])
+    for i in range(0, len(x) - 1):
+        d = n.dot(x[i] - x[-1])
+        if d < dhat:
+            s = d / dhat
+            local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n
+            g[i] += local_grad
+            g[-1] -= local_grad
+    return g
+
+def hess(x, n, o, contact_area):
+    IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
+    # floor:
+    for i in range(0, len(x)):
+        d = n.dot(x[i] - o)
+        if d < dhat:
+            local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n)
+            for c in range(0, 2):
+                for r in range(0, 2):
+                    IJV[0].append(i * 2 + r)
+                    IJV[1].append(i * 2 + c)
+                    IJV[2] = np.append(IJV[2], local_hess[r, c])
+    # ceil:
+    n = np.array([0.0, -1.0])
+    for i in range(0, len(x) - 1):
+        d = n.dot(x[i] - x[-1])
+        if d < dhat:
+            local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n)
+            index = [i, len(x) - 1]
+            for nI in range(0, 2):
+                for nJ in range(0, 2):
+                    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], ((-1) ** (nI != nJ)) * local_hess[r, c])
+    return IJV
+
+def init_step_size(x, n, o, p):
+    alpha = 1
+    # floor:
+    for i in range(0, len(x)):
+        p_n = p[i].dot(n)
+        if p_n < 0:
+            alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n)
+    # ceil:
+    n = np.array([0.0, -1.0])
+    for i in range(0, len(x) - 1):
+        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)
+    return alpha
+
+def compute_mu_lambda(x, n, o, contact_area, mu):
+    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/FrictionEnergy.py

@@ -0,0 +1,60 @@
+import numpy as np
+import utils
+
+epsv = 1e-3
+
+def f0(vbarnorm, epsv, hhat):
+    if vbarnorm >= epsv:
+        return vbarnorm * hhat
+    else:
+        vbarnormhhat = vbarnorm * hhat
+        epsvhhat = epsv * hhat
+        return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0
+
+def f1_div_vbarnorm(vbarnorm, epsv):
+    if vbarnorm >= epsv:
+        return 1.0 / vbarnorm
+    else:
+        return (-vbarnorm + 2.0 * epsv) / (epsv * epsv)
+
+def f_hess_term(vbarnorm, epsv):
+    if vbarnorm >= epsv:
+        return -1.0 / (vbarnorm * vbarnorm)
+    else:
+        return -1.0 / (epsv * epsv)
+
+def val(v, mu_lambda, hhat, n):
+    sum = 0.0
+    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)
+    return sum
+
+def grad(v, mu_lambda, hhat, n):
+    g = np.array([[0.0, 0.0]] * len(v))
+    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)
+    return g
+
+def hess(v, mu_lambda, hhat, n):
+    IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)]
+    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])
+            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)
+            local_hess = mu_lambda[i] * T.dot(utils.make_PD(inner_term)).dot(np.transpose(T)) / hhat
+            for c in range(0, 2):
+                for r in range(0, 2):
+                    IJV[0].append(i * 2 + r)
+                    IJV[1].append(i * 2 + c)
+                    IJV[2] = np.append(IJV[2], local_hess[r, c])
+    return IJV

self_contact/GravityEnergy.py

@@ -0,0 +1,17 @@
+import numpy as np
+
+gravity = [0.0, -9.81]
+
+def val(x, m):
+    sum = 0.0
+    for i in range(0, len(x)):
+        sum += -m[i] * x[i].dot(gravity)
+    return sum
+
+def grad(x, m):
+    g = np.array([gravity] * len(x))
+    for i in range(0, len(x)):
+        g[i] *= -m[i]
+    return g
+
+# Hessian is 0

self_contact/InertiaEnergy.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+def val(x, x_tilde, m):
+    sum = 0.0
+    for i in range(0, len(x)):
+        diff = x[i] - x_tilde[i]
+        sum += 0.5 * m[i] * diff.dot(diff)
+    return sum
+
+def grad(x, x_tilde, m):
+    g = np.array([[0.0, 0.0]] * len(x))
+    for i in range(0, len(x)):
+        g[i] = m[i] * (x[i] - x_tilde[i])
+    return g
+
+def hess(x, x_tilde, m):
+    IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))]
+    for i in range(0, len(x)):
+        for d in range(0, 2):
+            IJV[0][i * 2 + d] = i * 2 + d
+            IJV[1][i * 2 + d] = i * 2 + d
+            IJV[2][i * 2 + d] = m[i]
+    return IJV

self_contact/NeoHookeanEnergy.py

@@ -0,0 +1,175 @@
+import utils
+import numpy as np
+import math
+
+def polar_svd(F):
+    [U, s, VT] = np.linalg.svd(F)
+    if np.linalg.det(U) < 0:
+        U[:, 1] = -U[:, 1]
+        s[1] = -s[1]
+    if np.linalg.det(VT) < 0:
+        VT[1, :] = -VT[1, :]
+        s[1] = -s[1]
+    return [U, s, VT]
+
+def dPsi_div_dsigma(s, mu, lam):
+    ln_sigma_prod = math.log(s[0] * s[1])
+    inv0 = 1.0 / s[0]
+    dPsi_dsigma_0 = mu * (s[0] - inv0) + lam * inv0 * ln_sigma_prod
+    inv1 = 1.0 / s[1]
+    dPsi_dsigma_1 = mu * (s[1] - inv1) + lam * inv1 * ln_sigma_prod
+    return [dPsi_dsigma_0, dPsi_dsigma_1]
+
+def d2Psi_div_dsigma2(s, mu, lam):
+    ln_sigma_prod = math.log(s[0] * s[1])
+    inv2_0 = 1 / (s[0] * s[0])
+    d2Psi_dsigma2_00 = mu * (1 + inv2_0) - lam * inv2_0 * (ln_sigma_prod - 1)
+    inv2_1 = 1 / (s[1] * s[1])
+    d2Psi_dsigma2_11 = mu * (1 + inv2_1) - lam * inv2_1 * (ln_sigma_prod - 1)
+    d2Psi_dsigma2_01 = lam / (s[0] * s[1])
+    return [[d2Psi_dsigma2_00, d2Psi_dsigma2_01], [d2Psi_dsigma2_01, d2Psi_dsigma2_11]]
+
+def B_left_coef(s, mu, lam):
+    sigma_prod = s[0] * s[1]
+    return (mu + (mu - lam * math.log(sigma_prod)) / sigma_prod) / 2
+
+def Psi(F, mu, lam):
+    J = np.linalg.det(F)
+    lnJ = math.log(J)
+    return mu / 2 * (np.trace(np.transpose(F).dot(F)) - 2) - mu * lnJ + lam / 2 * lnJ * lnJ
+
+def dPsi_div_dF(F, mu, lam):
+    FinvT = np.transpose(np.linalg.inv(F))
+    return mu * (F - FinvT) + lam * math.log(np.linalg.det(F)) * FinvT
+
+def d2Psi_div_dF2(F, mu, lam):
+    [U, sigma, VT] = polar_svd(F)
+
+    Psi_sigma_sigma = utils.make_PD(d2Psi_div_dsigma2(sigma, mu, lam))
+
+    B_left = B_left_coef(sigma, mu, lam)
+    Psi_sigma = dPsi_div_dsigma(sigma, mu, lam)
+    B_right = (Psi_sigma[0] + Psi_sigma[1]) / (2 * max(sigma[0] + sigma[1], 1e-6))
+    B = utils.make_PD([[B_left + B_right, B_left - B_right], [B_left - B_right, B_left + B_right]])
+
+    M = np.array([[0, 0, 0, 0]] * 4)
+    M[0, 0] = Psi_sigma_sigma[0, 0]
+    M[0, 3] = Psi_sigma_sigma[0, 1]
+    M[1, 1] = B[0, 0]
+    M[1, 2] = B[0, 1]
+    M[2, 1] = B[1, 0]
+    M[2, 2] = B[1, 1]
+    M[3, 0] = Psi_sigma_sigma[1, 0]
+    M[3, 3] = Psi_sigma_sigma[1, 1]
+
+    dP_div_dF = np.array([[0, 0, 0, 0]] * 4)
+    for j in range(0, 2):
+        for i in range(0, 2):
+            ij = j * 2 + i
+            for s in range(0, 2):
+                for r in range(0, 2):
+                    rs = s * 2 + r
+                    dP_div_dF[ij, rs] = M[0, 0] * U[i, 0] * VT[0, j] * U[r, 0] * VT[0, s] \
+                        + M[0, 3] * U[i, 0] * VT[0, j] * U[r, 1] * VT[1, s] \
+                        + M[1, 1] * U[i, 0] * VT[1, j] * U[r, 0] * VT[1, s] \
+                        + M[1, 2] * U[i, 0] * VT[1, j] * U[r, 1] * VT[0, s] \
+                        + M[2, 1] * U[i, 1] * VT[0, j] * U[r, 0] * VT[1, s] \
+                        + M[2, 2] * U[i, 1] * VT[0, j] * U[r, 1] * VT[0, s] \
+                        + M[3, 0] * U[i, 1] * VT[1, j] * U[r, 0] * VT[0, s] \
+                        + M[3, 3] * U[i, 1] * VT[1, j] * U[r, 1] * VT[1, s]
+    return dP_div_dF
+
+def deformation_grad(x, elemVInd, IB):
+    F = [x[elemVInd[1]] - x[elemVInd[0]], x[elemVInd[2]] - x[elemVInd[0]]]
+    return np.transpose(F).dot(IB)
+
+def dPsi_div_dx(P, IB):  # applying chain-rule, dPsi_div_dx = dPsi_div_dF * dF_div_dx
+    dPsi_dx_2 = P[0, 0] * IB[0, 0] + P[0, 1] * IB[0, 1]
+    dPsi_dx_3 = P[1, 0] * IB[0, 0] + P[1, 1] * IB[0, 1]
+    dPsi_dx_4 = P[0, 0] * IB[1, 0] + P[0, 1] * IB[1, 1]
+    dPsi_dx_5 = P[1, 0] * IB[1, 0] + P[1, 1] * IB[1, 1]
+    return [np.array([-dPsi_dx_2 - dPsi_dx_4, -dPsi_dx_3 - dPsi_dx_5]), np.array([dPsi_dx_2, dPsi_dx_3]), np.array([dPsi_dx_4, dPsi_dx_5])]
+
+def d2Psi_div_dx2(dP_div_dF, IB):  # applying chain-rule, d2Psi_div_dx2 = dF_div_dx^T * d2Psi_div_dF2 * dF_div_dx (note that d2F_div_dx2 = 0)
+    intermediate = np.array([[0.0, 0.0, 0.0, 0.0]] * 6)
+    for colI in range(0, 4):
+        _000 = dP_div_dF[0, colI] * IB[0, 0]
+        _010 = dP_div_dF[0, colI] * IB[1, 0]
+        _101 = dP_div_dF[2, colI] * IB[0, 1]
+        _111 = dP_div_dF[2, colI] * IB[1, 1]
+        _200 = dP_div_dF[1, colI] * IB[0, 0]
+        _210 = dP_div_dF[1, colI] * IB[1, 0]
+        _301 = dP_div_dF[3, colI] * IB[0, 1]
+        _311 = dP_div_dF[3, colI] * IB[1, 1]
+        intermediate[2, colI] = _000 + _101
+        intermediate[3, colI] = _200 + _301
+        intermediate[4, colI] = _010 + _111
+        intermediate[5, colI] = _210 + _311
+        intermediate[0, colI] = -intermediate[2, colI] - intermediate[4, colI]
+        intermediate[1, colI] = -intermediate[3, colI] - intermediate[5, colI]
+    result = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * 6)
+    for colI in range(0, 6):
+        _000 = intermediate[colI, 0] * IB[0, 0]
+        _010 = intermediate[colI, 0] * IB[1, 0]
+        _101 = intermediate[colI, 2] * IB[0, 1]
+        _111 = intermediate[colI, 2] * IB[1, 1]
+        _200 = intermediate[colI, 1] * IB[0, 0]
+        _210 = intermediate[colI, 1] * IB[1, 0]
+        _301 = intermediate[colI, 3] * IB[0, 1]
+        _311 = intermediate[colI, 3] * IB[1, 1]
+        result[2, colI] = _000 + _101
+        result[3, colI] = _200 + _301
+        result[4, colI] = _010 + _111
+        result[5, colI] = _210 + _311
+        result[0, colI] = -_000 - _101 - _010 - _111
+        result[1, colI] = -_200 - _301 - _210 - _311
+    return result
+
+def val(x, e, vol, IB, mu, lam):
+    sum = 0.0
+    for i in range(0, len(e)):
+        F = deformation_grad(x, e[i], IB[i])
+        sum += vol[i] * Psi(F, mu[i], lam[i])
+    return sum
+
+def grad(x, e, vol, IB, mu, lam):
+    g = np.array([[0.0, 0.0]] * len(x))
+    for i in range(0, len(e)):
+        F = deformation_grad(x, e[i], IB[i])
+        P = vol[i] * dPsi_div_dF(F, mu[i], lam[i])
+        g_local = dPsi_div_dx(P, IB[i])
+        for j in range(0, 3):
+            g[e[i][j]] += g_local[j]
+    return g
+
+def hess(x, e, vol, IB, mu, lam):
+    IJV = [[0] * (len(e) * 36), [0] * (len(e) * 36), np.array([0.0] * (len(e) * 36))]
+    for i in range(0, len(e)):
+        F = deformation_grad(x, e[i], IB[i])
+        dP_div_dF = vol[i] * d2Psi_div_dF2(F, mu[i], lam[i])
+        local_hess = d2Psi_div_dx2(dP_div_dF, IB[i])
+        for xI in range(0, 3):
+            for xJ in range(0, 3):
+                for dI in range(0, 2):
+                    for dJ in range(0, 2):
+                        ind = i * 36 + (xI * 3 + xJ) * 4 + dI * 2 + dJ
+                        IJV[0][ind] = e[i][xI] * 2 + dI
+                        IJV[1][ind] = e[i][xJ] * 2 + dJ
+                        IJV[2][ind] = local_hess[xI * 2 + dI, xJ * 2 + dJ]
+    return IJV
+
+def init_step_size(x, e, p):
+    alpha = 1
+    for i in range(0, len(e)):
+        x21 = x[e[i][1]] - x[e[i][0]]
+        x31 = x[e[i][2]] - x[e[i][0]]
+        p21 = p[e[i][1]] - p[e[i][0]]
+        p31 = p[e[i][2]] - p[e[i][0]]
+        detT = np.linalg.det(np.transpose([x21, x31]))
+        a = np.linalg.det(np.transpose([p21, p31])) / detT
+        b = (np.linalg.det(np.transpose([x21, p31])) + np.linalg.det(np.transpose([p21, x31]))) / detT
+        c = 0.9  # solve for alpha that first brings the new volume to 0.1x the old volume for slackness
+        critical_alpha = utils.smallest_positive_real_root_quad(a, b, c)
+        if critical_alpha > 0:
+            alpha = min(alpha, critical_alpha)
+    return alpha