We present a formulation of nonpenetration constraint between pairs of polytopes which accounts for all possible combinations of active contact between geometric features. This is the first formulation that exactly models the body geometries near points of potential contact, preventing interpenetration while not overconstraining body motions. Unlike many popular methods, ours does not wait for penetrations to occur as a way to identify which contact constraints to enforce. Nor do we overconstrain by representing the free space between pairs of bodies as convex, when it is in fact nonconvex. Instead, each contact constraint incorporates all feasible potential contacts in a way that represents the true geometry of the bodies. This ensures penetration-free, physically correct configurations at the end of each time step while allowing bodies to accurately traverse the free space surrounding other bodies. The new formulation improves accuracy, dramatically reduces the need for ad hoc corrections of constraint violations, and avoids many of the inevitable instabilities consequent of other contact models. Although the dynamics problem at each time step is larger, the inherent stability of our method means that much larger time steps can be taken without loss of physical fidelity. As will be seen, the results obtained with our method demonstrate the effective elimination of interpenetration, and as a result, correction-induced instabilities, in multibody simulations.
Introduction
There is a wide spectrum of applications for simulation, but an understandable bias toward real-time interactivity has bred a culture that leaves concerns of physical accuracy in a dark corner. As a result, applications which require some level of physical fidelity from simulation are left with a relatively underdeveloped set of tools. The major focus of the work presented herein is on improving the simulation accuracy, and consequently stability, without significantly increasing the computational cost.
The tradeoff of speed versus accuracy in simulation is a classic one, and many applications simply do not gain from improved physical accuracy. The gaming community is naturally concerned with methods that are fast and reliable, but not necessarily accurate [1–3], inspiring the term “game physics.” Many games, in fact, intentionally incorporate nonrealistic physics into their play. The graphics community, which is constantly pushing the boundaries of realistic-looking visual effects, does not offer many improvements to physically accurate simulation. Although there are many examples in a long history of “physics-based” methods in use in animation [4–8], there is really no need for such methods to do more than approximate some convenient alternative version of reality. Indeed, the goal in many graphics applications is to present the viewer with something that “feels” correct.
Particularly in the field of robotics, physical fidelity is a pressing concern and is especially challenging when simulating systems experiencing intermittent frictional contact, which is the case when robots work in unstructured, cluttered environments. Robotics experiments tend to be exceedingly costly and time consuming, so that the ability to perform realistic experiments in simulation would be a great benefit. Although work has been done regarding validation of simulation [9–12], the problem of measuring and quantifying the physical accuracy of simulation is an open issue that is challenging to solve. Comparison of a particular simulator's results with those obtained from a commercial simulator is meaningless without proof of the commercial simulator's accuracy. Even validation of a simulator with a physical experiment can be misinterpreted when it takes the tuning of several parameters to match a single or small number of experiments. Such an attempt at validation says little, if anything, of the simulator's ability to predict a different experiment.
Simulation can be a powerfully useful tool if shown to be accurate. A simulation requires accuracy in order to make any claims as to its ability to predict behavior in the physical world, yet the growing number of variants on old simulation methods all have at least this in common: they sacrifice physical fidelity at multiple stages in order to achieve computational speed-up. The ideal simulation tool would be both accurate and fast, but when we sacrifice accuracy, we are necessarily sacrificing the usefulness of that tool.
The polytopal exact geometry (PEG) contact model presented herein is a step toward improving the physical fidelity of simulations of polytopal rigid bodies in intermittent contact. In most popular simulation tools, in order to make it easy to formulate the nonpenetration constraints, the shapes of the bodies are effectively altered. Specifically, facets of polytopes are represented as hyperplanes (e.g., faces of polyhedra in 3D are represented as planes). As will be illustrated below, the price for this convenience is very high, as it leads to nonphysical simulation artifacts, such as huge constraint forces that eject bodies from the scene, or worse, cause the simulation's solvers to fail completely. By contrast, PEG exactly represents the boundaries of polytopal bodies, including their finite extent, and thus avoids these problems. To achieve this, the PEG contact model is more complicated than the standard contact model. With the same time-step sizes, simulation with PEG takes approximately twice as long as simulation with the standard contact model. However, its improved accuracy avoids nonphysical artifacts, including penetration-induced instabilities that can crash a simulation. In our opinion, this speed reduction is a penalty worth paying.
In the remainder of this section, we review multibody dynamics and some of the approaches used in the simulation. In Sec. 2, we detail the deficiencies of standard contact modeling practices and begin to derive the PEG contact model by introducing new approaches to cases that standard contact models fail to accurately represent. In Sec. 3, we define the general formulations of three classes of nonpenetration constraints and provide abstractions of them in order to model contact in a clean way. In Sec. 4, we precisely define contact constraint applicability and feasibility, and establish geometric tests to determine which contacts to include in PEG. In Sec. 5, we utilize the abstractions from Sec. 4 in order to define four fundamental contact constraints for spatial problems, which reduce to two when specialized to the planar case. We then demonstrate how to formulate a time-stepping subproblem that incorporates PEG. In Sec. 6, we compare the performance of multibody simulations using a popular time-stepping method with the standard contact model against the same time stepper with the PEG contact model. Finally, Sec. 7 discusses our results and possibilities for future work.
Previous Work.
The two main classes of simulation methods for multibody systems are known as event-driven and time-stepping. In event-driven methods, the simulation is monitored for impact and separation events as well as changes between sticking and slipping (see Ref. [13]). For systems with small numbers of contacts, this method is preferred, because it uses an adaptive step size to accurately pinpoint and process the contact state transitions. Its downfall is that in systems with many bodies and contact events, the size of the time step becomes so small that computation times become impractically large. On the other hand, this is precisely the circumstance in which time-stepping methods have their greatest value. Time-stepping methods use large time steps that can advance the simulation across a number of contact events in one step, such that at the end of the time step, Newton's laws are satisfied and bodies do not overlap [14].
Time-stepping simulation methods utilizing the complementarity problem (defined formally below) were introduced by Moreau and Panagiotopoulos [15]. In these methods, the data needed to advance the simulation by one time step are obtained as the solution of a complementarity problem, the so-called time-stepping subproblem. Stewart and Trinkle introduced a modification of this problem that incorporated stabilization of nonpenetration constraints [16], and they provided a proof that body trajectories approximated by the solutions to a sequence of these subproblems converged to a solution of the instantaneous-time formulation as the step size goes to zero [17]. Anitescu and Potra [18] included bilateral constraints and showed that the subproblem with linearized friction cones is always solvable.
Physics engines such as DART [19] use semi-implicit, velocity-based time-stepping subproblems, formulated as linear complementarity problems with nonpenetration constraint stabilization. However, because it uses standard contact modeling, it can suffer from inaccuracy and instability as discussed above. Chrono::Engine [20] is built on top of the Bullet collision detection engine [21], which reports penetration depth and gap distance, implying that penetration is not avoided. Across the gamut of available physics engines, simulations with bodies modeled as spheres or unions of spheres are popular for benchmarking, especially when there are large numbers of contacts. This is because when simulating polytopes in contact using the standard contact model, simulations can fail due to nonphysical, overconstraint imposed on the polygons. These overconstrained situations, henceforth referred to as standard-model traps (SM traps), can cause bodies to obtain huge nonphysical velocities or result in complete failure of the simulation.
Background.
In this section, we will introduce an instantaneous-time model of polyhedral bodies in intermittent contact, and then discretize it to obtain the discrete-time counterpart. To simulate the motions of the bodies, the discrete-time model (i.e., the time-stepping subproblem) must be formulated and solved repeatedly, each time advancing the simulation forward in time by the chosen time step. Note that the development herein excludes contact friction in order to maintain a sharp focus on the new contact model, which depends only on body geometry and not friction. Friction can, however, be easily incorporated by augmenting the time-stepping subproblem with one's favorite friction model, several of which are detailed in Refs. [22,23].
Before formulating PEG and a time-stepping subproblem that employs it, it is important to introduce the complementarity problem [24].
Complementarity.
The complementarity problem offers a convenient way to model contact because it naturally represents disjunctive conditions. The two key cases in multibody dynamics are: two bodies are in contact OR not in contact and an existing contact is sticking OR sliding. There is a vast body of research on complementarity problems and methods for their solution [25,26].
where represents the orthogonality of condition 3 in Eq. (1). It is common to refer to all three of these conditions jointly as a complementarity condition.
There are many general-purpose solvers available for complementarity problems [27] and in particular for linear complementarity problems (LCPs) [24,28]. In an LCP, is a linear function of , i.e., , where is a known constant square matrix, and is a known constant vector of compatible size. Much theory for LCPs has been developed and used to design reliable solvers. It is known, for example, that if is positive definite, then the LCP is equivalent to a convex quadratic program, and therefore has a unique solution and can be solved efficiently. If is positive semidefinite, then a solution can be found efficiently also, but some unknowns may not have unique solutions. In multibody dynamics, this case arises only for frictionless systems, and for these, the body motions have unique solutions, but the contact forces do not.
If friction is included in the model, these nice properties are no longer present. Time steppers using polyhedral friction cones still yield LCPs, but becomes indefinite, and correspondingly, the solutions are not unique; they might not exist and when they do, there can be infinitely many. In the special case when all contacts are sticking and Coulomb friction cones are approximated by four-sided pyramids, the solutions are known to exist [29]. However, the problems are more challenging to solve if the friction model is nonlinear, as is the case for quadratic Coulomb friction, where no existence results are available. Nonetheless, some solvers have success solving the nonlinear complementarity problems (NCPs) that result. We utilize the general-purpose path solver for many NCP-based simulations. Work has been done to create specialized solvers for multibody dynamics [30].
Instantaneous-Time Model.
Consider two bodies and as depicted in Fig. 1. When the signed gap distance ψn is positive (as in the figure), the bodies are separated. When ψn is zero or negative, they are in contact or interpenetrating. Ideally, the latter is not permitted, that is, . Generally, a contact between two bodies may be represented as a five-tuple , where and are body indices or identifiers for and , respectively, is the contact point on in world coordinates, and is the contact normal vector in the direction of onto by convention. The point of contact on is available by . The vectors and span from the center of mass of each body to the point of contact on that body. While not required, we use the convention that the origin of a body's body-fixed reference frame is coincident with its center of mass. Vectors and form an orthonormal basis with .
where is the vector from the center of mass of body i to the point of application of the force due to the jth contact as in Fig. 1 above. The generalized body configurations are represented by for position and unit quaternion q [26].
for a contact where is the signed gap distance which is dependent on the configuration of the bodies and time t, and λn is the magnitude of the force applied in the normal direction of the contact. Just as we will do for and below, we drop the functional dependence to simplify notation.
The orthogonality condition of Eq. (4) enforces for each contact that if the force being applied is positive (compressive), then the gap distance must be zero, and if the gap distance is positive, then the force must be zero. The inequalities imply that the force cannot be tensile and the bodies cannot overlap. In the frictionless case, the continuous-time dynamic model of rigid bodies in contact is composed of Eqs. (3) and (4).
Discrete-Time Model.
where denotes an impulse over the time step, i.e., .
Equations (8) and (9) define the time-stepping subproblem of Stewart and Trinkle [16] without friction. The first row of Eq. (8) is the discrete-time Newton–Euler equation. The second row is merely a definition used to shorten the statement of the normal complementarity condition (9). Importantly, Eq. (9) defines the standard model of unilateral contact used widely in multibody simulators. The term appearing in the standard model acts as a constraint stabilization term, which if removed, allows interpenetrations to occur and then remain indefinitely. Regardless of whether term is removed or retained, the inequality on the left-hand side of condition (9) is the source of SM traps that we will see PEG eliminates.
State Update.
Since Eq. (10) does not preserve length, each quaternion should be renormalized after each time step.
A New Approach
One approach to multibody simulation is to allow interpenetrations (and other constraint violations) to occur and then apply whatever hacks are necessary to catch poorly defined geometric configurations that inevitably result. Our approach attempts to prevent nonphysical configurations from occurring. In order to achieve this, we must detect potential contacts before interpenetration occurs.
Contact constraints have been traditionally modeled as fundamentally unilateral, however, representing features as infinite half-spaces at points of potential contact generates nonphysical behavior at corners where multiple finite features meet and terminate. Consider a vertex v near a corner as depicted in Fig. 2. When the collision detection routine identifies both and , the vertex becomes erroneously trapped in the convex subspace of the free space surrounding the corner if both contacts are enforced. We refer to this as the standard-model trap (SM trap).

