Source code for sfepy.mechanics.contact_bodies
import numpy as nm
from sfepy.base.base import assert_, Struct
import sfepy.linalg as la
[docs]
class ContactPlane(Struct):
def __init__(self, anchor, normal, bounds):
Struct.__init__(self, anchor=nm.array(anchor, dtype=nm.float64),
bounds=nm.asarray(bounds, dtype=nm.float64))
self.normal = nm.asarray(normal, dtype=nm.float64)
norm = nm.linalg.norm
self.normal /= norm(self.normal)
e3 = [0.0, 0.0, 1.0]
dd = nm.dot(e3, self.normal)
rot_angle = nm.arccos(dd)
if nm.abs(rot_angle) < 1e-14:
mtx = nm.eye(3, dtype=nm.float64)
bounds2d = self.bounds[:, :2]
else:
rot_axis = nm.cross([0.0, 0.0, 1.0], self.normal)
mtx = la.make_axis_rotation_matrix(rot_axis, rot_angle)
mm = la.insert_strided_axis(mtx, 0, self.bounds.shape[0])
rbounds = la.dot_sequences(mm, self.bounds)
bounds2d = rbounds[:, :2]
assert_(nm.allclose(nm.dot(mtx, self.normal), e3,
rtol=0.0, atol=1e-12))
self.adotn = nm.dot(self.anchor, self.normal)
self.rot_angle = rot_angle
self.mtx = mtx
self.bounds2d = bounds2d
[docs]
def mask_points(self, points):
mm = la.insert_strided_axis(self.mtx, 0, points.shape[0])
points2d = la.dot_sequences(mm, points)[:, :2]
return la.flag_points_in_polygon2d(self.bounds2d, points2d)
[docs]
def get_distance(self, points):
dist = la.dot_sequences(points, self.normal) - self.adotn
return dist
[docs]
class ContactSphere(Struct):
def __init__(self, centre, radius):
self.centre = nm.asarray(centre)
self.radius = radius
[docs]
def mask_points(self, points, eps):
dist2 = la.norm_l2_along_axis(points - self.centre, squared=True)
radius2 = self.radius**2
mask = dist2 <= ((1 + eps)**2) * radius2
return mask
[docs]
def get_distance(self, points):
"""
Get the penetration distance and normals of points w.r.t. the sphere
surface.
Returns
-------
d : array
The penetration distance.
normals : array
The normals from the points to the sphere centre.
"""
vecs = self.centre - points
dist = la.norm_l2_along_axis(vecs)
# Prevent zero division.
ii = dist > 1e-8
normals = nm.where(ii[:, None], vecs[ii] / dist[ii][:, None],
vecs[ii])
return self.radius - dist, normals
def _get_derivatives(self, points):
vecs = self.centre - points
dist = la.norm_l2_along_axis(vecs)
# Distance derivative w.r.t. point coordinates.
dd = vecs / dist[:, None]
normals = dd
# Unit normal derivative w.r.t. point coordinates.
dim = points.shape[1]
ee = nm.eye(dim)[None, ...]
nnt = normals[..., None] * normals[..., None, :]
dn = - (ee - nnt) / dist[:, None, None]
return dd, dn
[docs]
def plot_polygon(ax, polygon):
from sfepy.postprocess.plot_dofs import _get_axes
dim = polygon.shape[1]
ax = _get_axes(ax, dim)
pp = nm.r_[polygon, polygon[:1]]
px, py = pp[:, 0], pp[:, 1]
if dim == 2:
ax.plot(px, py)
else:
pz = pp[:, 2]
ax.plot(px, py, pz)
return ax
[docs]
def plot_points(ax, points, marker, **kwargs):
from sfepy.postprocess.plot_dofs import _get_axes
dim = points.shape[1]
ax = _get_axes(ax, dim)
px, py = points[:, 0], points[:, 1]
if dim == 2:
ax.plot(px, py, marker, **kwargs)
else:
pz = points[:, 2]
ax.plot(px, py, pz, marker, **kwargs)
return ax
if __name__ == '__main__':
import matplotlib.pyplot as plt
# Test and draw the plane.
anchor = [1, 1, 1]
normal = [2, -1, 1]
bounds = [[-2, 0, 0],
[2, 1, 0],
[4, 3, 1],
[1, 3, 1],
[2, 2, 1]]
cp = ContactPlane(anchor, normal, bounds)
pps = 2 * nm.random.rand(20, 3)
mask = cp.mask_points(pps)
dist = cp.get_distance(pps)
v1, v2 = la.get_perpendiculars(cp.normal)
ax = plot_polygon(None, cp.bounds)
ax = plot_polygon(ax, nm.r_[cp.anchor[None, :],
cp.anchor[None, :] + cp.normal[None, :]])
ax = plot_polygon(ax, nm.r_[cp.anchor[None, :],
cp.anchor[None, :] + v1])
ax = plot_polygon(ax, nm.r_[cp.anchor[None, :],
cp.anchor[None, :] + v2])
ax = plot_points(ax, cp.anchor[None, :], 'r*')
ax = plot_points(ax, pps[mask], 'bs', ms=10, mec='None')
ax = plot_points(ax, pps[~mask], 'go', ms=10, mec='None')
mask = dist >= 0.0
ax = plot_points(ax, pps[mask], 'r^', mec='None')
ax = plot_points(ax, pps[~mask], 'kv', mec='None')
# Test and draw the sphere.
pps = nm.random.rand(5000, 3)
centre = [0, 0.5, 0.5]
radius = 0.8
cs = ContactSphere(centre, radius)
mask = cs.mask_points(pps, 0.0)
dist = cs.get_distance(pps)
ax = plot_points(None, cs.centre[None, :], 'b*', ms=30)
ax = plot_points(ax, pps[mask], 'kv')
ax = plot_points(ax, pps[~mask], 'r.')
plt.show()