Out of various phases of the physics engine, Constraint Resolution was the hardest for me to understand personally. I need to read many different papers and articles to fully understand how constraint resolution works. Therefore, I decided to write this article to help me understand it more easily in the future if, for instance, I forget how this works.

This article will tackle this problem by giving an example, then make a general formula out of it. Now, let us delve into a pretty common scenario when two of our rigid bodies collide and penetrate each other as depicted below.

From the scenario above we can formulate

$d = \left(\right(\overrightarrow{p1}+\overrightarrow{r1}) – (\overrightarrow{p2}+\overrightarrow{r2}\left)\right) . \overrightarrow{n}$

We do not want our rigid bodies to intersect each other, thus we construct a constraint where the penetration depth must be more than zero.

$C: d \ge 0$

This is an inequality constraint. We can transform it to a simpler equality constraint by only solving it if two bodies are penetrating each other. If two rigid bodies don’t collide with each other, we don’t need any constraint resolution. So:

$$\begin{gathered} if d \ge 0, then do nothing \\ else if d<0 \\ solve C: d = 0 \end{gathered}$$

Next, we can solve this constraint by calculating \(\Delta \vec{p1},\Delta \vec{p2},\Delta \vec{r1},and \Delta \vec{r2}\) that cause the constraint above satisfied. This method is called the position-based method. This will satisfy the above constraint immediately in the current frame. But, in the next frame, the constraint will be violated again due to the velocity still not being corrected. This velocity will keep growing.

A much more modern and preferable method that is used in Box2d, Chipmunk, Bullet and my physics engine is called the impulse-based method. In this method, we derive a velocity constraint equation from the position constraint equation above.

$$\begin{gathered} \dot{C}:\dot{d}=0 \\ \Rightarrow \dot{C}:\left(\overrightarrow{v1}+w1\times \overrightarrow{r1}–\overrightarrow{v2}–w2\times \overrightarrow{r2}\right)·\overrightarrow{n}=0 \\ \Rightarrow \dot{C}:\left[\begin{array}{cccc}{\overrightarrow{n}}^T& magnitude{\left(\overrightarrow{r1}\times \overrightarrow{n}\right)}^T& –{\overrightarrow{n}}^T& –magnitude\left(\overrightarrow{r2}\times \overrightarrow{n}\right)\end{array}\right]\left[\begin{array}{c}\overrightarrow{v1}\\ w1\\ \overrightarrow{v2}\\ w2\end{array}\right]=0 \\ if J=\left[\begin{array}{cccc}{\overrightarrow{n}}^T& magnitude(\overrightarrow{r1}\times \overrightarrow{n}{)}^T& –{\overrightarrow{n}}^T& –{magnitude(\overrightarrow{r2}\times \overrightarrow{n})}^T\end{array}\right], and V=\left[\begin{array}{c}\overrightarrow{v1}\\ w1\\ \overrightarrow{v2}\\ w2\end{array}\right] then, \\ \dot{C}: JV=0, ...............equation\left(1\right) \\ V = V_{prev}+△v \\ \Rightarrow \dot{C}: J\left(V_{prev}+△v\right)=0 \end{gathered}$$

We are working in 2D so angular velocity and the cross result of two vectors are scalars.

Next, we need to find \(\Delta V\) or impulse to satisfy the velocity constraint. This \(\Delta V\) is caused by a force. We call this force ‘constraint force’. Constraint force only exerts a force on the direction of illegal movement in our case the penetration normal. We don’t want this force to do any work, contribute or restrict any motion of legal direction.

$F_{constraint} = J^T\lambda$

\(\lambda\) is a scalar, called Lagrangian multiplier. To understand why constraint force working on \(J^{T}\) direction (remember J is a 12 by 1 matrix, so \(J^{T}\) is a 1 by 12 matrix or a 12-dimensional vector), try to remember the equation for a three-dimensional plane.

$$\begin{gathered} ax+by+cz=0, \\ if \overrightarrow{n}=\left[\begin{array}{c}a\\ b\\ c\end{array}\right], and \overrightarrow{v}=\left[\begin{array}{c}x\\ y\\ z\end{array}\right] , then, \\ {\overrightarrow{n}}^T\overrightarrow{v}=0....... equation\left(2\right) \end{gathered}$$