The standard-model trap. A vertex v approaching two edges e1 and e2 at a corner of a body . If unilateral constraints are enforced against both edges, the vertex becomes trapped by the sum of the edges' half-spaces (light gray regions plus body). The same trap can occur in 3D between a vertex and set of faces or an edge against multiple edges.

The standard-model trap. A vertex v approaching two edges e1 and e2 at a corner of a body . If unilateral constraints are enforced against both edges, the vertex becomes trapped by the sum of the edges' half-spaces (light gray regions plus body). The same trap can occur in 3D between a vertex and set of faces or an edge against multiple edges.
Figure 3 shows a simple situation in which two SM traps collude to generate a time-stepping subproblem with no solution. A common heuristic in collision detection is to choose contacts with deepest penetration. These contacts are then used to formulate a dynamics problem to move the simulation forward in time. Since the standard contact model requires that by the end of the next time step, both of the identified contacts must be non-negative, an impossible problem is generated. Poor contact choices lead to poorly defined dynamics problems, resulting in the all too familiar “popping” or “exploding” bodies in a simulation. Note that allowing bodies such as those in Fig. 3 to first interpenetrate before identifying contacts does not avoid this erroneous contact problem, as the same heuristic results in similarly poorly defined dynamics. Fundamental errors such as these are what lead to common ad hoc “corrections” to these problems, such as limiting the maximum magnitudes of contact forces.

