Algorithmic and Architectural Gaming Design: Implementation and Development Practical Introduction to Rigid Body LCP (Linear Complementary Problem) Constraint Solvers

Algorithmic and Architectural Gaming Design:
Implementation and Development
ISBN 978-1-4666-1634-9
p159-2
02

Chapter 8

Practical Introduction to
Rigid Body Linear
Complementary Problem
(LCP) Constraint Solvers

Ben Kenwright
Newcastle University, UK
b.kenwright@ncl.ac.uk

Graham Morgan
Newcastle University, UK
graham.morgan@ncl.ac.uk






ABSTRACT
This chapter introduces Linear Complementary Problem (LCP) Solvers as a method for implementing
real-time physics for games. We explain principles and algorithms with practical examples and
reasoning. When first investigating and writing a solver, one can easily become overwhelmed by the
number of different methods and lack of implementation details, so this chapter will demonstrate the
various methods from a practical point of view rather than a theoretical one; using code samples and real
test cases to help understanding.

INTRODUCTION
With the ever-increasing visual realism in today's computer-generated scenes, it should come as no shock
that people also expect the scene to move and react no less realistically. With the computational power
available today, the ability to run physically accurate real-time simulations is required to hold a players
attention.

Simulating scenes, using physics-based methods, is important because it enables us to produce
environments that respond to unpredictable actions and simulate situations that are indistinguishable from
real life, e.g. buildings collapsing. However, it is very difficult to simulate reliably, large number of
objects and complex articulated structures as shown in Figure 1.


DOI: 10.4018/978-1-4666-1634-9.ch008


Copyright (c) 2012, IGI Global. Copying or distributing in print or electronic forms without written permission of IGI Global is prohibited.

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers


Figure 1. Simulation screenshots demonstrate stable stacking (left), articulated joints for characters
(middle) and chains of objects (right).

Writing a flexible, scalable rigid body simulator is a challenging task because you need strong
background knowledge in programming and Newtonian mechanics. While there are several approaches
(i.e. penalty methods, impulse methods), solvers offer numerous advantages, such as requiring less user
tuning and the ability to handle highly coupled configurations (e.g. large stacks or chains).

The reader, after being introduced to how solvers operate and how they are constructed, is introduced to
dependent techniques; such as sparse matrices. And while we try and explain everything from the bottom-
up, it is still required that the reader is at least familiar with basic Newton's laws and calculus techniques.
After completing this chapter, the reader should have a basic understanding of what a solver is, how to
implement one, and how to use it.


BACKGROUND
Rigid body dynamics
Rigid body dynamics is a well understood and documented field, and as such, will not be covered here.
For background information, we recommend reading (Baraff, 1999; David H. Eberly, 2004; Hecker,
1998), which gives details on unconstrained dynamics and concepts such as body mass, acceleration,
velocity and the equations of motion, which we use throughout this chapter.

While we introduce the reader to writing a practical LCP solver, there are also commercial and open
source engines, which can be taken advantage of, and we would recommend them for the purposes of
background knowledge. Some well-known LCP simulation engines are:

* Open Dynamics Engine (ODE), (Smith, 2004)
* PhysX (NVIDIA, 2011)
* Newton Game Dynamics (Jerez & Suero, 2003)
* Havok (Havok, 1998)
* Crisis Physics Engine (Vondrak, 2006).

Block matrix methods
The equations that make up our dynamic system and constraints can be large, cumbersome, error-prone
and difficult to manage, and to help alleviate this problem, we represent them in matrix form. This gives
us a more manageable high-level view of the system and its components, which is more intuitive to work
with.

In our simulator, a large majority of the computation cost is in calculating an inverse matrix for our
solution. As with real numbers, when you have `ax=b' you can solve for `x' by dividing both sides by `a'

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

to get `x=b/a', which is acceptable, as long as `a' is not zero. Similarly, when working with matrices, we
formulate the equation `AX=B', and divide both sides by `A' to get `X=B/A'. But instead of dividing
both sides by `A', we calculate the inverse of `A' (i.e.
1
A- ) and multiply both sides, to achieve the same
result.

This chapter uses numerical methods to calculate the inverse of the matrix, which is central to achieving a
usable solver. However, on occasions, we are unable to calculate the inverse, i.e. we get a singular matrix,
similar to a divide by zero with real numbers. When this occurs, usually some ill-conditioned
configuration has developed or perhaps some numerical problem has occurred, which we detect as a
singular matrix (i.e. determinant is zero) and we determine why it has arisen so that we can fix it and
prevent it happening again.