We can draw similarity between equation(1) and equation(2), where \(\vec{n}^{T}\) is similar to J and \(\vec{v}\) is similar to V. Consequently, we can interpret equation(1) as a 12 dimensional plane, we can conclude that \(J^{T}\) as the normal of this plane. If a point is outside a plane, the shortest distance from this point to the surface is the normal direction.

$$\begin{gathered} if V = \left[\begin{array}{c}\overrightarrow{v1}\\ w1\\ \overrightarrow{v2}\\ w2\end{array}\right], A=\left[\begin{array}{c}\overrightarrow{a1}\\ \alpha 1\\ \overrightarrow{a2}\\ \alpha 2\end{array}\right], M=\left[\begin{array}{cccccc}m1& 0& 0& 0& 0& 0\\ 0& m1& 0& 0& 0& 0\\ 0& 0& I1& 0& 0& 0\\ 0& 0& 0& m2& 0& 0\\ 0& 0& 0& 0& m2& 0\\ 0& 0& 0& 0& 0& I2\end{array}\right], F=\left[\begin{array}{c}\overrightarrow{f1}\\ \tau 1\\ \overrightarrow{f2}\\ \tau 2\end{array}\right], then \\ ∆V=At \\ \Rightarrow ∆V=\frac{F}{M} \\ \Rightarrow ∆V=\frac{J^T\lambda }{M} \\ \Rightarrow ∆V=M^{–1}{J}^T\lambda .......equation\left(3\right) \\ Next we substitute this to previous equation \\ J\left(V+∆V\right)=0 \\ \Rightarrow J\left(V+M^{–1}{J}^T\lambda \right)=0 \\ \Rightarrow \lambda =\frac{–\left(JV\right)}{J{M}^{–1}{J}^T}..........equation\left(4\right) \end{gathered}$$

After we calculate the Lagrangian multiplier, we have a way to get back the impulse from equation(3). Then, we can apply this impulse to each rigid body.

Baumgarte Stabilization

Note that solving the velocity constraint doesn’t mean that we satisfy the position constraint. When we solve the velocity constraint, the position constraint is already violated. We call this violation position drift. What we achieve is stopping the two bodies from penetrating deeper (The penetration depth will stop growing). It might be fine for a slow-moving object as the positional drift is not visually noticeable, but it will be a problem as the object moving faster. The animation below demonstrates what happens when we solve the velocity constraint.

Thus, instead of purely solving the velocity constraint, we add a bias term to fix any violation that happens in the position constraint.

$$\begin{gathered} \dot{C}:\hspace{0.17em}J(V_{prev}+△V)+b\ge 0, where b =bias \\ \Rightarrow \lambda =\frac{–(JV+b)}{J{M}^{–1}{J}^T}...............equation\left(5\right) \end{gathered}$$

So, what is the value of the bias? As mentioned before we need this bias to fix positional drift. So we want this bias to be in proportion to penetration depth.

$b=–\frac{\beta }{△t}d$

This method is called Baumgarte Stabilization and \(\beta\) is a baumgarte term. The right value for this term might differ for different scenarios. We need to tweak this value between 0 and 1 to find the right value that makes our simulation stable.

Sequential Impulse

If our world consists only of two rigid bodies and one contact constraint, the above method will work decently. But in most games, there are more than two rigid bodies. One body can collide and penetrate with two or more bodies. We need to satisfy all the contact constraint simultaneously. For a real-time application, solving all these constraints simultaneously is not feasible. Erin Catto proposes a practical solution, called sequential impulse. The idea here is similar to Project Gauss-Seidel. We calculate \(\lambda\) and \(\Delta V\) for each constraint one by one, from constraint one to constraint n(n = number of constraint). After we finish iterating through the constraints and calculate \(\Delta V\), we repeat the process from constraint one to constraint n until the specified number of iteration. This algorithm will converge to the actual solution.The more we repeat the process, the more accurate the result will be. In Box2d, Erin Catto set ten as the default for the number of iteration.

Another thing to notice is that while we satisfy one constraint we might unintentionally satisfy another constraint. Say for example that we have two different contact constraint on the same rigid body.

$$\begin{gathered} Constraint that we need to solve: \\ \dot{C1}:\dot{ d1} \ge \hspace{0.17em}0 \\ \dot{C2}: \dot{d2} \ge 0 \\ Initially: \dot{d1} < 0 and \dot{d2} < 0 \end{gathered}$$