Infeasibility caused by the standard contact model. Vertex is penetrating the line containing edge (i.e., ). Similarly, is penetrating the line containing edge (i.e., ). Since the bodies are rigid, it is impossible to eliminate both penetrations at the end of the next time step. Therefore, the time-stepping subproblem with constraint stabilization will not have a solution.

Infeasibility caused by the standard contact model. Vertex is penetrating the line containing edge (i.e., ). Similarly, is penetrating the line containing edge (i.e., ). Since the bodies are rigid, it is impossible to eliminate both penetrations at the end of the next time step. Therefore, the time-stepping subproblem with constraint stabilization will not have a solution.
Deriving PEG, the New Model.
allowing v to penetrate either half-space corresponding to e1 or e2, but not both.
Lemma 1.1. Given .
Proof.
Case .
Case .◻
Lemma 1.2.Given.
Proof.
Case : .
Case a > 0: .
Case : .
Case : .◻
To give some insight into the slack variable c and its relationship with ψ1 and ψ2, consider the figure below (Fig. 4) and the value of c on each side of the bisector where . A vertex in the free space right of the bisector has , which requires c = 0 to satisfy the complementarity condition of Eq. (12). A vertex to the left of the bisector has , which requires .
We now wish to include potential forces λ1 and λ2 while ensuring that only one is nonzero since allowing both to be simultaneously nonzero would be nonphysical.
Lemma 1.3.Given.
Proof.
Case : .
Case a > 0: .
Case : .
Case : .◻
Lemma 1.4. Given.
Proof. The only way to satisfy both and is for both to reach equality. When , it is possible that , so in order to have , we must have . The argument for the reverse of a and b is similar.◻
Essentially, we have taken a set of contacts, in this case C1 and C2, and replaced a logical AND condition of with a NAND condition of . Geometrically, this corresponds to replacing line representations of a body with line segments. At this point, Eqs. (12)–(14) correspond to the locally nonconvex (LNC) model proposed by Nguyen [32], which models the case for a single vertex against multiple edges. Although this model has been shown to contribute some improvement in physical fidelity over other methods [33], stopping here would correct the standard-model trap at the expense of permitting interpenetrations [34,35]. To illustrate this error, let us continue with a less trivial case.
Consider two bodies approaching as depicted in Fig. 5. In this configuration, our collision detection routine identifies four potential contacts. These are , , , and . We should not simply constrain all of these contacts, for this would result in a dual standard-model trap. It is possible to make a guess as to which contacts to enforce, but this inserts the likely possibility of generating contact forces where there are none. Of course, we could also ignore all four contacts and allow interpenetration, but this too is clearly a nonphysical approach and leads to instabilities.

