trefny.net

LCP Formulations for Contact Solver Implementation

The following text is intented for those who decided to code their own LCP solver of resting contacts in rigid body simulations and who would appreciate having a brief extract of all the important information. To newcomers: without reading some of the references below the following won't make much sense.

Notation. pa is an arbitrary point on body A in world space, xa is the center of mass of body A in world space, ra denotes pa - xa, pa can be expressed as Rap0 + xa where p0 is an arbitrary point on body A in body space and Ra is the body's 3x3 rotation matrix. ni is normal at i-th contact and by convention it always points towards body A (first body reference in the contact data structure). All state variables are functions of time - i.e. Ra is actually Ra(t) - but this is omitted for the sake of concise writing and better understanding.

1 Acceleration-based LCP formulation for contact solving. See [1] for in-depth explanation. Relative acceleration at each contact point is expressed as:

(1)

and we want to find contact forces that meet the following constraints:

(2)
(3)
(4)
(2) means that the two bodies won't accelerate towards each other, (3) that the forces are repulsive, i.e. do not glue bodies together, and (4) mandates that a contact force becomes zero when the bodies break contact.

Because pa = pb at time t0, the relative acceleration at i-th contact is:

(5)

All forces acting on either body in the i-th contact can contribute to the relative acceleration and must be taken into account when ai1..ain are being solved for. bi can be computed directly from velocities and external forces/torques acting on the bodies in given contact. The linear and angular contributions from fj to the acceleration of body A at i-th contact point are:

The same can be written for body B. These four results combined and multiplied by fj give the acceleration contribution resulting from the force acting at j-th contact point. The yet unknown fj is thus multiplied by:

The parts of (1) unaffected by contact forces are kept in bi. Its linear and angular components for body A can be derived to be:

The same can be written for body B. All four components combined along with the remaining part of (5) gives us bi in the following way:

The only remaing part is the time derivative of contact normal. When the normal belongs to a face, the negative derivative is its cross product with the body's angular velocity, when the collision is edge-edge it's a little bit trickier and it involves a time derivative of a cross product of normalized edge directions. The derivation is in [3], with the following as the result (ea and eb are edge direction vectors):

In vector form, we can write:

(6)

and together with conditions (2), (3) and (4), we're looking for an equivalent LCP formulation. As shown in [2] they can be easily transformed into:

, and (7)

which is what is known as a linear complementarity problem (LCP). As A is symmetric and positive semidefinite (PSD) the solution always exists. It is not so in systems where static and dynamic friction forces are taken into account.

2 Velocity and impulse-based LCP formulation for contact solving. Let function C(x): Rn -> Rm describe m constraints placed on the n dimensional system. If C = 0 was satisfied at the previous time-step it is enough to have C' = Jx' = 0 in order to ensure that the constraints will be satisfied in the current time-step. To make Jx' equal to 0 we must add an unknown constraint impulse j, so the equation becomes:

(8)

J is the Jacobian (a matrix of gradients of each of m constraint functions), M-1 is a mass matrix that comprises the change in linear and angular velocity due to an impulse. If we want to model the restitution law, i.e., have the impact resolution as part of the LCP, we can assign the right hand side of the equation a vector c. This vector can also be used in some error reduction scheme that will probably be inevitable to implement anyway. So equation (8) becomes:

(9)

The problem with (9) is that we have m equations and only n variables. Because j must not bring energy to the system we may assume that j = JTλh, where λ is an unknown m-dimensional vector of Lagrange multipliers and h is the integration step. The current velocity has two elements: linear and angular velocity resulting from body's kinetic energy and velocity resulting from acceleration due to external forces. Also here the integration step plays its role and the rewritten equation looks like this:

Rearranging it we get:

(10)

Equation (10) is valid under the assumption that we'll use Euler integrator for evolving the dynamic and kinematic variables of the system. That can be disadvantage to acceleration-based LCP formulations, such as (6), because by using (10) we can't trade speed for accuracy by plugging in more precise integration module. The equation (10) can be written as:

and conditions (2), (3), and (4) can be written the same way in terms of λ and b. If we have a mixed LCP solver available, we can have the conditions (2-4) valid only for certain rows of A and the remaining rows can describe bilateral constraints such as joints with a simpler set of conditions (equality, unbounded λ, zero c, etc.). Thus we'll have the whole simulation system with many types of constraints evolved by a single call to the solver.

References:

[1] Baraff, D., An Introduction to Physically Based Modeling: Rigid Body Simulation II - Nonpenetration Constraints
[2] Baraff, D., Fact Contact Force Computation for Nonpenetrating Rigid Bodies
[3] Eberly, D., Game Physics, ISBN 1-55860-750-4

© 2002-4 Marek Trefny