Contact points

In Solfec-1.0 contact points are generated from volumetric intersections of convex primitive shapes. For example, in case of finite elements, a single contact point is generated as a result of an intersection of two elements. Once an intersection is calculated, a contact point is obtained as a centroid of the intersection surface; a contact normal is calculated by averaging normal directions of the constituent surfaces. This is illustrated in Figure Fig. 7.


Fig. 7 Contact points and normals obtained from volumetric intersections of convex primitives.

Calculating volumetric intersection

Intersection of two convex polyhedra is a convex polyhedron. The convex intersection algorithm, based on [1], is implemented in cvi.c and it can be summarized as follows. Let vectors

\[\begin{split}\mathbf{v}=\left[\begin{array}{c} \mathbf{v}_{1}\\ ...\\ \mathbf{v}_{n} \end{array}\right],\,\,\,\mathbf{p}=\left[\begin{array}{c} \mathbf{p}_{1}\\ ...\\ \mathbf{p}_{m} \end{array}\right]\end{split}\]

store vertices and face planes of a polyhedron. Vertices are made of triplets of coordinates

\[\begin{split}\mathbf{v}_{i}=\left[\begin{array}{c} v_{x}\\ v_{y}\\ v_{z} \end{array}\right]_{i}\end{split}\]

Planes are made of four components

\[\begin{split}\mathbf{p}_{i}=\left[\begin{array}{c} n_{x}\\ n_{y}\\ n_{z}\\ d \end{array}\right]_{i}\end{split}\]

The plane equation reads

\[n_{x}\cdot x+n_{y}\cdot y+n_{z}\cdot z+d=0\]

where \(\left[n_{x},n_{y},n_{z}\right]^{T}\) can be interpreted as a direction normal to the plane. The convex intersection takes as input

va - vertices of polyhedron 'a'
pa - face planes of polyhedron 'a'
vb, pb - vertices and planes of polyhedron 'b'

as seen in cvi.c:118. The algorithm begins by finding a distance between, and a pair closest points in ‘a’ and ‘b’

1  d,p,q = gjk(va, vb)
2  if d > 0 return NULL
3  else x = p

which maps to cvi.c:132. The GJK algorithm [3], implemented in gjk.c:342, is used. In case there is a positive separating distance between the input polyhedrons, null intersection is returned. For the intersection algorithm to work we need a point \(\mathbf{x}\), that is inside of both input polyhedrons. GJK will at best return a common point on the surfaces of both polyhedra. In the next step

4  x = refine_point(x)

point \(\mathbf{x}\) is refined by pushing it deeper inside of polyhedrons ‘a’ and ‘b’. This maps to cvi.c:136. Once \(\mathbf{x}\) has been refined, we transform the input plane definitions

5  transform(pa)
6  transform(pb)

so that \(\mathbf{x}\) acts as an origin of the coordinate system \(\left[0,0,0\right]^{T}\). In the course of this transformation we also scale the normal direction components so that \(d_{i}=-1\). This maps to cvi.c:143-159. In the next step

7  h = quickhull(normals(pa+pb))

we calculate the convex hull of the transformed normal directions of ‘a’ and ‘b’ (cvi.c:163). The quickhull algorithm [2], implemented in hul.c:555, is used. The result can be interpreted as a dual polyhedron of the intersection polyhedron of ‘a’ and ‘b’: for a convex polyhedron \(\left(\mathbf{v},\mathbf{p}\right)\), its dual (or polar) polyhedron is made by reinterpreting vertices as planes, and planes as vertices, as follows

\[\begin{split}\left[\begin{array}{c} v_{x}\\ v_{y}\\ v_{z} \end{array}\right]_{i}\rightarrow\left[\begin{array}{c} v_{x}\\ v_{y}\\ v_{z}\\ -1 \end{array}\right]_{i},\,\,\,l_{v}=\sqrt{v_{x}^{2}+v_{y}^{2}+v_{z}^{2}}\end{split}\]
\[\begin{split}\left[\begin{array}{c} n_{x}\\ n_{y}\\ n_{z}\\ -1 \end{array}\right]_{i}\rightarrow\left[\begin{array}{c} n_{x}\\ n_{y}\\ n_{z} \end{array}\right]_{i},\,\,\,l_{n}=\sqrt{n_{x}^{2}+n_{y}^{2}+n_{z}^{2}}\end{split}\]

where vertices at distance \(l_{v}\) from the origin become planes at distance \(1/l_{v}\) from the origin, while planes at distance \(1/l_{n}\) from the origin become vertices at distance \(l_{n}\). In order to obtain vertices of the intersection polyhedron we then calculate the polar set