A dual case of a vertex near two edges in 2D. The physically feasible trajectories of va are interdependent with the trajectories of vb and vice versa. Consider that if va goes “on top” of , then vb cannot simultaneously go “on top” of .
but we make the important observation that this model erroneously allows for ψ1 and ψ4 or ψ2 and ψ3 to be simultaneously violated. An example of such a consequence is depicted in Fig. 6. The model of Eq. (15) can be completed by including constraints to prevent such interpenetrations. That is, we additionally constrain and with

A valid solution to the dual vertex–edge case using the LNC model. Note that neither vertex is penetrating the other body, perfectly satisfying the constraints of Eq. (15).

A valid solution to the dual vertex–edge case using the LNC model. Note that neither vertex is penetrating the other body, perfectly satisfying the constraints of Eq. (15).
Equations (15) and (16) together represent our first example of a geometrically accurate contact model. We will refer to the constraint type in Eq. (15) as intercontact constraint and the type in Eq. (16) as cross-contact constraint. These, along with unilateral constraint, form the set of three contact constraints necessary for our model.
Three Fundamental Contact Constraints
In order to resolve the issues discussed in Sec. 2, we had to change the way we formulate contact constraints. In this section, we define three fundamental contact constraints in their general form and begin to describe how they can be applied.
Unilateral Contact Constraint.
where λi is the force corresponding to contact Ci.
Intercontact Constraint.
An intercontact constraint ( -constraint) is that which was demonstrated in Eq. (15) for preventing the standard-model trap. An -constraint deals with contacts of a single feature of one body against multiple features of another body. Here, we will define the general formulation of -constraint for a vertex of against multiple features of where a subset of these potential contacts may feasibly result in a contact force at the end of the current time step.
Consider two sets of contacts: which is composed of all contacts which could feasibly result in a contact force, and which contains all contacts that could not feasibly result in a contact force but are necessary to accurately represent the contact geometry. An -constraint seeks to
constrain at least one gap distance of to be non-negative
permit a nonzero contact force for at most one contact in
only permit when and all other are nonpositive
Equations (20) and (21) enforce that at least one contact in is non-negative. Equations (22) and (23) allow at most one contact force to be nonzero, and only permit this force to be nonzero when all other gap distances are nonpositive. Solving such a set of constraints is possible as a linear program with complementarity constraints (LPCCs) [36], or by introducing an auxiliary variable to convert inequalities (21) into a linear complementarity conditions before using an LCP solver [32].
Cross-Contact Constraint.
In contrast to -constraint which deals with a single feature of against multiple features of , a cross-contact constraint ( -constraint) deals with sets of contacts that may not depend on the same features, but could result in interpenetration if simultaneously violated, e.g., Eq. (16).
Given the sets of contacts and , a -constraint attempts to constrain either
at least one non-negative ψa in
OR
at least one non-negative ψb in
and can write the LCP in multiple forms. Conveniently, -constraints can be written in the form of Eqs. (20) and (21). That is, is similar to , but excludes Eqs. (22) and (23) which involve contact forces.
Although a -constraint necessarily deals with contacts between the same pair of bodies, the contacts need not share features. That is, the geometric interdependencies of identified potential contacts, regarding which are to be enforced as defined by an -constraint, may be between two otherwise seemingly independent contacts, e.g., opposite corners of the face of a box. Subsequently, it is conceivable for two potential contacts, neither of which have a potential force associated with them, i.e., contacts in two different sets of , to be -constrained. In such cases, the dynamics problem requires sufficient - or -constraints so as to allow generation of forces which may contribute to achieving configurations that will satisfy all constraints. This issue, however, while an important one when employing a heuristic approach to choosing primary contacts in -constraints, is beyond the scope of this paper.
Contact Between Moving Geometries
In this section, we briefly discuss the nature of interpolytope contact and define a set of geometric tests that are useful for identifying and making heuristic choices regarding potential contacts.
We define a convex polyhedron in terms of features (Fig. 7). We may refer to a vertex either as an object v or as the vector representing the vertex's position. Similarly, an edge object corresponds to a vector . A body is composed of a set of vertices V, edges E, and faces F defined by three vertices in counterclockwise order. The outward pointing normal vector for a given face is perpendicular to f. We define two vectors and for each edge that are planar with f1 and f2, respectively, and perpendicular to . These vectors are defined as
for convex bodies.
As we develop these tests, we keep in mind that a contact can occur anywhere on a polytope, but the possible normal vectors are restricted by the contacted feature. Figures 8–12 depict the possible positions and orientations of contact normal vectors for each feature in two and three dimensions. Since a vertex does not have a uniquely defined normal, we use the normal from the edges that connected to the vertex. It is important to realize that when using a time-stepping method, we hope to identify contacts which will reflect these feasible contact normals at the end of the time step. Yet, many methods of contact identification use approaches that assume continuous or static body positions. This is why we geometrically relax the following set of tests that will prove useful for identifying and making heuristic choices regarding potential contacts.