Lagrange Multiplier Formulation
We have the unconstrained dynamic equations of motion from classical mechanics, which describes how
the rigid bodies move, and a set of constraint conditions - which describe how they cannot move. We
then combine these two equations and solve the unknowns by using a powerful technique of multivariable
calculus, known as `Lagrange multipliers'.

We create the equations for our system using Newton's second law `f = ma', in combination with
constraint equations which we form through differentiation and substitution to establish a combined
problem, which is solved using Lagrange's multiplier methods.


Equations of Motion
Each unconstrained body has six Degrees of Freedom (DOF), three for translation and three rotation. For
a group of rigid bodies (m), the total DOFs is 6m. Since constraints restrict the relative motion, the total
number of DOFs of a group of rigid bodies, with constraints, is less than 6m. The rigid body dynamics in
collaboration with the constraint configurations form the Equations of Motion (EOM), that describe how
the group of rigid bodies will move as time changes.

We can categorise the EOM into two types: Maximal and Reduced coordinates.

Maximal coordinates use Cartesian space, so each body has 6m state variables, and requires 6m-n
constraint equations (where n represents the number of constraints). These constraints explicitly remove
extraneous DOF through their formulation. For further reading on explicit constraint methods see
(Shabana, 1994), and further details on maximal coordinate methods are available in (Baraff, 1999; David
H. Eberly, 2004; Hecker, 1998).

Reduced coordinates use an implicit incorporation of the constraints to formulate the equations of motion.
The system uses n state variables to represent the various DOF, so for example, if we have a single object
which can only rotate around the `y' axis (no translation or x-z rotation), then the system state only needs
a single state variable to represent the system (i.e. the object's angle). It has a major drawback, whereby
for each unique configuration we need to derive by hand a set of equations for that particular
arrangement.

Both Maximal and Reduced coordinate methods are able to run in `O(n)' time. Maximal coordinates are
more popular because of their modularity and straightforwardness to understand and implement.
Although maximal coordinates operate in Cartesian space, we still sometimes need to use awkward
conversions, to convert between constraint and Cartesian space. Maximal coordinates can drift due to

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

numerical errors and integration inaccuracies, in addition they need a minimum of 6m state variables to
represent the system, so optimised methods need to be used to reduce memory and bandwidth impacts
(sparse matrices). Due to the modular flexible nature of maximal coordinates, we use this method in this
chapter, so we can formulate constraints once, and use them again and again for various configurations.

Simulation Approaches
Three main types of constraint methods exist: Penalty methods, Impulse methods and Global methods.

Penalty methods (springs) - are the easiest technique for formulating constraints, whereby the violated
constraint error is fed back into the system as restoring forces to correct the error. They have the ability to
be simple, fast and intuitive, and can be combined with other methods to add controllability. Their
downside is that the constraints rely on error feedback forces, and suffer from stability issues, which
require small time-steps (offline) or computationally expensive integration techniques to remain stable.
The reader can refer to (Ka, Nordenstam, & Bullock, 2003) for a practical example of penalty methods
being used to create constraints.

Impulse methods (velocity impulses) - use instantaneous force changes, known as velocity impulses to
implement constraints. These velocity impulses are repeatedly applied to each constraint, where you solve
one constraint after the other, re-evolving the system to satisfy all constraint conditions, until the system
converges, or you reach a maximum update limit. Further reading can be found in (Guendelman, Bridson,
& Fedkiw, 2003) which presents a realistic impulse-based simulator with stacking.

Global methods (analytical methods) - which we use and apply in this chapter, involve computing the
exact magnitude of force that will satisfy the constraints at every step of the simulation. It is accurate and
requires minor parameter tuning by the user and can maintain stability for relatively large integration
steps. It works, fundamentally, by constructing a linear system of the form.

SOLVER (LCP)
The Linear Complementarily Problem (LCP) is a special kind of problem that aims to find a solution to a
set of equations, subject to constraint limits. The type of LCP we focus on in this chapter is the box-
constrained LCP, which aims to find a solution to the form `y = Ax+b', subject-to limits on `y':


where
y = Ax + b
subject to
y 0 x = xlower
(1)

y 0 x = xupper
y = 0 x
< x < x
lower
upper