8  v = polarize(h)

which maps to cvi.c:164 and is implemented in tri.c:351. The vertices \(\mathbf{v}\) of the dual intersection polyhedron are translated back, \(\mathbf{v}+\mathbf{x}\), into the input coordinate system, and triangulated

9  t = triangulate(v+x)

which maps to cvi.c:170-220. The triangulation is then returned

10 return t

at cvi.c:231.

Deriving contact points and normals

Contact points and contact normals are calculated based on the intersection surfaces obtained in the previous step. For various pairing of geometrical objects, calculation of contact points and contact normals is implemented in goc.c. For the pairing of two convex polyhedral surfaces contact detection is implemented in goc.c:detect_convex_convex, where fist at goc.c:272, triangulation of the intersection of the input surfaces is obtained, and next at goc.c:274 a contact point and contact normal are obtained out of this triangulation. Implementation of this calculation is found at goc.c:130 and it can be summarized as follows. Let \(\partial A\) and \(\partial B\) be the surfaces of the input polytopes \(A\) and \(B\). Let \(\left\{ t_{i}\right\}\) and \(\left\{ \mathbf{v}_{i}\right\}\) be the sets of triangles and vertices of the intersection surface of \(A\cap B\). Then

1 \(\,\,\) \(\mathbf{p}=\mathbf{0}\), \(\mathbf{n}=\mathbf{0}\), \(area = 0\)
2 \(\,\,\) for each \(t_{i}\in\left\{ t_{i}\right\}\) do
3 \(\,\,\,\,\,\,\) \(a = area \left(t_{i}\right)\), \(b = a^{2}\)
4 \(\,\,\,\,\,\,\) if \(t_{i}\in\partial A\) then \(\mathbf{n}=\mathbf{n}+b\cdot\text{normal}\left(t_{i}\right)\)
5 \(\,\,\,\,\,\,\) else \(\mathbf{n}=\mathbf{n}-b\cdot\text{normal}\left(t_{i}\right)\)
6 \(\,\,\,\,\,\,\) \(\mathbf{p}=\mathbf{p}+a\cdot\text{centroid}\left(t_{i}\right)\)
7 \(\,\,\,\,\,\,\) \(area=area+a\)
8 \(\,\,\,\,\,\,\) \(\mathbf{p}=\mathbf{p}/area\), \(\mathbf{n}=\mathbf{n}/\left\Vert \mathbf{n}\right\Vert\) , \(area=0.5\cdot area\)
9 \(\,\,\,\,\,\,\) if \(\mathbf{p}\) outside of \(A\) or \(B\) return NULL
10 \(\,\,\,\,\,\) \(spair_{0}=\text{nearest_surface_id}\left(\mathbf{p},\partial A\right)\)
11 \(\,\,\,\,\,\) \(spair_{1}=\text{nearest_surface_id}\left(\mathbf{p},\partial B\right)\)
12 \(\,\,\,\,\,\) \(gap=\underset{\mathbf{v}_{i}\in\left\{ \mathbf{v}_{i}\right\} }{\min}\mathbf{n}\cdot\mathbf{v}_{i}- \underset{\mathbf{v}_{i}\in\left\{ \mathbf{v}_{i}\right\} }{\max}\mathbf{n}\cdot\mathbf{v}_{i}\)
13 \(\,\,\,\,\,\) if \(\left|gap\right|\) seems too large
14 \(\,\,\,\,\,\,\,\,\,\) \(A^{\prime}=A+\mathbf{n}\cdot\left|gap\right|\), \(B^{\prime}=B-\mathbf{n}\cdot\left|gap\right|\)
15 \(\,\,\,\,\,\,\,\,\,\) \(gap=\min\left(\text{gjk}\left(A^{\prime},B^{\prime}\right)-2\left|gap\right|,0\right)\)
16 \(\,\,\) return \(\mathbf{p}\), \(\mathbf{n}\), \(area\), \(spair\), \(gap\)

Lines 1-8 above map to goc.c:141-158. Line 9 corresponds to goc.c:160 . Lines 10-11 map to goc.c:163-172 . Lines 12-15 map to goc.c:214-243. The extra check in line 13-15 is added to improve robustness of gap calculation. We note that, apart from the contact point, the contact normal, and the gap, we also calculate contact area, and surface pairing spair, storing identifiers of the input surfaces that are nearest to the contact point. In line 4, accumulated normal directions are scaled by square area of triangles, weighting down the influence of triangles with small areas. In line 9, we terminate in case the contact point fell outside of the input surfaces due to roundoff.

Contact sparsification