Two-dimensional normal region of an edge. Contact anywhere between the vertices of e1 results in the same normal direction .

Two-dimensional normal wedge at a vertex. A contact at this vertex may result in a contact normal anywhere in this wedge.

Three-dimensional normal region for a face. The simplest of the three 3D normal regions, it is defined by a single half-space. The Voronoi region (depicted) is a subspace of that half-space.

Three-dimensional normal region at an edge. Again, the Voronoi region (pictured) is a subset of the normal region.

Three-dimensional normal region at a vertex joining three faces. For the general case of m joined faces, the polyhedral region will be defined by m half-spaces planar with the faces and is equivalent to the Voronoi region for the vertex.
Consider the simple example in Fig. 13 where the two vertices, and , of body are near an edge eb of body . In order to achieve stability in such a configuration, it is clear that both potential contacts and must be considered by the dynamics formulation. If only one were included, e.g., C1, then it is possible for the other to penetrate eb given a large enough time step or velocity. We observe that the second contact normal n2 is outside of the normal cone region of and does not correspond to a physically reasonable contact normal. However, this is because n2 is determined at the beginning of the time step; yet, we wish to prevent interpenetration for the end of the time step. In order to avoid interpenetrations, we must detect and consider such contacts as C2, even when they are not physically feasible at the current time step. This necessity requires that we geometrically relax the normal cone region or, equivalently, relax the definition of applicability.