When we solve \(\dot{C1}\), we might incidentally make \(\dot{d2} >= 0\). Remember that equation(5), is a formula for \(\dot{C}: \dot{d} = 0\) not \(\dot{C}: \dot{d} >= 0\). So we don’t need to apply it to \(\dot{C2}\) anymore. We can detect this by looking at the sign of \(\lambda\). If the sign of \(\lambda\) is negative, that means the constraint is already satisfied. If we use this negative lambda as an impulse, it means we pull it closer instead of pushing it apart. It is fine for individual \(\lambda\) to be negative. But, we need to make sure the accumulation of \(\lambda\) is not negative. In each iteration, we add the current lambda to normalImpulseSum. Then we clamp the normalImpulseSum between 0 and positive infinity. The actual Lagrangian multiplier that we will use to calculate the impulse is the difference between the new normalImpulseSum and the previous normalImpulseSum

$\sum _{}\lambda \ge 0$

Restitution

Okay, now we have successfully resolve contact penetration in our physics engine. But what about simulating objects that bounce when a collision happens. The property to bounce when a collision happens is called restitution. The coefficient of restitution, denoted /(C_{r}\), is the ratio of the parting speed after the collision and the closing speed before the collision.

$$\begin{gathered} C_r=\frac{V_{final}}{V_{initial}} \\ \Rightarrow V_{final}=C_r\times V_{initial} \end{gathered}$$

The coefficient of restitution only affects the velocity along the normal direction. Therefore, we need to do the dot operation with the normal vector.

$$\begin{gathered} V_{initial}=(V{1}_{initial}–V{2}_{initial})·\overrightarrow{n} \\ \Rightarrow V_{initial} =\left(\overrightarrow{v1}+w1\times \overrightarrow{r1}–\overrightarrow{v2}–w2\times \overrightarrow{r2}\right)·\overrightarrow{n} \end{gathered}$$

Notice that in this specific case the \(V_{initial}\) is similar to JV. If we look back at our constraint above, we set \(\dot{d}\) to zero because we assume that the object does not bounce back(\(C_{r}=0\)).So, if \(C_{r} != 0\), instead of 0, we can modify our constraint so the desired velocity is \(V_{final}\).

$$\begin{gathered} \dot{C}:JV+b=V_{final} \\ \Rightarrow \dot{C}:JV+b–V_{final}=0 \\ \Rightarrow \dot{C}:JV+b–\left(C_r{V}_{initial}\right) = 0 \\ \Rightarrow \dot{C}:JV+b–\left(C_{r}JV\right) = 0 \end{gathered}$$

We can merge our old bias term with the restitution term to get a new bias value.

$\dot{C}:JV+b_{new }=0,where b_{new }=–\frac{\beta }{△t}d–C_{r}JV$

Final Result:

Final Code:

// init constraint
// Calculate J(M^-1)(J^T). This term is constant so we can calculate this first
for (int i = 0; i < constraint->numContactPoint; i++)
{
    ftContactPointConstraint *pointConstraint = &constraint->pointConstraint[i];

    pointConstraint->r1 = manifold->contactPoints[i].r1 - (bodyA->transform.center + bodyA->centerOfMass);
    pointConstraint->r2 = manifold->contactPoints[i].r2 - (bodyB->transform.center + bodyB->centerOfMass);

    real kNormal = bodyA->inverseMass + bodyB->inverseMass;

    // Calculate r X normal
    real rnA = pointConstraint->r1.cross(constraint->normal);
    real rnB = pointConstraint->r2.cross(constraint->normal);

    // Calculate J(M^-1)(J^T).
    kNormal += (bodyA->inverseMoment * rnA * rnA + bodyB->inverseMoment * rnB * rnB);

    // Save inverse of J(M^-1)(J^T).
    pointConstraint->normalMass = 1 / kNormal;

    pointConstraint->positionBias = m_option.baumgarteCoef *

    manifold->penetrationDepth[i];

    ftVector2 vA = bodyA->velocity;
    ftVector2 vB = bodyB->velocity;
    real wA = bodyA->angularVelocity;
    real wB = bodyB->angularVelocity;
    ftVector2 dv = (vB + pointConstraint->r2.invCross(wB) - vA - pointConstraint->r1.invCross(wA));
    //Calculate JV
    real jnV = dv.dot(constraint->normal);
    pointConstraint->restitutionBias = -restitution * (jnV);

}