We can broadly classify LCP solvers into two method types, iterative methods and pivoting methods.

Pivoting methods use recursion; where the solution to the problem depends on solutions to smaller
instances of the same problem, which can be solved in a finite number of steps. While pivoting methods
can be fast, it is our experience that for a large number of constraints, they can produce erroneous results
for perfectly valid systems due to floating point errors. Further reading and examples of pivoting methods
are Lemke's algorithm used in (David H. Eberly, 2004), and Dantzig's algorithm, used in (Baraff, 1994).

Iterative methods alternatively do not terminate but converge on a solution finitely, where convergence
depends on a number of factors such as the initial starting value. In addition, because iterative methods
move closer to the solution with every update, if they were interrupted early, the current result can be

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

good enough for the simulation to continue. Furthermore, we can take advantage of the starting guess and
coherency between frame updates to accelerate convergence, by feeding the previous frame's solution to
the next. This property makes them ideal for real-time applications, where we can break out at varied
times, trading accuracy for speed. Finally, iterative methods in practice are able to find acceptable results
in ill-conditioned or singular configurations; due to contacts or overly constrained systems that allow the
simulation to continue and recover.

Further reading about LCP methods for solving can be found in (Cottle, Pang, & Stone, 1992).

To restate, we use iterative methods because:
* The stopping criteria can be adjusted to trade accuracy for performance.
* In practice, they are more stable and reliable.
* They are simple to implement compared to other direct methods.
* There is a greater potential for optimisation (exploiting matrix sparsity for speedups).

Two popular iterative methods for solving systems of linear equations are `Gauss-Seidel' and the `Jacobi
method', which can be modified to handle equality constraints for our complementary problems, such as,
contacts. For this chapter, we use a modified `Gauss-Seidel' algorithm, called the `Projected Gauss-
Seidel', which offers a simple and intuitive implementation with good convergence rates. An in-depth
explanation on how systems of linear and complementary equations are solved can be obtained by reading
(Hagger, 1988), (Cottle et al., 1992) and (Erleben, 2004), also for a more detailed explanation of the
Gauss-Seidel and its differences to projected Gauss-Seidel, see (Catto & Park, 2005).

The Gauss-Seidel equation is shown below, followed by its implementation in code.


1

-

k +i
1
i
n
(
)
(k 1
+ )
k
x
=
b - x
- x
(2)
i
i
j
j
aii
j 1
=
j =i 1
+


where the subscript i and j indicates the column and row of the matrix elements from our linear equation
Ax=b. It works by starting from some initial value (e.g. 0), then iteratively updating the answer repeatedly
using the result from the previous step to converge on a solution. In the equation above, we have xk as
our current result, and xk+1 as the next.

The convergence on an acceptable answer depends upon the size and complexity of the configuration,
where it can take anywhere from two or three iterations to hundreds, depending on the topology and
initial starting value. For example, highly coupled configurations such as stacks of objects or long chains
take longer to converge than less densely coupled ones.

We add an extra step to our vital Gauss-Seidel method to incorporate boundary conditions and enforce the
complementary constraints with an additional projection step:


x = max( min( x ,
upper
x
),
lower
x
)
(3)
j
j
j
j

Stop Condition
The sample code uses a fixed iteration count, but a check can be added to determine if the solution is
within a certain tolerance and provide an early breakout. We can calculate this value using the equation
below:

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers


(k )
|| b - Ax
|| <
(4)
|| b ||




Acceleration or Velocity Level
We can broadly classify LCP solvers into two main types, acceleration-based (Baraff, 1989; 1994;
Lotstedt, 1984) or velocity-based (Anitescu & Potra, 1996; Stewart & Trinkle, 1996), where the solver is
classified according to the level it operates on to solve its constraints. In the next few sections, we review
both the acceleration and velocity level solvers, outlining their similarities and differences, but towards
the end of this chapter, we will focus on velocity-based solvers due to their added simplicity and
practicality.

We give a brief comparison of both methods and review their advantages and disadvantages. To begin
with, we demonstrate the two main sets of equations and the steps for formulating constraints at the
various levels to illustrate their differences.


Velocity
Acceleration
-1 T
-1
JM J + Jq + JM F 0
-1 T
-1
JM J + Jq + JM F 0
ext
ext
using
using
-1
q
-

= q + M (F + F ) t
1
q