Two vertices of body near an edge of . Vertex has classical applicability with eb and the normal n1 of is within the normal cone. Vertex does not have classical applicability with eb and the normal n2 of is outside its corresponding normal cone.
Contact Applicability.
The time required to solve a time-stepping subproblem, and therefore the time required to compute a simulation, is strongly dependent on the number of contacts nc. While it may seem that the number of contacts is determined by the simulation, taking a close look at an individual discrete time step, as we do in Sec. 1.2.3, reveals that to enhance simulation accuracy and reduce instability, it is beneficial to include “potential contacts” even though they increase the problem size. We slightly modify classical applicability conditions to derive a method to choose a small set of extra contacts.
where is the outward unit normal of eb, and the superscript ve denotes that the test is for a vertex and an edge.
Notice that the applicability test is dependent on ; more specifically, it is dependent on the relative orientation of the polygons, and not their relative translation. Later, when considering potential contacts to include in a time-stepping subproblem, we will introduce a relaxed form of inequality (28) to allow for possible rotation in a time step. In the case of our example, if the top polygon rotated enough in the clockwise direction in the time-step under consideration, contact could occur first at vertex rather than vertex .
Relaxation of 2D Applicability.
Whereas classical applicability is a Boolean function, our function returns a value as we may wish to use the value of applicability as a heuristic when comparing contacts.
Relaxation of 3D Applicability.
Figure 14 depicts two edges, ea and eb, in contact. Just as there are vertex–face configurations that have proximity but could not result in a force, so too are there edge–edge configurations that should not be associated with potential contact forces. First, we should note that given an edge–edge contact normal , we cannot immediately say if we have the correct sign for . However, we can still determine edge–edge applicability in the following manner. Let

Edge–edge contact at the border of stability for classical applicability (). The result in this configuration is dependent on machine precision error. Relaxation eliminates this dependency by increasing the domain of applicability.
Contact Feasibility.
Feasibility is similar to applicability, but is dependent on feature positions as opposed to orientations. The region of feasibility is essentially the region in which we anticipate a contact between a vertex v and a facet f could occur. We define vertex–edge and vertex–face feasibility similarly. Given an angle and a small value δ greater than machine precision, a vertex with contact has feasibility if

