• # Understanding Constraint Resolution in Physics Engine

Math and Physics

• Posted By kevinyu

Original Post: Limitless Curiosity

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

This article will tackle this problem by giving an example then make a general formula out of it. So 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:

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

$$C: d>=0$$

This is an inequality constraint, we can transform it to a more simple 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:

if d>=0, do nothing else if d < 0 solve C: d = 0

Now we can solve this equation 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 and might cause a jittery effect.

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.

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.

$$\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.

Now 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. So 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.

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, there is already a violation in the position constraint. 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 position drift is not noticeable, but it will be a problem as the object moving faster. The animation below demonstrates what happens when we solve the velocity constraint.

[caption id="attachment_38" align="alignnone" width="800"]

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

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.

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. Then 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.

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

#### 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.

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

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}$$.

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

// 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;

pointConstraint->r1 = manifold->contactPoints.r1 - (bodyA->transform.center + bodyA->centerOfMass);
pointConstraint->r2 = manifold->contactPoints.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;
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 + m_option.restitutionSlop);
}

// solve constraint
while (numIteration > 0)
{

for (int i = 0; i < m_constraintGroup.nConstraint; ++i) {
ftContactConstraint *constraint = &(m_constraintGroup.constraints); 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$$. is always $$\begin{bmatrix} \vec{v1} \\ w1 \\ \vec{v2} \\ w2\end{bmatrix}$$ for every constraint. So we need to find 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:
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.

References

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

Report Article

## User Feedback

Some of your formulas are shown as text and not as formular, can you double check those?

##### Share on other sites

@Finalspace Sorry. I think there are bugs in the equation editor, and it made me really frustrated, so I kinda gave up back then. Now I already understand how to insert equation without the editor. So thank you for reminding me.

## Create an account

Register a new account

• ### Game Developer Survey

We are looking for qualified game developers to participate in a 10-minute online survey. Qualified participants will be offered a \$15 incentive for your time and insights. Click here to start!

• 0
• 0
• 4
• 3

• 13
• 30
• 9
• 16
• 12
• ### Similar Content

• Hi,  I was wanting to read the box2d source, but wanted to get a broad overview first.  I'll just say what I understand so far:
Box2d has discrete & continuous collision detection.  We first integrate our shapes velocity, and using a 'conservative advancement algorithm, we advance the shapes & find toi.
It then uses gjk & barycentric coordinates to find the closest point on the shapes.  If  theyre overlapping we use epa or sat (and dont get a contact point? Or run gjk again?)
It then builds an incremental manifold (i probably have this wrong) adding a point each frame.
Then once all collision points have been collected for the frame, they go through the collision solver that takes each collision, & adds an impulse to the shape to bring the relative velocity <= 0.  We go through the list of contacts several times (the iterative rigid body solver), to stabilise the simulation.
I'm not sure what it uses for discrete collisions?
What things I don't understand is
1. When do we know we don't need a collision point anymore? When the collision points are moving away?
2. How do we know a contact point is unique with gjk/barycentric coords? Or do we just add them and wait till they get pulled off the list?
3. If the toi is less than one & we want to use the rest up, do we run the whole collision routine again for a max say 4 times, to use it up, and where does this fit into the iterative solver?
I kind of just want to get an overview of how a physics engine fits together and the parts or algorithms we need.   Its just for a hobby engine, and with understanding as a priority.  Any help would be great, and sorry if the question is a bit over simplified.

• I am developing a simply 2d physics engine and I' ve read the source code of box2d and matter.js. I found some insteresting thing on finding contact point:
Box2d use indicent edge, reference edge and clip to determine a contact point. It's very complex method and hard to understand.
However, matter.js use a very simply method to do the same thing. just find the vertex that contained by opposite polygon with hill-climbing:
var verticesB = SAT._findSupports(bodyA, bodyB, collision.normal), supports = []; // find the supports from bodyB that are inside bodyA if (Vertices.contains(bodyA.vertices, verticesB[0])) supports.push(verticesB[0]); if (Vertices.contains(bodyA.vertices, verticesB[1])) supports.push(verticesB[1]); // find the supports from bodyA that are inside bodyB if (supports.length < 2) { var verticesA = SAT._findSupports(bodyB, bodyA, Vector.neg(collision.normal)); if (Vertices.contains(bodyB.vertices, verticesA[0])) supports.push(verticesA[0]); if (supports.length < 2 && Vertices.contains(bodyB.vertices, verticesA[1])) supports.push(verticesA[1]); } and it works well in demo.
My question is: why does box2d use such complex method? It's clear that matter.js's method is better. Or there are some potential shortcomings in matter.js's method?

• Hey there,
I recently came across this: https://github.com/ssloy/tinyraytracer
It's a raytracer implemented in a very minimal way.
This person uses a method to find intersections between a ray and a sphere that I can't understand. His implementation is as follows:
bool ray_intersect(const Vec3f &orig, const Vec3f &dir, float &t0) const { Vec3f L = center - orig; float tca = L*dir; float d2 = L*L - tca*tca; if (d2 > radius*radius) return false; float thc = sqrtf(radius*radius - d2); t0 = tca - thc; float t1 = tca + thc; if (t0 < 0) t0 = t1; if (t0 < 0) return false; return true; } This code confuses me, because I've always used the quadratic formula to get the discriminant of the ray-sphere function. It looks like he is doing the same thing, but perhaps using a different formula to begin with?
If someone has the time I'd appreciate an explanation or a breakdown of this method.

• I'm currently implementing a 2D physics engine, then i stuck in collision resolution. So i did a search in google, and i found this passage, It explains how to use impulse to separate two objects.
But after that, i found another another passage, this article is very complicated, but they seem talking the same thing ——— how to implementing the collision resolution. So now, what i confuse is why are there two completely different approaches of collision resolution? What's the difference between them? Which approaches is right?
I am not a native english speakers, so i hope you can understand what i wrote.
Thanks.

• I am a beginner in computer graphics but i have solid understanding of linear algebra. I am trying to implement my own forward kinematics for a skeleton. I defined global and local translation vector in child-parent relationships but i am struggling with propagation of rotations from the node n to n+1, n+2 ...
I figured that motion builder uses XYZ rotation order. I created a matrix to transform child vector and "rotate it".
But if i have 3 joints where 1st is parent to 2nd and 2nd is parent to 3rd, if i rotate 1st joint say (10,20,0) locally, i get accurate global position for 2nd and 3rd joint but if i have a local rotation of 2nd joint say (20,0, 0) on top of (10, 20, 0) for the 1st, my 3rd joint's position is wrong. I notice there should be some stacking so that i should probably do (10+20, 20+0, 0+0) for the second rotation but i am not sure why is that the case.
If i could get some links or a book that will address this topic, i would appreciate it. Thanks!
×