= M
F
+ F
n +1
( ext
c )
n +
n
ext
c
1
T
F = J
T
F = J
c
c



The constraint formulation steps are as follows:

Velocity
Acceleration
1. Create positional constraint C.
1. Create positional constraint C.
2. Differentiate C with respect to time to obtain
2. Differentiate C with respect to time to obtain
the velocity constraint C .
the velocity constraint C .
3. Isolate and extract the Jacobian from C .
3. Isolate and extract the Jacobian from C .
4. Solve for the derivative of the Jacobian ( J )

w.r.t. time.



From the constraint formulation steps above, the reader may notice that the initial steps are very similar,
but the acceleration level requires additional work to calculate the Jacobian derivative. The velocity
approach is basically a subset of the acceleration level, where it moves the acceleration problem into the
LCP integration step in order to obtain a discrete problem, having velocities as unknowns rather than
accelerations.

The velocity-based method has the slight advantage of only having to compute the first differential for the
constraint equation due to the velocity method not needing the Jacobian derivative to calculate the
unknowns. Earlier solvers were formulated at the acceleration level, but had problems where friction

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

constraints could produce unstable systems. Later, the problem was modified so it could be solved at the
velocity level where this problem was solved.


Acceleration Level
Velocity Level
Step 1. Matrix version of Newton's second law, but splitting the total forces into two parts.
( f external force and f unknown constraint forces).
e
c
Mq f
f
e
c
M (


q t)
f
f
e
c
Construct constraint equation C(q) and differentiate it twice for acceleration level,
Step 2. and once for velocity level.
C(
q) 0
C(
q) 0
C(
q)
J
q 0
C(
q)
J
q 0
C
( q) J q Jq


0
Step 3. Using the Jacobian (J) and Lagrange multiplier ( ) we are able to solve
for the unknowns.
1
q
M ( f
f )
1
q
M
( f
f ) t
e
c
e
c
M (
1
T
f J
)
M (
1
T
f J
) t
c
c
Arranging the equation into the form 'Ax=b', with as the unknowns, we can use
Step 4. linear methods to invert the matrix and solve for the system of equations, and then
reinsert them back into step 3.
JM J
1 T
J 1
Jq
M f
JM J
1 T
Jq
e
J 1M f e
A
b
A
b

Figure 2. Basic four-step breakdown of acceleration and velocity based methods.

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

If principles of how to resolve constraints at the velocity level are understood, then the knowledge is there
to implement it for the acceleration level (and vice versa). For the remainder of this chapter, we will focus
on deriving and implementing methods for the velocity level, and where appropriate, mention any
extraneous details that might be relevant to the acceleration level.


CONSTRAINTS
Overview
The formulation of the equations we use for our simulator can be broken down into four easy steps as
shown in (Hecker, 1998) and reproduced in Figure 2. We introduce the steps briefly here, showing how
the solver equations fit together, to attain a physics-based simulator. Even though they may not be
completely understood initially, we reintroduce them again with further details and applied examples as
we advance through the chapter.

Equality and Non-Equality Constraints
There are two types of constraints, Equality "==" and Inequality ">,>=,<,<=". Examples of equality
constraints are rigid-ropes and ball-joints, while inequality constraints would be contacts and collisions.