The region of feasibility for a vertex against a facet f of a body is the region above the thick dashed line. A vertex in this region is considered to have a potential contact force λ with f.
We can also write an edge–edge feasibility in 3D, which checks only to see if the nearest two points pa on ea and pb on eb are within some small distance .
Polytopal Exact Geometry
From the set of fundamental constraints presented in Sec. 3, which are written in terms of complementarity problems, abstract out another step and look at pairs of bodies by cases of feature pairs.
This abstraction layering is depicted in Fig. 16. At the lowest level is the complementarity problem which is utilized first as a model of unilateral contact with force, but further as a tool for generating logical NAND conditions on subsets of potential contacts. Sets of these low-level constraints can be entirely represented by abstracting to the three fundamental constraints of unilateral, intercontact, and cross-contact. In turn, the fundamental constraints are formulaically generated by the highest abstraction layer which looks only at what pairs of features between two bodies are near each other. In Sec. 5.2, we will see how precisely the same abstraction can be developed in 3D.
PEG 2D.
We consider two contact configurations in 2D: vertex–edge and vertex–vertex. We do not consider edge–edge because this case is redundantly covered by the other two configurations [38]. To be clear, edge–edge penetration in 2D is impossible if vertex–edge and vertex–vertex constraints are properly enforced, simply because edges defined and composed of vertices, and for two edges to come near each other implies that the vertices of at least one of those edges are nearing the other edge. We can formulaically generate the set of constraints for the two 2D configurations as given below.
Vertex–Edge 2D.
In this case, the vertex is within a reasonably small distance of the edge, but farther than from either of the edge's vertices.
Vertex–Vertex 2D.
where contains at least one of C1 or C2, may contain one of C1 or C2, and similarly contains at least one of C3 or C4, and may contain one of C3 or C4. As is the case for all -constraints, in which contacts are included in or these are determined using the geometric heuristics defined in Sec. 4.
PEG 3D.
There are four contact configurations in 3D: vertex–face, edge–edge, vertex–edge, and vertex–vertex. Again, we exclude certain feature pairs just as we did in 2D, namely, edge–face and face–face, because they become redundant when the other configurations are properly enforced. Edge–face is redundant since edges are composed of vertices, and for an edge to approach a face requires that either the vertices of that edge approach that face, or the edge approaches the edges at the perimeter of that face (possibly adjacent edges), reducing to either vertex–face or edge–edge, respectively, and edge–vertex in the case that the edges of the second body are adjacent. By a similar argument, face–face becomes redundant; a face, being composed of edges in turn composed of vertices, nearing another face implies that edges and vertices are approaching the other face. In all possible configurations, this results in one of the four feature combinations considered.
Vertex–Face 3D.
The vertex–face case is analogous to the 2D vertex–edge case. Given a vertex va near a face fb as depicted in Fig. 17, we include

A well-defined vertex–face contact between vertex va of body and face fb of body . is clearly the only contact to consider given a reasonable time-step size.
in the set of constraints.
Edge–Edge 3D.
Vertex–Edge 3D.
Consider Fig. 18 where a vertex va of body approaches the edge eb of body . There are an arbitrary number of edges (greater or equal to three) that could connect to va; however, we can limit the edge–edge contacts considered to a subset of those with relatively large magnitudes of edge orientation . Given and , the set of constraints for vertex–edge is given by

An example of a vertex–edge configuration. Consider the relationship between the contacts and . If the first is not enforced, then the second must be and vice versa.
where C1 and C2 are distributed appropriately into and . Each Cei is an edge with positive edge orientation and each Cej is an edge with negative edge orientation, and both sets of edges have applicability and feasibility with eb.
Vertex–Vertex 3D.
In the case of a vertex va near another body's vertex vb as depicted in Fig. 19, we wish to avoid trapping va against the faces of , and similarly for vb against . This first requires the set of -constraints

A vertex va of body approaching a vertex vb of body from above. Vertex va should be permitted to break the half-spaces represented by the falsely extended faces of , but not all of them and only when doing so does not violate edge–edge contacts.
where fa has edges connected to va, and fb has edges and connected to vb. These -constraints are enforced only for edges ear and ebs, which have edge–edge applicability and feasibility. The constraint in Eq. (38) is similar to the -constraints in Eq. (36) for the vertex–edge case, but permits a vertex to pass on either side of a face instead of either side of an edge.
Time-Stepping Formulation With PEG.
Results
The experiments discussed below compare simulations performed with the time-stepping methods defined by Eqs. (8) and (9) and Eqs. (48) and (49) in 2D and 3D. The only difference in the time steppers was that one used the standard contact model and the other used PEG. Both methods retained the term for constraint stabilization, and the time-stepping subproblems were solved by path [27]. For simulations with PEG, a single primary potential contact was chosen for each set of potential contacts, which yielded the least accurate form of PEG. Despite this, the results show significant accuracy improvements when using PEG. The examples are of convex polytopes poured into nonconvex containers represented as unions of convex polytopes.
Box of Polygons.
Figure 20 shows a snapshot from the 2D experiment, which consisted of 160 simulations (ten different initial configurations for 16 different time steps). In each experiment, 20 randomly generated convex polygons were “poured” into an open box fixed in space (the same initial conditions were used for PEG as for the standard model). One polygon was dropped every 0.25 s until the simulation ended at 5.0 s. At the end of each time step, the area of overlap among all polygons was computed and used as a metric of accuracy. When simulating the standard model, the simulation frequently crashed prior to reaching 5 s of simulation time, due to SM traps that caused the solver to fail. Typically prior to failure with the standard model, configurations were reached that subsequently allowed interpenetration or nonphysically large body velocities. Pivoting solvers, such as Lemke's algorithm [24], demonstrated similar difficulties in finding physically sensible solutions in SM trap situations. By contrast, time-stepping with PEG never encountered such problems since interpenetration and overconstraining were avoided.

