Modelling and Solving Physical Constraints
A constraint is a restriction upon a physically modeled object within a simulation. In general, an object starts with six degrees of freedom, representing its ability to move about and rotate within the simulated world; by restricting these degrees of freedom in various ways, we can achieve many interesting and appealing effects.
As the CPU of modern computers becomes more and more powerful, computations can be put towards the modelling and solving of many interesting physical scenarios. Constraints are a generalized and mathematically backed way of producing such scenarios. We can all thank Erin Catto for his initial paper on this topic: Iterative Dynamics with Temporal Coherence. I will be using this paper as a reference when writing this post, so I suggest you at least take a peek before reading on, even if only out of respect for Erin's work (and his listed resource works and special thanks by each of their contributors).
Glossary
There are a few different terms commonly used when talking about physics engines. For clarity, here is a short glossary to use as a reference when reading this post:
- Rigid Body: An object simulated within a physics engine. The object is assumed to be completely rigid, like a diamond. Most of the time, rigid bodies bounce around, slide on each other, and make up the physical world of a game.
- Degree of Freedom: One of the six scalar values used to represent a rigid body's position and orientation within the world. Three scalar values represent position, and three represent orientation.
- Constraint: A limitation placed upon one or more degrees of freedom of a rigid body. Almost all constraints are pairwise, meaning they affect two rigid bodies in tandem.
- Joint: A joint is a pairwise constraint, although the type of constraint is not an interpenetration constraint. All other types of constraints are referred to as a joint.
- Contact Constraint: A pairwise constraint that is not a joint, meaning it is an interpenetration constraint.
- Contact Normal: The direction in which to solve an interpenetration constraint. This is usually determined by collision detection.
Prerequisites
A few prerequisites are required to take full advantage of this article. However, the general reader can still enjoy much the material, although it is best to have a basic understanding of:
- Algebra
- Calculus
- Vector calculus
- Intermediate to advanced programming skills (in order to write the code)
Types of Constraints
Since a constraint limits degrees of freedom, let's check out what a rigid body does when utilizing six at once:
The above rigid body is using three degrees of freedom to translate itself through the world. The last three are used to constantly change orientation of all three rotational axes.
Let's now look at a couple different examples of what constraints actually are. The most familiar constraint would be one that prevents two rigid bodies from penetrating. This type of constraint is only active when two bodies are penetrating one another, and drives the two bodies apart. Once this interpenetration constraint is active, it is easy to see that the degrees of freedom of the rigid bodies become limited and affected in such a way so as to produce an interesting result (the interesting result being that the two objects can collide with one another):
The Hello World of constraints would be the distance constraint, where two points on two rigid bodies are constrained to be an exact distance from one another. You can imagine a massless rod connecting two points together, where this rod cannot be stretched or compressed:
Many types of constraints exist for all sorts of interesting behaviors, including: friction, prismatic, revolute, weld, angle, and more.
Understanding Constraint Equations
In general form, a constraint is a scalar equation equal to some value (usually zero).
\begin{equation}
C( l_1, a_1, l_2, a_2 ) = 0
\label{eq1}
\end{equation}
The \(l\) and \(a\) terms in \eqref{eq1} are my own notation: \(l\) refers to linear while \(a\) refers to angular. The subscripts 1 and 2 refer to the two objects within the constraint. As you can see, there exist linear and angular inputs to a constraint equation, and each must be a scalar value.
Let's take a step back to look at the distance constraint. The distance constraint wants to drive the distance between two anchor points on two bodies to be equal to some scalar value:
\begin{equation}
C( l_1, a_1, l_2, a_2 ) = \frac{1}{2}[(P_2 - P_1)^2 - L^2] = 0
\label{eq2}
\end{equation}
\(L\) is the length of the rod connecting both bodies; \(P_1\) and \(P_2\) are the positions of the two bodies.
In its current form, this constraint is an equation of position. This sort of position equation is non-linear, which makes solving it very hard. A method of solving this equation can be to instead derive the position constraint (with respect to time) and use a velocity constraint. Resulting velocity equations are linear, making them solvable. Solutions can then be integrated using some sort of integrator back into positional form.
In general form, a velocity constraint is of the form:
\begin{equation}
\dot{C}( l_1, a_1, l_2, a_2 ) = 0
\label{eq3}
\end{equation}
During the derivative, a new term \(J\) appears via chain rule:
\begin{equation}
\dot{C}( l_1, a_1, l_2, a_2 ) = JV = 0
\label{eq4}
\end{equation}
The time derivative of \(C\) creates a velocity vector and a Jacobian. The Jacobian is a 1x6 matrix containing scalar values corresponding to each degree of freedom. In a pairwise constraint, a Jacobian will typically contain 12 elements (enough to contain the \(l\) and \(a\) terms for both bodies \(A\) and \(B\).
A system of constraints can form a joint. A joint can contain many constraints restricting degrees of freedom in various ways. In this case, the Jacobian will be a matrix where the number of rows is equal to the number of constraints active in the system.
The Jacobian is derived offline, by hand. Once a Jacobian is acquired, code to compute and use the Jacobian can be created. As you can see from \eqref{eq4}, the velocity \(V\) is transformed from Cartesian space to constraint space. This is important because in constraint space the origin is known. In fact, any target can be known. This means that any constraint can be derived to yield a Jacobian that can transform forces from Cartesian space to constraint space.
In constraint space, given a target scalar, the equation can move either towards or away from the target. Solutions can easily be obtained in constraint space to move the current state of a rigid body towards a target state. These solutions can then be transformed out of constraint space back into Cartesian space like so:
\begin{equation}
F = \lambda J^T
\label{eq5}
\end{equation}
\(F\) is a force in Cartesian space, where \(J^T\) is the inverse (transposed) Jacobian. \(\lambda\) (lambda) is a scalar multiplier.
Think of the Jacobian as a velocity vector, where each row is a vector itself (of two scalar values in 2D, and three scalar values in 3D):
\begin{equation}
J = \begin{bmatrix}
l_1 \\
a_1 \\
l_2 \\
a_2 \\
\end{bmatrix}
\label{eq6}
\end{equation}
To multiply \(V\) by \(J\) mathematically would involve matrix multiplication. However, most elements are zero, and this is why we treat the Jacobian as a vector. This allows us to define our own operation for computing \(JV\), as in \eqref{eq4}.
\begin{equation}
JV = \begin{bmatrix}
l_1 & a_1 & l_2 & a_2
\end{bmatrix}
\begin{bmatrix}
v_1 \\
ω_1 \\
v_2 \\
ω_2 \\
\end{bmatrix}
\label{eq7}
\end{equation}
Here, \(v\) represents linear velocity, and \(ω\) (omega) represents angular velocity. \eqref{eq7} can be written down as a few dot products and multiplications in order to provide a more efficient computation compared to full matrix multiplication:
\begin{equation}
JV = l_1 \cdot v_1 + a_1 \cdot ω_1 + l_2 \cdot v_2 + a_2 \cdot ω_2
\label{eq8}
\end{equation}
The Jacobian can be thought of as a direction vector in constraint space. This direction always points towards the target in the direction that requires the least work to be done. Since this "direction" Jacobian is derived offline, all that needs to be solved for is the magnitude of the force to be applied in order to uphold the constraint. This magnitude is called \(\lambda\). \(\lambda\) can be known as the Lagrange Multiplier. I myself have not formally studied Lagrangian Mechanics, however a study of Lagrangian Mechanics is not necessary in order to simply implement constraints. (I am proof of that!) \(\lambda\) can be solved using a constraint solver (more on this later).
Solving for Jacobians
In Erin Catto's paper, there exists a simple outline for hand-deriving Jacobians. The steps are:
- Start with constraint equation \(C\)
- Compute time derivative \(\dot{C}\)
- Isolate all velocity terms
- Identify \(J\) by inspection
The only hard part is computing the derivative, and this can come with practice. In general, hand-deriving constraints is difficult, but gets easier with time.
Let's derive a valid Jacobian for use in solving a distance constraint. We can start at Step 1 with \eqref{eq2}. Here are some details for Step 2:
\begin{equation}
\dot{C} = (P_2 - P_1)(\dot{P}_2 - \dot{P}_1)
\label{eq9}
\end{equation}
\begin{equation}
\dot{C} = (P_2 - P_1)((v_2 + ω_2 \times r_2) - (v_1 + ω_1 \times r_1))
\label{eq10}
\end{equation}
\(r_1\) and \(r_2\) are vectors from the center of mass to the anchor point, for bodies 1 and 2 respectively.
The next step is to isolate the velocity terms. To do this, we'll make use of the scalar triple product identity:
\begin{equation}
(P_2 - P_1) = d
\label{eq11}
\end{equation}
\begin{equation}
\dot{C} = (d \cdot v_2 + d \cdot ω_2 \times r_2) - (d \cdot v_1 + d \cdot ω_1 \times r_1)
\label{eq12}
\end{equation}
\begin{equation}
\dot{C} = (d \cdot v_2 + ω_2 \cdot r_2 \times d) - (d \cdot v_1 + ω_1 \cdot r_1 \times d)
\label{eq13}
\end{equation}
The last step is to identify the Jacobian by inspection. In order to do this, all the coefficients of all velocity terms (\(V\) and \(ω\)) will be used as the Jacobian elements. Therefore:
\begin{equation}
J = \begin{bmatrix} -d & -r_1 \times d & d & r_2 \times d \end{bmatrix}
\label{eq14}
\end{equation}
Some more Jacobians
Contact constraint (interpenetration constraint), where \(n\) is the contact normal:
\begin{equation}
J = \begin{bmatrix} -n & -r_1 \times n & n & r_2 \times n \end{bmatrix}
\label{eq15}
\end{equation}
Friction constraint (active during penetration), where \(t\) is an axis of friction (2D has one axis, 3D has two):
\begin{equation}
J = \begin{bmatrix} -t & -r_1 \times t & t & r_2 \times t \end{bmatrix}
\label{eq16}
\end{equation}
Solving Constraints
Now that we have an understanding of what a constraint is, we can talk about how to solve them. As stated earlier, once a Jacobian is hand-derived, we only need to solve for \(\lambda\). Solving a single constraint in isolation is easy, but solving many constraints simultaneously is hard, and very inefficient (computationally). This poses a problem, as games and simulations will likely want to have many constraints active all at once.
An alternative method to solving all constraints simultaneously (globally solve) would be to solve the constraints iteratively. By solving for approximations of the solution, and feeding in previous solutions to the equations, we can converge on the solution.
One such iterative solver is known as Sequential Impulses, as dubbed by Erin Catto. Sequential Impulses is very similar to Projected Gauss Seidel. The idea is to solve all constraints, one at a time, multiple times. The solutions will invalidate each other, but over many iterations each individual constraint will converge and a global solution can be achieved. This is good! Iterative solvers are fast.
Once a solution is achieved, an impulse can be applied to both bodies in the constraint in order to enforce the constraint.
To solve a single constraint, we can use the following equation:
\begin{equation}
\lambda = \frac{-(JV + b)}{JM^{-1}J^T}
\label{eq17}
\end{equation}
\(M^{-1}\) is the mass of the constraint; \(b\) is the bias (more on this later).
This is a matrix containing the inverse mass and inverse inertia of both rigid bodies in the constraint. The following is the mass of the constraint; note that \(m^{-1}\) is the inverse mass of a body, while \(I^{-1}\) is the inverse inertia of a body:
\begin{equation} M^{-1} =
\begin{bmatrix}
m_1 ^{-1} & 0 & 0 & 0 \\
0 & I_1 ^{-1} & 0 & 0 \\
0 & 0 & m_2 ^{-1} & 0 \\
0 & 0 & 0 & I_2 ^{-1}
\end{bmatrix}
\label{eq18}
\end{equation}
Although \(M^{-1}\) is theoretically a matrix, please do not actually model it as such (most of it is zeroes!). Instead, be smart about what sort of calculations you do.
\(JM^{-1}J^T\) is known as the constraint mass. This term is calculated one time and used to solve for \(\lambda\). We calculate it for a system like so:
\begin{equation}
JM^{-1}J^T = (l_1 \cdot l_1) * m_1 ^{-1} + (l_2 \cdot l_2) * m_2 ^{-1} + a_1 * (I_1 ^{-1} a_1) + a_2 * (I_2 ^{-1} a_2)
\label{eq19}
\end{equation}
Please note that you must invert \eqref{eq19} in order to compute \eqref{eq17}.
The above information is all that is needed to solve a constraint! A force in Cartesian space can be solved for and used to update the velocity of an object, in order to enforce a constraint. Please recall \eqref{eq5}:
\begin{equation}
F = \lambda J^T \\
V_{final} = V_{initial} + m^{-1} * F \\
∴ \\
\begin{bmatrix}
v_1 \\
ω_1 \\
v_2 \\
ω_2 \\
\end{bmatrix} += \begin{bmatrix}
m_1 ^{-1} & 0 & 0 & 0 \\
0 & I_1 ^{-1} & 0 & 0 \\
0 & 0 & m_2 ^{-1} & 0 \\
0 & 0 & 0 & I_2 ^{-1}
\end{bmatrix}\begin{bmatrix}
\lambda * l_1 \\
\lambda * a_1 \\
\lambda * l_2 \\
\lambda * a_2 \\
\end{bmatrix}
\label{eq20}
\end{equation}
Constraint Drift
Due to the linearization of the non-linear position equations, some information is lost. This results in solutions that don't quite satisfy the original position equation, but do satisfy the velocity equations. This error is known as constraint drift. One can think of this error as the result of a tangent line approximation.
There are a few different ways to solve such errors, all of which approximate the error and apply some form of correction. The simplest is known as Baumgarte.
Baumgarte is a small addition of energy in constraint space, and accounts for the \(b\) term in the previous equations. To account for bias, here is a modified version of \eqref{eq4}:
\begin{equation}
\dot{C} = JV + b = 0
\label{eq21}
\end{equation}
To calculate a Baumgarte term and apply it as a bias, we must inspect the original constraint equation and identify a suitable method for calculating error. Baumgarte is in the form:
\begin{equation}
JV = -\beta C
\label{eq22}
\end{equation}
\(\beta\) (Baumgarte term) is a tunable, unit-less, simulation-dependent factor. It is usually between 0.1
and 0.3
.
In order to calculate the bias term, let's look at the equation for the non-penetration constraint \eqref{eq15} before deriving with respect to time, where \(n\) is the contact normal:
\begin{equation}
C = \begin{bmatrix} -x_1 & -r_1 & x_2 & r_2 \end{bmatrix} \cdot \vec{n}
\label{eq23}
\end{equation}
The above equation is saying that the scalar error of \(C\) is the inter-penetration depth between two rigid bodies.
Conclusion
Thanks to Erin Catto and his paper on constraint solving, we've got a way to solve position constraints in terms of velocity. Many interesting behaviors can arise from constraints, and hopefully the article will be of use to many people. As always, please feel free to ask questions or give comments below.
Please see Box2D Lite for a resource on solving various types of 2D constraints, as well as insight into many implementation specifics that are not covered here.
Envato Tuts+ tutorials are translated into other languages by our community members—you can be involved too!
Translate this post