// solve constraint
while (numIteration > 0)
{

    for (int i = 0; i < m_constraintGroup.nConstraint; ++i) { 
        ftContactConstraint *constraint = &(m_constraintGroup.constraints[i]); int32 bodyIDA = constraint->bodyIDA;
        int32 bodyIDB = constraint->bodyIDB;
        ftVector2 normal = constraint->normal;
        ftVector2 tangent = normal.tangent();

        for (int j = 0; j < constraint->numContactPoint; ++j)
        {
            ftContactPointConstraint *pointConstraint = &(constraint->pointConstraint[j]);

            ftVector2 vA = m_constraintGroup.velocities[bodyIDA];
            ftVector2 vB = m_constraintGroup.velocities[bodyIDB];
            real wA = m_constraintGroup.angularVelocities[bodyIDA];
            real wB = m_constraintGroup.angularVelocities[bodyIDB];

            //Calculate JV. (jnV = JV, dv = derivative of d, JV = derivative(d) dot normal))
            ftVector2 dv = (vB + pointConstraint->r2.invCross(wB) - vA - pointConstraint->r1.invCross(wA));
            real jnV = dv.dot(normal);

            //Calculate Lambda ( lambda
            real nLambda = (-jnV + pointConstraint->positionBias / dt + pointConstraint->restitutionBias) *
            pointConstraint->normalMass;

            // Add lambda to normalImpulse and clamp
            real oldAccumI = pointConstraint->nIAcc;
            pointConstraint->nIAcc += nLambda;
            if (pointConstraint->nIAcc < 0) { 
                pointConstraint->nIAcc = 0;
            }
            // Find real lambda
            real I = pointConstraint->nIAcc - oldAccumI;

            // Calculate linear impulse
            ftVector2 nLinearI = normal * I;

            // Calculate angular impulse
            real rnA = pointConstraint->r1.cross(normal);
            real rnB = pointConstraint->r2.cross(normal);
            real nAngularIA = rnA * I;
            real nAngularIB = rnB * I;

            // Apply linear impulse
            m_constraintGroup.velocities[bodyIDA] -= constraint->invMassA * nLinearI;
            m_constraintGroup.velocities[bodyIDB] += constraint->invMassB * nLinearI;

            // Apply angular impulse
            m_constraintGroup.angularVelocities[bodyIDA] -= constraint->invMomentA * nAngularIA;
            m_constraintGroup.angularVelocities[bodyIDB] += constraint->invMomentB * nAngularIB;
        }
    }
    --numIteration;
}

General Step to Solve Constraint

In this article, we have learned how to solve contact penetration by defining it as a constraint and solve it. But this framework is not only used to solve contact penetration. We can do many more cool things with constraints like for example implementing hinge joint, pulley, spring, etc.

So this is the step-by-step of constraint resolution:

1. Define the constraint in the form \(\dot{C}: JV + b = 0\). V is always \(\begin{bmatrix} \vec{v1} \\ w1 \\ \vec{v2} \\ w2\end{bmatrix}\) for every constraint. So we need to find J or the Jacobian Matrix for that specific constraint.
2. Decide the number of iteration for the sequential impulse.
3. Next find the Lagrangian multiplier by inserting velocity, mass, and the Jacobian Matrix into this equation:
   $\lambda =\frac{–(JV+b)}{J{M}^{–1}{J}^T}$
4. Do step 3 for each constraint, and repeat the process as much as the number of iteration.
5. Clamp the Lagrangian multiplier if needed.

This marks the end of this article. Feel free to ask if something is still unclear. And please inform me if there are inaccuracies in my article. Thank you for reading.

NB: Box2d use sequential impulse, but does not use baumgarte stabilization anymore. It uses full NGS to resolve the position drift. Chipmunk still use baumgarte stabilization.

Reference

1. Allen Chou’s post on Constraint Resolution
2. A Unified Framework for Rigid Body Dynamics
3. An Introduction to Physically Based Modeling: Constrained Dynamics
4. Erin Catto’s Box2d and presentation on constraint resolution

Game Math Physics