A snapshot from a polygon pouring simulation. A polygon was dropped into a box every 0.25 s for 5 s of simulation. At each time step, the total area of overlap between all pairs of bodies was used as the performance metric.
Figure 21 shows the median and the first and third quartiles of area of overlap error for increasing time-step size. Even for small time steps, the time stepper using PEG produced errors several orders of magnitudes smaller than the time stepper using the standard contact model. As the error with the standard model grows with the step size, the error in PEG remains imperceptibly close to machine precision.

Comparison of area overlap errors from ten polygon pouring simulations. Predictably, time-stepping with the standard contact model generated more error for larger time steps. The error when using PEG was relatively imperceptible, set along the horizontal axis for all time steps tested.
An interesting statistic to observe is the rate of occurrence of SM traps. In other words, how frequently was PEG necessary to achieve a geometrically accurate interaction between bodies. Figure 22 depicts the number of SM traps encountered at each time step for the 2D experiment. The top curve represents the total number of contacts, and the curve below it shows the number of SM traps. It shows that PEG was necessary in approximately 25% of time steps. One should realize that as the size of the time step increases, so would the percentage of SM traps.

Numbers of contacts and SM traps encountered at each time step for a typical polygon pouring simulation with time step of s. Smaller time steps generated fewer SM traps since the contact proximity parameter was smaller.
Box of Polyhedra.
As shown in Fig. 23, we extend the polygon pouring experiment to 3D. In the 3D simulations, the random polygons were replaced by convex polyhedra, the box became the union of five right hexahedra, and volumes of overlap replaced the error metric used in the 2D simulations. Again, there were ten sets of different initial conditions for each of 16 time-step sizes, and these sets were each used for both PEG and the standard model. A polyhedron was dropped into the box every 0.5 s until 5.0 s of simulated time was reached.

Sample frame from a polyhedra pouring simulation. A polyhedron was dropped into a container every 0.5 s for 5 s of simulation. At each time step, the total volume of overlap between all pairs of bodies was used as a performance metric.
The constraint violation error defined as volume overlap is depicted in Fig. 24, which has extremely similar behavior to the 2D results. As the step size increases, the penetrations between polyhedra grow steadily larger with the standard contact model, whereas PEG virtually eliminates this error at all time steps.

Comparison of volume overlap error for ten instances of the polyhedron pouring experiment. Time-stepping with the standard contact model generated more error for larger time steps, while the error when using PEG was relatively imperceptible, set along the horizontal axis for all time steps tested.
The occurrence of SM traps is shown in Fig. 25, where we see a rate of occurrence similar to the 2D experiment. Although the shape similarity is intriguing, it is simply coincidence, as these results are only representative of a single experiment and time-step size for both 2D and 3D.

Numbers of contacts and SM traps encountered at each time step for one polyhedron pouring simulation with time step of h = 0.01 s. Smaller time steps generated fewer SM traps since the contact proximity parameter was smaller.
Conclusions and Future Work
There have been previous good results regarding physical fidelity and stability of differential variational inequality (DVI) methods [10,12,42] upon which we have built our contact model. PEG further improves stability by avoiding interpenetrations, which can generate nonphysical behavior in ways that are difficult to predict and correct. PEG is also extendable to nonconvex bodies simply by subdividing bodies into convex parts [23]. Although PEG results in significantly smaller error while maintaining dynamic stability, there is still some increase in error for any time-stepping method as the time step increases. This is due to the fact that the time-stepping approach solves for impulses at the end of the time step based on normal directions determined at the beginning of the time step. As such, body rotations may generate unpredicted interpenetrations at the end of the time step, which are exacerbated given larger time steps.
In future work, we plan to explore efficiency and accuracy tradeoffs associated with the design of heuristics used to define the set of active contacts, the size of time steps, and the number of primary potential contacts. When simulation speed is the main goal, it is desirable to minimize the size of the active set and the number of primary potential contacts. However, when high accuracy trumps speed, then multiple primary potential contacts per contact should be used and the active set would be larger. The pursuit of this work will require a concomitant effort in the development of efficient, large-scale solvers of linear programs with complementarity constraints.
Acknowledgment
This work was partially supported by the National Science Foundation through the National Robotics Initiative NSF CCF-1208468, DARPA W15P7T-12-1-0002, and Lockheed-Martin Advanced Technology Laboratory.