"""Kernels of the contact of surfaces (Numba): the functions of the faces, the point of a
face nearest a point, the forces of a slave point on a master face or on a rigid
obstacle, with Coulomb's friction, and their tangents.
A *slave point* is an integration point of a face of the slave surface, of weight ``w``
(its share of the reference area of the face). Its state (:data:`STATE` values) is kept
from the end of a step to the next: the multiplier of the augmented Lagrangian, the point
of the master surface that faced it (a face and its coordinates; for an obstacle, its
position relative to the obstacle), the tangential traction, the energy that friction
dissipated per unit area, its gap and its pressure.
"""
from __future__ import annotations
import math
import numpy as np
from numba import njit, prange
#: The parameters of a contact: the penalty of the normal gap, the stiffness of the
#: stick, the coefficient of friction, the deepest penetration looked for, the gap below
#: which two surfaces touch, the size of the faces, and the distance within which a face
#: of a surface that lies behind a point of it, at rest, is across a wall from the point;
#: how far beyond the rim of the master a point still faces it (:data:`RIM`), and whether
#: the weight of a point is the share of its face that the other surface sees
#: (:func:`_share`: the points at the nodes)
P_STIFF, P_STICK, P_MU, P_DEPTH, P_TOUCH, P_SIZE, P_BEHIND, P_RIM, P_SHARE = range(9)
#: How far beyond the rim of the master surface (in the coordinates of its faces) a slave
#: point still faces it, on the face continued: nothing for the points inside the faces
#: of the slave; a twentieth of a face for those at its nodes, which may lie on the rim
#: of the master (two bodies of the same section) and would otherwise face it or not
#: with the rounding of each iteration
RIM, RIM_NODES = 1e-12, 0.05
#: How far outside a master face (in its coordinates) a slave point stays on it
SLACK = 0.05
#: Two faces face each other when the cosine of the angle of their outward normals is
#: below this (more than 104 degrees apart): faces at right angles along an edge of a body,
#: a little distorted, do not
FACING = -0.25
#: The same for a surface against itself: the walls of a notch that closes touch at its
#: bottom while their normals are still far from opposite (the two walls of the corner of
#: a hexagonal cell: 60 degrees apart at rest), and what tells them from the two sides of
#: an edge of the body is where they lay at rest (:func:`_across`)
FACING_OWN = 0.6
#: The state of a slave point (the last: the share of its weight that presses)
S_LAMBDA, S_FACE, S_XI, S_ETA, S_TX, S_TY, S_TZ, S_WORK, S_GAP, S_PRESSURE, S_SHARE = range(11)
STATE = 11
#: The kinds of rigid obstacles
PLANE, SPHERE, CYLINDER, CONE = 0, 1, 2, 3
#: The points of the faces of three nodes (exact to degree 2) and of six (to degree 4):
#: ξ, η and weights that add up to 1
POINTS3 = np.array([[1 / 6, 1 / 6, 1 / 3], [2 / 3, 1 / 6, 1 / 3], [1 / 6, 2 / 3, 1 / 3]])
#: The points at the nodes of the faces, for surfaces whose corners and edges touch
#: first: the vertices of a face of three nodes (exact to degree 1); the vertices, the
#: middles of the edges and the centroid of a face of six (exact to degree 3)
NODES3 = np.array([[0.0, 0.0, 1 / 3], [1.0, 0.0, 1 / 3], [0.0, 1.0, 1 / 3]])
NODES6 = np.array(
[
[0.0, 0.0, 1 / 20],
[1.0, 0.0, 1 / 20],
[0.0, 1.0, 1 / 20],
[0.5, 0.0, 2 / 15],
[0.5, 0.5, 2 / 15],
[0.0, 0.5, 2 / 15],
[1 / 3, 1 / 3, 9 / 20],
]
)
#: The rules of the contact points of a surface
RULES = ("gauss", "nodes")
_GA, _GB = 0.445948490915965, 0.091576213509771
POINTS6 = np.array(
[
[_GA, _GA, 0.223381589678011],
[1.0 - 2.0 * _GA, _GA, 0.223381589678011],
[_GA, 1.0 - 2.0 * _GA, 0.223381589678011],
[_GB, _GB, 0.109951743655322],
[1.0 - 2.0 * _GB, _GB, 0.109951743655322],
[_GB, 1.0 - 2.0 * _GB, 0.109951743655322],
]
)
[docs]
@njit(cache=True)
def shapes(n, xi, eta, shape, dxi, deta):
"""The functions of the nodes of a face of ``n`` nodes (3, or 6: the corners, then the
middles of the edges 0-1, 1-2 and 2-0) at a point, and their derivatives."""
zeta = 1.0 - xi - eta
if n == 3:
shape[0], shape[1], shape[2] = zeta, xi, eta
dxi[0], dxi[1], dxi[2] = -1.0, 1.0, 0.0
deta[0], deta[1], deta[2] = -1.0, 0.0, 1.0
return
shape[0] = zeta * (2.0 * zeta - 1.0)
shape[1] = xi * (2.0 * xi - 1.0)
shape[2] = eta * (2.0 * eta - 1.0)
shape[3] = 4.0 * zeta * xi
shape[4] = 4.0 * xi * eta
shape[5] = 4.0 * eta * zeta
dxi[0] = 1.0 - 4.0 * zeta
dxi[1] = 4.0 * xi - 1.0
dxi[2] = 0.0
dxi[3] = 4.0 * (zeta - xi)
dxi[4] = 4.0 * eta
dxi[5] = -4.0 * eta
deta[0] = 1.0 - 4.0 * zeta
deta[1] = 0.0
deta[2] = 4.0 * eta - 1.0
deta[3] = -4.0 * xi
deta[4] = 4.0 * xi
deta[5] = 4.0 * (zeta - eta)
@njit(cache=True)
def _closest_flat(a, b, c, p):
"""The coordinates (ξ, η) of the point of the triangle ``a b c`` nearest ``p``
(Ericson 2005, §5.1.5: the regions of its corners, of its edges and of its face)."""
ab = b - a
ac = c - a
ap = p - a
d1 = ab[0] * ap[0] + ab[1] * ap[1] + ab[2] * ap[2]
d2 = ac[0] * ap[0] + ac[1] * ap[1] + ac[2] * ap[2]
if d1 <= 0.0 and d2 <= 0.0:
return 0.0, 0.0
bp = p - b
d3 = ab[0] * bp[0] + ab[1] * bp[1] + ab[2] * bp[2]
d4 = ac[0] * bp[0] + ac[1] * bp[1] + ac[2] * bp[2]
if d3 >= 0.0 and d4 <= d3:
return 1.0, 0.0
vc = d1 * d4 - d3 * d2
if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0:
return d1 / (d1 - d3), 0.0
cp = p - c
d5 = ab[0] * cp[0] + ab[1] * cp[1] + ab[2] * cp[2]
d6 = ac[0] * cp[0] + ac[1] * cp[1] + ac[2] * cp[2]
if d6 >= 0.0 and d5 <= d6:
return 0.0, 1.0
vb = d5 * d2 - d1 * d6
if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0:
return 0.0, d2 / (d2 - d6)
va = d3 * d6 - d5 * d4
if va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0:
w = (d4 - d3) / ((d4 - d3) + (d5 - d6))
return 1.0 - w, w
denom = 1.0 / (va + vb + vc)
return vb * denom, vc * denom
@njit(cache=True)
def _at(n, x, xi, eta, point, a, b):
"""The point of a face at (ξ, η) and its two tangents."""
shape = np.empty(n)
dxi = np.empty(n)
deta = np.empty(n)
shapes(n, xi, eta, shape, dxi, deta)
for c in range(3):
point[c] = 0.0
a[c] = 0.0
b[c] = 0.0
for i in range(n):
point[c] += shape[i] * x[i, c]
a[c] += dxi[i] * x[i, c]
b[c] += deta[i] * x[i, c]
@njit(cache=True)
def _edge_point(x, edge, t, point, tangent, curvature):
"""The point of an edge of a face of six nodes at ``t`` (edge 0: from the corner 0 to
1, 1: from 1 to 2, 2: from 2 to 0), its derivative and its second derivative."""
i, j, k = edge, (edge + 1) % 3, 3 + edge # its ends and its middle
n0 = (1.0 - t) * (1.0 - 2.0 * t)
n1 = t * (2.0 * t - 1.0)
n2 = 4.0 * t * (1.0 - t)
for c in range(3):
point[c] = n0 * x[i, c] + n1 * x[j, c] + n2 * x[k, c]
tangent[c] = (
(4.0 * t - 3.0) * x[i, c] + (4.0 * t - 1.0) * x[j, c] + (4.0 - 8.0 * t) * x[k, c]
)
curvature[c] = 4.0 * x[i, c] + 4.0 * x[j, c] - 8.0 * x[k, c]
[docs]
@njit(cache=True)
def closest(n, x, p, slack, rim, over=RIM):
"""The coordinates (ξ, η) of the point of a face of ``n`` nodes (``x``: their
positions) nearest ``p``, the square of their distance, and how it lies: 1, the foot
of the perpendicular from ``p`` (the gap is then along the normal of the face); 0, a
point of an edge that the face shares with another of its surface (a point over a
ridge); -1, a point of the rim of the surface (``rim``: which of the edges 0-1, 1-2
and 2-0 of the face no other face shares): ``p`` is beyond the surface. A face of six
nodes may be curved: Newton's method on its surface from the nearest point of the
triangle of its corners, then, where it leaves the face, on its three edges.
``slack``: how far outside the face (in its coordinates), across the edges that it
shares, the foot of the perpendicular is still taken, on the face continued: a point
that hovers over an edge then stays on one face; ``over``: likewise across the edges
of the rim (:data:`RIM`)."""
# how far out each edge lets the foot of the perpendicular be
out0 = over if rim[0] else slack # the edge 0-1: η = 0
out1 = over if rim[1] else slack # the edge 1-2: ξ + η = 1
out2 = over if rim[2] else slack # the edge 2-0: ξ = 0
point = np.empty(3)
a = np.empty(3)
b = np.empty(3)
if n == 3:
# the foot of the perpendicular on the plane of the triangle
for c in range(3):
a[c] = x[1, c] - x[0, c]
b[c] = x[2, c] - x[0, c]
point[c] = p[c] - x[0, c]
d00 = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]
d01 = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
d11 = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]
d20 = point[0] * a[0] + point[1] * a[1] + point[2] * a[2]
d21 = point[0] * b[0] + point[1] * b[1] + point[2] * b[2]
det = d00 * d11 - d01 * d01
xi = (d11 * d20 - d01 * d21) / det
eta = (d00 * d21 - d01 * d20) / det
free = xi >= -out2 and eta >= -out0 and xi + eta <= 1.0 + out1
if not free:
xi, eta = _closest_flat(x[0], x[1], x[2], p)
_at(3, x, xi, eta, point, a, b)
d2 = (point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2 + (point[2] - p[2]) ** 2
return xi, eta, d2, 1 if free else _on_rim(xi, eta, rim)
xi, eta = _closest_flat(x[0], x[1], x[2], p)
# the second derivatives of the position: constant on a face of six nodes
xx = np.empty(3)
yy = np.empty(3)
xy = np.empty(3)
for c in range(3):
xx[c] = 4.0 * (x[0, c] + x[1, c]) - 8.0 * x[3, c]
yy[c] = 4.0 * (x[0, c] + x[2, c]) - 8.0 * x[5, c]
xy[c] = 4.0 * (x[0, c] - x[3, c] + x[4, c] - x[5, c])
u, v = xi, eta
free = False
for _ in range(20):
_at(6, x, u, v, point, a, b)
r0, r1, r2 = point[0] - p[0], point[1] - p[1], point[2] - p[2]
g1 = r0 * a[0] + r1 * a[1] + r2 * a[2]
g2 = r0 * b[0] + r1 * b[1] + r2 * b[2]
aa = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]
bb = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]
ab = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
h11 = aa + r0 * xx[0] + r1 * xx[1] + r2 * xx[2]
h22 = bb + r0 * yy[0] + r1 * yy[1] + r2 * yy[2]
h12 = ab + r0 * xy[0] + r1 * xy[1] + r2 * xy[2]
det = h11 * h22 - h12 * h12
if det <= 1e-12 * aa * bb or h11 <= 0.0: # not convex there: Gauss-Newton
h11, h22, h12 = aa, bb, ab
det = h11 * h22 - h12 * h12
du = -(h22 * g1 - h12 * g2) / det
dv = -(h11 * g2 - h12 * g1) / det
u += du
v += dv
if u < -0.5 or v < -0.5 or u + v > 1.5:
break # far outside the face: its edges
if abs(du) + abs(dv) < 1e-13:
free = u >= -out2 - 1e-12 and v >= -out0 - 1e-12 and u + v <= 1.0 + out1 + 1e-12
break
if free:
_at(6, x, u, v, point, a, b)
d2 = (point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2 + (point[2] - p[2]) ** 2
return u, v, d2, 1
best = math.inf
for edge in range(3):
if edge == 0:
t = xi
elif edge == 1:
t = eta
else:
t = 1.0 - eta
t = min(max(t, 0.0), 1.0)
for _ in range(20):
_edge_point(x, edge, t, point, a, b)
r0, r1, r2 = point[0] - p[0], point[1] - p[1], point[2] - p[2]
g = r0 * a[0] + r1 * a[1] + r2 * a[2]
h = a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + r0 * b[0] + r1 * b[1] + r2 * b[2]
if h <= 0.0:
h = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]
new = min(max(t - g / h, 0.0), 1.0)
if abs(new - t) < 1e-13:
t = new
break
t = new
_edge_point(x, edge, t, point, a, b)
d2 = (point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2 + (point[2] - p[2]) ** 2
if d2 < best:
best = d2
if edge == 0:
xi, eta = t, 0.0
elif edge == 1:
xi, eta = 1.0 - t, t
else:
xi, eta = 0.0, 1.0 - t
return xi, eta, best, _on_rim(xi, eta, rim)
@njit(cache=True)
def _on_rim(xi, eta, rim):
"""-1 if a point of the boundary of a face lies on an edge of the rim of its surface,
0 otherwise."""
if rim[0] and eta <= 1e-12:
return -1
if rim[1] and xi + eta >= 1.0 - 1e-12:
return -1
if rim[2] and xi <= 1e-12:
return -1
return 0
@njit(cache=True)
def _normal(a, b, n):
"""The unit normal ``a × b`` of two tangents; returns the norm of the product."""
n[0] = a[1] * b[2] - a[2] * b[1]
n[1] = a[2] * b[0] - a[0] * b[2]
n[2] = a[0] * b[1] - a[1] * b[0]
norm = math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2])
n[0] /= norm
n[1] /= norm
n[2] /= norm
return norm
@njit(cache=True)
def _share(ns, xs, xi, eta, normal) -> float:
"""The share of the face of a slave point that the other surface sees: the cosine of
the angle of the inward normal of the face, at the point, and the ``normal`` of the
other surface there (1 for two surfaces that lie on each other, 0 for a face seen
edge-on or from behind). The points at the nodes of the faces carry it: a node of an
edge of a body belongs to the faces of both sides of the edge, and only what each
shows to the other surface presses on it (the side of a block that stands on a plane
carries nothing)."""
p = np.empty(3)
a = np.empty(3)
b = np.empty(3)
own = np.empty(3)
_at(ns, xs, xi, eta, p, a, b)
_normal(a, b, own)
return max(0.0, -(own[0] * normal[0] + own[1] * normal[1] + own[2] * normal[2]))
@njit(cache=True)
def _friction(par, pressure, slip, normal, old, predict, traction):
"""The tangential traction of a slave point that moved by ``slip`` along the master
since the end of the last step (a radial return on the traction of then, turned into
the tangent plane of now); returns the energy dissipated per unit area."""
mu, stick = par[P_MU], par[P_STICK]
traction[0] = traction[1] = traction[2] = 0.0
if mu <= 0.0:
return 0.0
# the traction of the last step in the tangent plane, its norm kept
t0, t1, t2 = old[S_TX], old[S_TY], old[S_TZ]
before = math.sqrt(t0 * t0 + t1 * t1 + t2 * t2)
along = t0 * normal[0] + t1 * normal[1] + t2 * normal[2]
t0 -= along * normal[0]
t1 -= along * normal[1]
t2 -= along * normal[2]
now = math.sqrt(t0 * t0 + t1 * t1 + t2 * t2)
if now > 0.0:
t0 *= before / now
t1 *= before / now
t2 *= before / now
along = slip[0] * normal[0] + slip[1] * normal[1] + slip[2] * normal[2]
t0 += stick * (slip[0] - along * normal[0])
t1 += stick * (slip[1] - along * normal[1])
t2 += stick * (slip[2] - along * normal[2])
norm = math.sqrt(t0 * t0 + t1 * t1 + t2 * t2)
limit = mu * max(pressure, 0.0)
if norm <= limit or predict:
traction[0], traction[1], traction[2] = t0, t1, t2
return 0.0
scale = limit / norm
traction[0], traction[1], traction[2] = scale * t0, scale * t1, scale * t2
# the slip of the return: (trial - traction) / stick, along the traction
return limit * (norm - limit) / stick
@njit(cache=True)
def _touched(par, old) -> bool:
"""Whether a point pressed or touched the master at the end of the last step."""
return old[S_PRESSURE] > 0.0 or old[S_GAP] <= par[P_TOUCH]
[docs]
@njit(cache=True)
def pair_point(ns, nm, xs, xm, xs_old, xm_old, rim, xi, eta, weight, par, old, anchored, anchor,
predict, f, new): # fmt: skip
"""The forces (``f``: the nodes of the slave face, then those of the master face) of a
slave point at (``xi``, ``eta``) of its face against a master face, and its state
``new``. ``anchored``: the point of the master that faced it at the end of the last
step lies on this face (at the coordinates of ``old``); otherwise ``anchor`` is its
position now; a point that no point of the master faced then rubs by the move of
the two points that touch now since then (``xs_old``, ``xm_old``: the nodes of the two
faces at the end of the last step). ``rim``: the edges of the master face on the rim
of its surface.
``predict``: a point that touched stays closed and sticks. Returns whether the point
presses."""
shape = np.empty(ns)
dxi = np.empty(ns)
deta = np.empty(ns)
shapes(ns, xi, eta, shape, dxi, deta)
p = np.zeros(3)
for i in range(ns):
for c in range(3):
p[c] += shape[i] * xs[i, c]
u, v, _, _ = closest(nm, xm, p, SLACK, rim, par[P_RIM])
there = np.empty(3)
a = np.empty(3)
b = np.empty(3)
_at(nm, xm, u, v, there, a, b)
normal = np.empty(3)
_normal(a, b, normal)
gap = (
(p[0] - there[0]) * normal[0]
+ (p[1] - there[1]) * normal[1]
+ (p[2] - there[2]) * normal[2]
)
touched = _touched(par, old)
closed = predict and touched
pressure = old[S_LAMBDA] - par[P_STIFF] * gap
for k in range(STATE):
new[k] = old[k]
new[S_XI], new[S_ETA], new[S_GAP] = u, v, gap
new[S_TX] = new[S_TY] = new[S_TZ] = 0.0
new[S_PRESSURE] = 0.0
new[S_SHARE] = 1.0
for k in range(3 * (ns + nm)):
f[k] = 0.0
if pressure <= 0.0 and not closed:
return False
if par[P_SHARE] > 0.0: # a point at a node: what its face shows to the master
new[S_SHARE] = _share(ns, xs, xi, eta, normal)
weight = weight * new[S_SHARE]
new[S_PRESSURE] = max(pressure, 0.0)
traction = np.zeros(3)
shape_m = np.empty(nm)
shapes(nm, u, v, shape_m, np.empty(nm), np.empty(nm))
if par[P_MU] > 0.0:
slip = np.empty(3)
if not touched: # it comes into contact: the move of the two points over the step
for c in range(3):
slip[c] = p[c] - there[c]
for i in range(ns):
slip[c] -= shape[i] * xs_old[i, c]
for i in range(nm):
slip[c] += shape_m[i] * xm_old[i, c]
elif anchored:
_at(nm, xm, old[S_XI], old[S_ETA], slip, a, b)
for c in range(3):
slip[c] = there[c] - slip[c]
else:
for c in range(3):
slip[c] = there[c] - anchor[c]
rubbed = _friction(par, pressure, slip, normal, old, closed, traction)
new[S_WORK] = old[S_WORK] + new[S_SHARE] * rubbed
new[S_TX], new[S_TY], new[S_TZ] = traction[0], traction[1], traction[2]
for c in range(3):
force = weight * (pressure * normal[c] - traction[c]) # on the slave point
for i in range(ns):
f[3 * i + c] = -shape[i] * force
for i in range(nm):
f[3 * (ns + i) + c] = shape_m[i] * force
return True
@njit(cache=True)
def _slave_normal(ns, xs, xi, eta, p, normal):
"""A slave point and the outward normal of its face there."""
a = np.empty(3)
b = np.empty(3)
_at(ns, xs, xi, eta, p, a, b)
_normal(a, b, normal)
@njit(cache=True)
def _across(nm, xm, p, rim, par) -> bool:
"""Whether a face of a surface is across a wall of solid from a point of the same
surface: at rest (``xm``: the nodes of the face, ``p``: the point, both in the
reference positions), the point lies behind the face, within the distance
:data:`P_BEHIND` of it. The two sides of a thin ligament, back to back, look like two
surfaces that went through each other by its thickness; surfaces that do come to
touch were in front of each other at rest."""
u, v, d2, _ = closest(nm, xm, p, 0.0, rim)
if d2 > par[P_BEHIND] * par[P_BEHIND]:
return False
there = np.empty(3)
a = np.empty(3)
b = np.empty(3)
normal = np.empty(3)
_at(nm, xm, u, v, there, a, b)
_normal(a, b, normal)
gap = ((p[0] - there[0]) * normal[0] + (p[1] - there[1]) * normal[1]
+ (p[2] - there[2]) * normal[2]) # fmt: skip
return gap < -par[P_TOUCH]
[docs]
@njit(parallel=True, cache=True)
def pair_forces(x, x_old, x_ref, ident, slave, master, rims, face, xi, eta, weight, candidates,
last, par, old, new, own, tangent, predict, fe, ke, nodes, active): # fmt: skip
"""The forces and tangents of the slave points of a pair of surfaces in the positions
``x`` of the nodes (``x_old``: at the end of the last step; ``x_ref``: at rest;
``ident``: the node that each is, itself or, for the image of a node of a periodic
cell, the one that it is the same as across a face of the cell). For
each point, the master face among its ``candidates`` whose nearest point is the
closest, among those that face it (opposite normals) within the deepest penetration
looked for, and not beyond the rim of the master surface
(``rims``: the edges of each master face that no other shares); ``own``: the two
surfaces are one (the faces that share a node with that of the point are left out,
and those that it has gone through but that lay behind it at rest, across a wall of
solid: :func:`_across`).
``last``: the face of each point at the
last evaluation (-1: none), which it keeps while the foot of its perpendicular stays on
it or within :data:`SLACK` of it: a point that hovers over an edge between two faces,
where the tangent of the forces breaks, would otherwise go from one to the other at
each iteration. ``fe`` (points x dofs), ``ke`` (points x
dofs x dofs, by central differences of the forces), ``nodes`` (the nodes of the dofs)
and ``active`` (whether the point presses)."""
ns, nm = slave.shape[1], master.shape[1]
nd = 3 * (ns + nm)
for q in prange(face.shape[0]):
xs = np.empty((ns, 3))
for i in range(ns):
xs[i] = x[slave[face[q], i]]
p = np.empty(3)
normal_s = np.empty(3)
_slave_normal(ns, xs, xi[q], eta[q], p, normal_s)
xm = np.empty((nm, 3))
there = np.empty(3)
a = np.empty(3)
b = np.empty(3)
normal = np.empty(3)
p_ref = np.empty(3) # the point at rest, for the faces that it has gone through
if own:
for i in range(ns):
xs[i] = x_ref[slave[face[q], i]]
_slave_normal(ns, xs, xi[q], eta[q], p_ref, normal)
for i in range(ns):
xs[i] = x[slave[face[q], i]]
best, best_d2, best_gap = -1, math.inf, math.inf
before = int(old[q, S_FACE])
apart = FACING_OWN if own else FACING
if last[q] >= 0: # the face of the last evaluation, while the point is over it
m = last[q]
for i in range(nm):
xm[i] = x[master[m, i]]
u, v, d2, how = closest(nm, xm, p, SLACK, rims[m], par[P_RIM])
if how == 1:
_at(nm, xm, u, v, there, a, b)
_normal(a, b, normal)
gap = ((p[0] - there[0]) * normal[0] + (p[1] - there[1]) * normal[1]
+ (p[2] - there[2]) * normal[2]) # fmt: skip
facing = (normal[0] * normal_s[0] + normal[1] * normal_s[1]
+ normal[2] * normal_s[2]) < apart # fmt: skip
if facing and gap >= -par[P_DEPTH]:
through = False
if own and gap < 0.0:
for i in range(nm):
xm[i] = x_ref[master[m, i]]
through = _across(nm, xm, p_ref, rims[m], par)
if not through:
best, best_d2, best_gap = m, d2, gap
kept = best >= 0
for k in range(candidates.shape[1]):
m = candidates[q, k]
if m < 0 or kept:
continue
if own:
shared = False
for i in range(3):
for j in range(3):
if ident[slave[face[q], i]] == ident[master[m, j]]:
shared = True
if shared:
continue
for i in range(nm):
xm[i] = x[master[m, i]]
u, v, d2, how = closest(nm, xm, p, 0.0, rims[m], par[P_RIM])
if how < 0:
continue # beyond the rim of the surface
if how == 0:
# on an edge that two faces share: over a ridge of the surface (the point
# is then within 60 degrees of the normal of the face), or beside the
# face, a little further than the corner where the rim meets that edge
_at(nm, xm, u, v, there, a, b)
_normal(a, b, normal)
along = ((p[0] - there[0]) * normal[0] + (p[1] - there[1]) * normal[1]
+ (p[2] - there[2]) * normal[2]) # fmt: skip
beside = d2 - along * along
if beside > (0.02 * par[P_SIZE]) ** 2 and along * along < 0.25 * d2:
continue
# a point on an edge is as near two faces: it stays on that of the last step,
# or goes to the first of them, whatever the rounding and the order of the search
margin = 1e-9 * best_d2 + 1e-300
if d2 > best_d2 + margin:
continue
if d2 >= best_d2 - margin and (best == before or (m != before and m > best)):
continue
_at(nm, xm, u, v, there, a, b)
_normal(a, b, normal)
if (normal[0] * normal_s[0] + normal[1] * normal_s[1]
+ normal[2] * normal_s[2]) >= apart: # fmt: skip
continue # a face that does not face the point: the other side of a wall
gap = ((p[0] - there[0]) * normal[0] + (p[1] - there[1]) * normal[1]
+ (p[2] - there[2]) * normal[2]) # fmt: skip
if gap < -par[P_DEPTH]:
continue
if own and gap < 0.0:
for i in range(nm):
xm[i] = x_ref[master[m, i]]
if _across(nm, xm, p_ref, rims[m], par):
continue # the other side of a ligament, not a face gone through
best, best_d2, best_gap = m, d2, gap
active[q] = False
for k in range(STATE):
new[q, k] = old[q, k]
new[q, S_FACE] = best
new[q, S_GAP] = best_gap
new[q, S_PRESSURE] = 0.0
new[q, S_TX] = new[q, S_TY] = new[q, S_TZ] = 0.0
if best < 0:
continue
xs_old = np.empty((ns, 3))
xm_old = np.empty((nm, 3))
for i in range(ns):
xs_old[i] = x_old[slave[face[q], i]]
for i in range(nm):
xm[i] = x[master[best, i]]
xm_old[i] = x_old[master[best, i]]
anchored = before == best
anchor = np.zeros(3)
if not anchored and before >= 0: # the point that faced it, on another face
xo = np.empty((nm, 3))
for i in range(nm):
xo[i] = x[master[before, i]]
_at(nm, xo, old[q, S_XI], old[q, S_ETA], anchor, a, b)
state = old[q].copy()
if before < 0: # no point faced it: nothing to rub against yet
state[S_GAP] = math.inf
state[S_PRESSURE] = 0.0
f = np.empty(nd)
fresh = np.empty(STATE)
pressed = pair_point(ns, nm, xs, xm, xs_old, xm_old, rims[best], xi[q], eta[q], weight[q],
par, state, anchored, anchor, False, f, fresh) # fmt: skip
fresh[S_FACE] = best
new[q] = fresh
closed = predict and _touched(par, state)
if not (pressed or closed):
continue
active[q] = True
fe[q] = f
for i in range(ns):
nodes[q, i] = slave[face[q], i]
for i in range(nm):
nodes[q, ns + i] = master[best, i]
if not tangent:
continue
size = math.sqrt(best_d2) + math.sqrt(
(xm[1, 0] - xm[0, 0]) ** 2 + (xm[1, 1] - xm[0, 1]) ** 2 + (xm[1, 2] - xm[0, 2]) ** 2
)
step = 1e-7 * size
fp = np.empty(nd)
fm = np.empty(nd)
dummy = np.empty(STATE)
for j in range(nd):
node, c = j // 3, j % 3
target = xs if node < ns else xm
row = node if node < ns else node - ns
keep = target[row, c]
target[row, c] = keep + step
pair_point(ns, nm, xs, xm, xs_old, xm_old, rims[best], xi[q], eta[q], weight[q],
par, state, anchored, anchor, closed, fp, dummy) # fmt: skip
target[row, c] = keep - step
pair_point(ns, nm, xs, xm, xs_old, xm_old, rims[best], xi[q], eta[q], weight[q],
par, state, anchored, anchor, closed, fm, dummy) # fmt: skip
target[row, c] = keep
for i in range(nd):
ke[q, i, j] = (fp[i] - fm[i]) / (2.0 * step)
[docs]
@njit(cache=True)
def obstacle_distance(kind, geometry, p, normal):
"""The signed distance of the point ``p`` to a rigid obstacle (positive outside it)
and its outward normal there. ``geometry``: a point (0-2), a unit axis (3-5), a size
(6: the radius of a sphere or of a cylinder, the half-angle of a cone) and a sign (7:
-1 for the inside of a sphere or of a cylinder that holds the body). A plane: its
point and its outward normal; a sphere: its centre; a cylinder: a point of its axis;
a cone: its apex and its axis, from the apex into the cone."""
d0, d1, d2 = p[0] - geometry[0], p[1] - geometry[1], p[2] - geometry[2]
a0, a1, a2 = geometry[3], geometry[4], geometry[5]
if kind == PLANE:
normal[0], normal[1], normal[2] = a0, a1, a2
return d0 * a0 + d1 * a1 + d2 * a2
sign = geometry[7]
if kind == SPHERE:
r = math.sqrt(d0 * d0 + d1 * d1 + d2 * d2)
if r > 0.0:
normal[0], normal[1], normal[2] = sign * d0 / r, sign * d1 / r, sign * d2 / r
else:
normal[0], normal[1], normal[2] = sign * a0, sign * a1, sign * a2
return sign * (r - geometry[6])
along = d0 * a0 + d1 * a1 + d2 * a2
r0, r1, r2 = d0 - along * a0, d1 - along * a1, d2 - along * a2
r = math.sqrt(r0 * r0 + r1 * r1 + r2 * r2)
if kind == CYLINDER:
if r > 0.0:
normal[0], normal[1], normal[2] = sign * r0 / r, sign * r1 / r, sign * r2 / r
else:
normal[0], normal[1], normal[2] = 0.0, 0.0, 0.0
return sign * (r - geometry[6])
# a cone: its side where the nearest point is on it, its apex otherwise
cos, sin = math.cos(geometry[6]), math.sin(geometry[6])
if along * cos + r * sin <= 0.0: # behind the apex
full = math.sqrt(d0 * d0 + d1 * d1 + d2 * d2)
if full > 0.0:
normal[0], normal[1], normal[2] = d0 / full, d1 / full, d2 / full
else:
normal[0], normal[1], normal[2] = -a0, -a1, -a2
return full
if r > 0.0:
e0, e1, e2 = r0 / r, r1 / r, r2 / r
else: # on the axis, inside: any radial direction
e0, e1, e2 = _any_normal(a0, a1, a2)
normal[0] = cos * e0 - sin * a0
normal[1] = cos * e1 - sin * a1
normal[2] = cos * e2 - sin * a2
return r * cos - along * sin
@njit(cache=True)
def _any_normal(a0, a1, a2):
"""A unit vector normal to the unit vector ``a``."""
if abs(a0) < 0.9:
b0, b1, b2 = 1.0, 0.0, 0.0
else:
b0, b1, b2 = 0.0, 1.0, 0.0
along = b0 * a0 + b1 * a1 + b2 * a2
b0, b1, b2 = b0 - along * a0, b1 - along * a1, b2 - along * a2
norm = math.sqrt(b0 * b0 + b1 * b1 + b2 * b2)
return b0 / norm, b1 / norm, b2 / norm
[docs]
@njit(cache=True)
def obstacle_point(ns, xs, xi, eta, weight, kind, geometry, par, old, predict, f, new):
"""The forces (the nodes of its face) of a slave point against a rigid obstacle and
its state ``new``; its state keeps, in place of a point of a master face, its position
relative to the obstacle at the end of the last step. Returns whether it presses."""
shape = np.empty(ns)
shapes(ns, xi, eta, shape, np.empty(ns), np.empty(ns))
p = np.zeros(3)
for i in range(ns):
for c in range(3):
p[c] += shape[i] * xs[i, c]
normal = np.empty(3)
gap = obstacle_distance(kind, geometry, p, normal)
touched = _touched(par, old)
closed = predict and touched
pressure = old[S_LAMBDA] - par[P_STIFF] * gap
for k in range(STATE):
new[k] = old[k]
new[S_GAP] = gap
new[S_TX] = new[S_TY] = new[S_TZ] = 0.0
new[S_PRESSURE] = 0.0
# where it is, relative to the obstacle
new[S_FACE], new[S_XI], new[S_ETA] = p[0] - geometry[0], p[1] - geometry[1], p[2] - geometry[2]
new[S_SHARE] = 1.0
for k in range(3 * ns):
f[k] = 0.0
if pressure <= 0.0 and not closed:
return False
if par[P_SHARE] > 0.0: # a point at a node: what its face shows to the obstacle
new[S_SHARE] = _share(ns, xs, xi, eta, normal)
weight = weight * new[S_SHARE]
new[S_PRESSURE] = max(pressure, 0.0)
traction = np.zeros(3)
if par[P_MU] > 0.0:
slip = np.empty(3)
slip[0] = new[S_FACE] - old[S_FACE]
slip[1] = new[S_XI] - old[S_XI]
slip[2] = new[S_ETA] - old[S_ETA]
rubbed = _friction(par, pressure, slip, normal, old, closed, traction)
new[S_WORK] = old[S_WORK] + new[S_SHARE] * rubbed
new[S_TX], new[S_TY], new[S_TZ] = traction[0], traction[1], traction[2]
for c in range(3):
force = weight * (pressure * normal[c] - traction[c]) # on the slave point
for i in range(ns):
f[3 * i + c] = -shape[i] * force
return True
[docs]
@njit(parallel=True, cache=True)
def obstacle_forces(x, slave, face, xi, eta, weight, kind, geometry, par, old, new, tangent,
predict, fe, ke, active): # fmt: skip
"""The forces and tangents (central differences) of the slave points of a surface
against a rigid obstacle in the positions ``x`` of the nodes."""
ns = slave.shape[1]
nd = 3 * ns
for q in prange(face.shape[0]):
xs = np.empty((ns, 3))
for i in range(ns):
xs[i] = x[slave[face[q], i]]
f = np.empty(nd)
fresh = np.empty(STATE)
pressed = obstacle_point(ns, xs, xi[q], eta[q], weight[q], kind, geometry, par, old[q],
False, f, fresh) # fmt: skip
new[q] = fresh
closed = predict and _touched(par, old[q])
active[q] = pressed or closed
if not active[q]:
continue
fe[q] = f
if not tangent:
continue
step = 1e-7 * math.sqrt(
(xs[1, 0] - xs[0, 0]) ** 2 + (xs[1, 1] - xs[0, 1]) ** 2 + (xs[1, 2] - xs[0, 2]) ** 2
)
fp = np.empty(nd)
fm = np.empty(nd)
dummy = np.empty(STATE)
for j in range(nd):
node, c = j // 3, j % 3
keep = xs[node, c]
xs[node, c] = keep + step
obstacle_point(ns, xs, xi[q], eta[q], weight[q], kind, geometry, par, old[q],
closed, fp, dummy) # fmt: skip
xs[node, c] = keep - step
obstacle_point(ns, xs, xi[q], eta[q], weight[q], kind, geometry, par, old[q],
closed, fm, dummy) # fmt: skip
xs[node, c] = keep
for i in range(nd):
ke[q, i, j] = (fp[i] - fm[i]) / (2.0 * step)