Equality constraints have a fixed solution and cannot be varied along the constraint direction. They're
formed by setting the constraint condition `c' to zero, using the equality sign.

Inequality constraints can have numerous solutions and are formed by using a greater than or equal
operator in constraint equations.

In the steps mentioned earlier for solving the unknown constraint values, we used an equal sign for
A = b . For inequality conditions, we modify our linear solver to handle linear complementary
problems. The modification adds the condition ' ', and only accepts values greater or equal to zero,
essentially clamping so it is always positive:

Equality
Inequality
A + b = 0
A + b 0
0

(A + b) x = 0
c = 0
c 0


c = 0
c 0

Due to inequality constraints having multiple results, we can check if we need them before we add them
to our solver, i.e.:

if c 0 :
add c 0 to constraint solver
else
ignore

Jacobian

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

Forward kinematics enables us to define functions that convert between Cartesian space (i.e. positions and
orientations) to constraint space, and the Jacobian represents how this Cartesian-constraint space
relationship changes with respect to time, (e.g. Cartesian-constraint space velocity relationship).

The Jacobian gives the instantaneous transformation between the constraint velocities and the rigid body
velocities. Where, for objects in 3D, each rigid body has six DOF, which represents the three linear and
three angular velocities, for (m) rigid bodies we are able to form a (6m) column vector containing all the
rigid body velocities, in addition to our (n) constraints, to form a (6m x n) Jacobian matrix, describing
how all the objects in our system are allowed to move.

Jacobian is a matrix function of the form:

c = J q
(5)

where q
(linear and angular) Cartesian space velocities and c

the constraint velocities.

We can also express q
as its separate angular and linear velocity components:


q = [ v, ]
(6)

for each body, q
is a 6x1 Cartesian velocity vector, (3x1) linear vector and (3x1) rotational vector
stacked together.

We can also write:

J = [ J , J
(7)
v
]
i.e.

v = J q
v


(8)
= J q


As we said earlier, a single unlinked object moving in 3D, can move in six possible ways, three linear and
three angular. If you wanted to constrain the object from moving in a certain direction, the `z' direction,
for example, then you would set the `z' velocity to zero, effectively limiting translation movement to the
x-z plane. This is a simplified example of how a Jacobian works, and describes how constraints operate to
remove DOF to achieve a desired motion trajectory. While the Jacobian is at the heart of our analytical
method, it allows us to relate the classical equations of motion (f=ma) with Lagrange's multiplier to solve
systems of constraints. Later in the chapter we give numerous examples on how to describe and derive a
mixture of common constraints and their Jacobian.

Because the Jacobian is central to our constraint formulation, we will spend a bit more time explaining its
mechanics in detail with examples to give a rock-solid understanding. The Jacobian, sometimes called the
constraint Jacobian, is a matrix which allows us to specify which motions are not allowed. The number
of rows of the matrix determines the order of the constraint or the number of degrees of freedom removed
from the system. For most constraints we calculate J on a frame by frame basis as it depends on the
body's positions and orientations.


Change Input
I
n
J =
=

(9)
Change Output
O
ut

---

Practical Introduction to Rigid Body Linear Complementary Problem (LCP) Constraint Solvers

We can combine Jacobian matrices to formulate more complex ones, by which we mean, we can
construct simple constraints and combine them to build more difficult ones.

To repeat the most important equation of this section, the Jacobian velocity constraint is shown again
below in Equation (10), where q is the system body velocities, and c is the derivative of the positional
constraints c with respect to time, and produces constraints by removing degrees of freedom from the
system.


J q = c
(10)
To derive the Jacobian, we need to obtain c , which we attain by constructing a positional constraint `c'
and differentiating it. The positional constraint is constructed by representing how the body is allowed to
move with a kinematic equation such that when it is satisfied, it evaluates to zero:


c = 0
(11)
If we differentiate our positional constraint equation ( c ) with respect to time, it will describe the
constraint velocity properties. Also as `c' is equal to zero, the derivative should be zero:


c = 0
(12)
Since we have said that c and c will be zero and have explained that the Jacobian has six elements for
each body which constitute how the six velocity components change with respect to time, we can
conclude that any non-zero values in J will affect the corresponding body velocities. Remember that
these are relative to the body's point of reference (its position and rotation at that instance in time and can
need re-calculating as the object moves).

Using this knowledge, the Jacobian can be calculated in three straightforward steps; firstly, by
constructing an equation for the positional constraints, secondly by differentiating it with respect to time
to get the velocity properties, and finally by isolating and extracting the Jacobian.

In some cases, you can compose the Jacobian constraint matrix by visually looking at the system, but for
more complicated constraint schemes, you will need to follow the steps above, whereby you'll find that
the ability to construct the Jacobian gets easier with practice. We give examples of simple Jacobian
constraint matrices in the next section.

Note
When a constraint only affects a single object, and does not affect or rely upon any other objects, they are
referred to as `unary constraints', whilst a constraint involving another point-mass or rigid body is
termed a `binary constraint'. In addition, the Jacobian for binary constraints is usually the same but
reversed.


Constraint Equations
We now focus on explaining how we resolve the constraints at the velocity level and the formulation of
our system of equations that we use to represent our procedure. Even though we focus on the velocity
level, we can apply the same principles and practice to the acceleration level without too much effort.

During the simulation update, we calculated a set of constraint forces, which we combined with the
external forces, to keep the constraints valid. The main equation, which we construct and use to determine
these cancelling constraint forces, is shown in Figure 3:

---