Contact geometries made of many individual convex objects often generate many contact points. Some of these contact points are ill–conditioned, in the sense that their corresponding contact normals do not necessarily represent a most natural direction of contact resolution. This frequently happens near corners or sharp edges, due to roundoff error. Also, for contact problems among bodies represented by simple kinematic models (e.g. rigid or pseudo-rigid) complex geometries may give rise to the number of contact points far exceeding the available kinematic freedom. This renders the \(\mathbf{W}\) matrix ill-conditioned, as already explained in the section on local dynamics. For the above reasons, a heuristic method of refining contact points, or sparsification, has been implemented in dom.c:sparsify_contacts. The result of application of this routine is seen in Fig. 8.


Fig. 8 Heuristic filtering of redundant contact points (736 to 168).

The sparsification approach can be summarized as follows. Let \(\left\{ c_{i}\right\}\) be a set of all contact points and let \(threshold\), \(minarea\) and \(mindist\) be given. Then

1 \(\,\,\) for all newly detected \(c_{i}\in\left\{ c_{i}\right\}\) do
2 \(\,\,\,\,\,\,\) if \(\text{area}\left(c_{i}\right)<minarea\) then \(\text{delete}\left(c_{i}\right)\)
3 \(\,\,\,\,\,\,\) for all \(c_{j}\in\text{adjacency}\left(c_{i}\right)\) do
4 \(\,\,\,\,\,\,\,\,\,\,\) if \(\text{area}\left(c_{i}\right)<threshold\cdot\text{area}\left(c_{j}\right)\) and
\(\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\) \(c_{i}\text{ and }c_{j}\text{ are topologically adjacent}\) then \(\text{delete}\left(c_{i}\right)\)
5 \(\,\,\,\,\,\,\,\,\,\,\) else if \(\left\Vert \mathbf{p}\left(c_{i}\right)-\mathbf{p}\left(c_{j}\right)\right\Vert <mindist\) then \(\text{delete}\left(c_{i}\right)\)

Contact points are “topologically adjacent” if they are generated by geometrical primitives which themselves are topologically adjacent (e.g. finite elements that share element faces). We note, that parameters \(threshold\), \(minarea\) and \(mindist\) can be adjusted by using the CONTACT_SPARSIFY input command.

Broad phase contact detection

Broad phase contact detection precedes the detailed pairwise checks, one of which is described above. During the broad phase we only intend to find a likely candidates for the detailed pairwise overlap tests and for this reason axis aligned bounding boxes are used to represent geometrical primitives. For example, each finite element is represented by a corresponding bounding box, and so are spheres and ellipsoids present in a simulation. A number of box overlap test algorithms are implemented, as seen in Fig. 9, where a test program, implemented in tst/boxtest.c, is shown. All these algorithms are detailed in thesis [5]. The driver interface for various box overlap algorithms is implemented in box.c and box.h. The hybrid algorithm [4] is currently used in Solfec-1.0 as a fixed choice, cf. dom.c:114. The box.c:AABB_Update routine is called inside of the time integration loop in dom.c:3595. When box overlaps are detected the callback dom.c:overal_create is invoked, from within which the goc.c:gobjcontact pairwise overlap detection routined is called. Should an overlap occur, an individual contact point is created as a result, in dom.c:384-402.


Fig. 9 Box test program illustrating various box overlap detection algorithms.

Geometric epsilon

It is important to note that GEOMETRIC_EPSILON, defined in alg.c:24, has significant effect on the behavior of most of the geometrical calculations in Solfec-1.0. For example, often points are regarded as coincident if they are closer than this value. The input command GEOMETRIC_EPSILON allows to change the default value of 1E-6. It is recommended to use about 0.0001 to 0.01 times the dimension of a smallest significant geometrical feature in a model.

Other implementation aspects

Test examples

can be used to improve understanding of the pairwise overlap test described above.

[1]D. E. Muller and F. P. Preparata, Finding the intersection of two convex polyhedra, Theoretical Computer Science, 7, 217-236, 1978.
[2]C. B. Barber, D. P. Dobkin, and H. Huhdanpaa, The Quickhull Algorithm for Convex Hulls, ACM Transactions on Mathematical Software, 22 (4), 469-483, 1996.
[3]E. G. Gilbert, and D. W. Johnson, and S. S. Keerthi, Fast procedure for computing the distance between complex bjects in three-dimensional space, IEEE journal of robotics and automation, 4 (2), 193-203, 1988.
[4]A. Zomorodian and H. Edelsbrunner, Fast software for box intersections, International Journal of Computational Geometry and Applications, 12 (1-2), 143-172, 2002.
[5]Koziara, PhD thesis, 2008.