Game Physics: Resolution – Contact Constraints

This post is part of my Game Physics Series.

Contact constraints are amongst the most important constraints for a physics engine, because you would want to resolve collision in most scenarios. And if not done properly, your rigid bodies can behave in a very visually jarring way to the player.

Derivation

Remember that Sequential Impulse models constraints in the form of JV + b = 0, where J is the Jacobian matrix, V is the velocity vector, and b is the bias term. Erin Catto (author of Box2D) proposed a systematic way to derive velocity constraints: you first find the position constraint C, and then differentiate it with respect to time to obtain the velocity constraint \dot{C}. Sometimes, it is more intuitive to directly derive the velocity constraint itself, and I think it is the case for contact constraints. However, for the purpose of demonstration, I will first find the equation for the position constraint, and then derive the velocity constraint.

Based on the contact format previously described, a possible interface for contact data may look as follows.

struct Contact
{
  // contact point data
  Vec3 globalPositionA;
  Vec3 globalPositionB;
  Vec3 localPositionA;
  Vec3 localPositionB;
 
  // these 3 vectors form an orthonormal basis
  Vec3 normal; // points from colliderA to colliderB
  Vec3 tangent1, tangent2;
 
  // penetration depth
  float depth; 
 
  // for clamping (more on this later)
  float normalImpulseSum;
  float tangentImpulseSum1;
  float tangentImpulseSUm2;
 
  Contact(void)
    : normalImpulseSum(0.0f)
    , tangentImpulseSum1(0.0f)
    , tangentImpulseSum2(0.0f)
  { }
 
  // there's typically more stuff
  // but omitted for simplicity's sake
};

Let us look at the figure below before moving onto finding the position constraint.

contacts-figure

C_A and C_B are centers of mass of collider A and collider B, respectively. P_A and P_B are two contact points (deepest penetrating points) on two colliders. \overrightarrow{r_A} and \overrightarrow{r_b} are vectors from centers of mass to contact points. \overrightarrow{n} is the contact normal (always pointing from collider A to collider B for consistency). \overrightarrow{t_1} and \overrightarrow{t_2} are contact tangents, and \overrightarrow{t_2} is pointing “into” the screen.

We basically want our penetration depth to be zero, so the position constraint states that: the vector from P_A to P_B must be in the same direction as \overrightarrow{n} (the dot products of both vectors, i.e. the separation, is \ge 0).

We can see that:

    \[P_A = C_A + \overrightarrow{r_A}\]


    \[P_B = C_B + \overrightarrow{r_B}\]

So, the position constraint C is:

    \[C \, : \, (P_B - P_A) \cdot \overrightarrow{n} \ge 0\]


    \[C \, : \, (C_B + \overrightarrow{r_B} - C_A - \overrightarrow{r_A}) \cdot \overrightarrow{n} \ge 0\]

By differentiating C with respect to time, we get the velocity constraint \dot{C}:

    \[\dot{C} \, : \, (-\overrightarrow{V_A} - \overrightarrow{\omega_A} \times \overrightarrow{r_A} + \overrightarrow{V_B} + \overrightarrow{\omega_B} \times \overrightarrow{r_B}) \cdot \overrightarrow{n} \ge 0\]

By some algebraic manipulation and making use of the triple product identity \overrightarrow{A} \times \overrightarrow{B} \cdot \overrightarrow{C} = \overrightarrow{C} \times \overrightarrow{A} \cdot \overrightarrow{B}, we get:

    \[\dot{C} \, : \, JV + b \ge 0,\]

where:

    \[J ={\left[ {\begin{array}{cccc}-\overrightarrow{n}^T & (-\overrightarrow{r_A} \times \overrightarrow{n})^T & \overrightarrow{n}^T & (\overrightarrow{r_B} \times \overrightarrow{n})^T \\\end{array} } \right]},\]

    \[V ={\left[ {\begin{array}{ccc}\overrightarrow{V_A} \\\overrightarrow{\omega_A} \\\overrightarrow{V_B} \\\overrightarrow{\omega_B} \\\end{array} } \right]}\]

    \[b = 0\]

Note that the form of our velocity constraint is JV + b \ge 0 instead of JV + b = 0. However, don’t worry. This simply means that we impose the constraint JV + b = 0 when the two colliders are penetrating.

The geometric interpretation of the velocity constraint we just derived is as follows: the projection of the relative velocity of the two contact points onto the contact normal is zero. This means if we solve this constraint, the two contact points would not penetrate further.

Recall that the correction to the velocity vector V is \Delta V = M^{-1} J^T \lambda, where \lambda is the Lagrangian Multiplier:

    \[\lambda = \frac {-(JV + b)} {J M^{-1} J^T}\]

and M is the mass matrix:

    \[M ={\left[ {\begin{array}{cccc}M_A & 0 & 0 & 0 \\0 & I_A & 0 & 0 \\0 & 0 & M_B & 0 \\0 & 0 & 0 & I_B \\\end{array} } \right]},\]

Clamping The Lagrangian Multiplier

We have found out how to solve the contact constraint. However, this is not the entire picture. Remember there are usually more than one contact constraints in the entire physics system. The Sequential Impulse method iterates through all contacts and solve each contact constraints one-by-one:

for (Manifold manifold: manifolds)
  for (Contact contact : manifold.contacts)
    SolveContactConstraints(contact);

The change to a collider’s velocity will affect the result of later resolution of the same collider. At one point, you may end up with a velocity change that actually pulls a collider pairs closer instead of pushing them apart. For this reason, we need to perform some clamping when applying velocity change using \lambda. This is the reason why in the interface for contact shown earlier contains the data members normalImpulseSum.

We want to make sure that throughout all iterations within one time step, the following inequality is satisfied:

    \[\Sigma \lambda_i \ge 0,\]

where \lambda_i is the Lagrangian Multiplier computed during the i-th iteration.

Each time we compute \lambda for a contact, we make a copy of normalImpulseSum, add \lambda to normalImpulseSum, clamp normalImpulseSum between 0 and positive infinity, and then calculate the difference of the clamped normalImpulseSum from the previously copied normalImpulseSum. This difference is the actual Lagrangian Multiplier we want to use to solve the contact constraint.

Baumgarte Stabilization

Solving the velocity constraint is not enough. Remember that by solving the velocity constraint, we only prevent the two colliders from further penetrating. It does nothing to actually push them apart. The accumulated error results in “positional drift”.

This is when Baumgarte Stabilization comes into play. Some call this method a hack, some view it as an elegant way to fix positional drifts. Either way, it works.

The basic idea is to feed the penetration depth (position error) back into the velocity constraint as a bias, so just a little bit of energy is introduced into the system to push the two colliders apart.

As previously shown, the bias term b is zero. But it is not zero any more when Baumgarte Stabilization comes into the equation.

To compute the penetration depth d, we negate the separation formula:

    \[d = (P_B - P_A) \cdot (- \overrightarrow{n})\]

We then feed this back into the constraint equation JV + b = 0 after multiplying d with a Baumgarte term:

    \[b = -\frac{\beta}{\Delta t} \cdot d,\]

where \beta is typically a value between 0 and 1 (usually close to 0). Unfortunately, there is no one “correct” value for \beta; you will have to tweak this value based on the specific scenarios in your game. This is one of the minor quirks about constraint-based physics engines, lots of parameter tweaking. Erin Catto proposed that you start increasing the value from 0 until your physics system starts to become unstable, and then use half that value.

Restitution

Wait, what about having the colliders “bouncing” apart? So far by solving the velocity contact constraints only make sure that the two colliders do not penetrate any further. There is another quantity we are going to add to the bias term in the constraint equation in addition to the Baumgarte term: the restitution term.

The coefficient of restitution between two objects, denoted C_R, is defined as the ratio between the parting speed of two colliders after collision and the closing speed before collision; for realistic result, you would want to set this value between zero and one.

Therefore, the contribution of the restitution term to the bias term is simply the coefficient of restitution times the projection of the closing velocity of two contact points onto the normal vector:

    \[C_R (-\overrightarrow{V_A} - \overrightarrow{\omega_A} \times \overrightarrow{r_A} + \overrightarrow{V_B} + \overrightarrow{\omega_B} \times \overrightarrow{r_B}) \cdot \overrightarrow{n}\]

So our final bias term b with contribution from both the Baumgarte term and restitution term is:

    \[b = -\frac{\beta}{\Delta t} \cdot d + C_R (-\overrightarrow{V_A} - \overrightarrow{\omega_A} \times \overrightarrow{r_A} + \overrightarrow{V_B} + \overrightarrow{\omega_B} \times \overrightarrow{r_B}) \cdot \overrightarrow{n}\]

Frictions

So far we have only covered the normal part of contact constraints. Now I’m going to talk about the tangential part of contact constraints, a.k.a. frictions.

Frictional forces are proportional to normal forces, so they are dependent on the result of contact resolution in the normal direction. Thus, we typically solve the tangential part of contact constraints after the normal part.

The normal part of contact constraints attempts to zero out the projection of the relative velocity of two contact points onto the contact normal (without considering the bias term b, that is). The tangential part of contact constraints does something very similar: it attempts to zero out the projection of the relative velocity of two contact points onto the contact tangent plane (formed by the two contact tangents).

The tangential part and normal part of contact constraints are basically doing the same thing, except that the directions onto which the relative velocity of the two contact points are projected are different. So there is no surprise to see that the Jacobians for the two contact tangents look very similar to the Jacobian for the contact normal. Let’s relabel the Jacobian for the contact normal J_\overrightarrow{n}, and put it side-by-side with the Jacobians for the two contact tangents, denoted J_\overrightarrow{t_1} and J_\overrightarrow{t_2}.

    \[J_\overrightarrow{n} ={\left[ {\begin{array}{cccc}-\overrightarrow{n}^T & (-\overrightarrow{r_A} \times \overrightarrow{n})^T & \overrightarrow{n}^T & (\overrightarrow{r_B} \times \overrightarrow{n})^T \\\end{array} } \right]}\]


    \[J_\overrightarrow{t_1} ={\left[ {\begin{array}{cccc}-\overrightarrow{t_1}^T & (-\overrightarrow{r_A} \times \overrightarrow{t_1})^T & \overrightarrow{t_1}^T & (\overrightarrow{r_B} \times \overrightarrow{t_1})^T \\\end{array} } \right]}\]


    \[J_\overrightarrow{t_2} ={\left[ {\begin{array}{cccc}-\overrightarrow{t_2}^T & (-\overrightarrow{r_A} \times \overrightarrow{t_2})^T & \overrightarrow{t_2}^T & (\overrightarrow{r_B} \times \overrightarrow{t_2})^T \\\end{array} } \right]}\]

Pretty similar, right?

What is also similar is that the Lagrangian Multipliers computed for the contact tangents also need clamping. However, instead of clamping between zero and positive infinity, the boundaries the Lagrangian Multipliers are clamped depend on the normal impulse sum, which is stored in the normalImpulseSum data member. The clamping boundaries are negative and positive normalImpulseSum multiplied by the coefficient of friction, denoted C_F of the two colliders, which is typically between zero and one. Let us also denote normalImpulseSum as \lambda_\overrightarrow{n}_{sum}.

The clamping process is extremely similar to the normal part: we make copies of tagentImpulseSum1 and tangentImpulseSum2, add the Lagrangian Multipliers computed from both tangential Jacobians to tangentImpulseSum1 and tangentImpulseSum2, clamp tangentImpulseSum1 and tangentImpulseSum2 between -C_F \lambda_\overrightarrow{n}_{sum} and C_F \lambda_\overrightarrow{n}_{sum}, and then respectively calculate the difference of the clamped tangentImpulseSum1 from the previously copied tangentImpulseSum1 and the difference of the clamped tangentImpulseSum2 from the previously copied tangentImpulseSum2. These differences are the actual Lagrangian Multipliers we want to use to solve the tangential part of contact constraints.

You might notice that this clamping method is not perfect. We can end up with clamping the magnitude of total tangential impulse to \sqrt{2} C_F \lambda_\overrightarrow{n}_{sum}. However, this error is usually not noticeable, so I usually just let it be (Joshua Davis, a member of DigiPen Institute of Technology, informed me of this idea).

End of Contact Constraints

This is the end of the derivation of contact constraints and demonstration on how to solve them. I will cover stability issues and how to improve stability using a technique called “warm starting” in later posts.

About Allen Chou

Physics / Graphics / Procedural Animation / Visuals
This entry was posted in Gamedev, Physics. Bookmark the permalink.

23 Responses to Game Physics: Resolution – Contact Constraints

  1. Jay says:

    I implemented this method for multiple corners of a cube to prevent it from falling through the floor.

    Most things seem to work well, however if I place the cube on an incline it will automatically slide UP the incline.

    Any thoughts on where that sort of error might be?

    • Allen Chou says:

      Nothing immediately comes to mind. Unfortunately, I can only give the boring suggestion of stepping through the calculations to figure out where the upward velocity component comes from.

      • Jay says:

        Thanks. The issue was my contact normal. I didn’t do the transformation correctly between by code and the 3d model code I’m working with (which is left handed).

        I have one comment about the line C_dot : JV+b >= 0. Either I misunderstood something, or maybe there needs to be a clarification. It seems that if the contact normal always points from object A to object B then C_dot will always be less than 0 if the two objects are closing on one another (even when not making contact). There has to be something to flip the sign, like having the normal direct be scaled by the sign of the penetration depth.

        • Allen Chou says:

          C_dot is expected to violate the constraint whenever the two objects are closing on one another. The velocity constraint JV + b >= 0 is only enforced when there’s actual penetration.

  2. John M. says:

    Thank you, Allen! Using these articles as a guide, I was able to create a simple 2D physics system (no rotation) using circles and boxes. At first it was a bit strange thinking in terms of constraint systems, but it helped to write down and understand each step. I’m pleased with the stability and flexibility of the simulations.

  3. Helped me a lot to understand it! In 2D, what will the mass matrix be?

  4. Asylum says:

    Hi!

    How do you derive that differentiating rB is wB x rB?

    Thx

  5. gbatt says:

    Thanks, I think I get it now.

  6. gbatt says:

    Hi, I’m working on a physics engine based on constraints. At the point “Clamping the Langrangian Multiplier” I dont understand what you are doing. What is the variable normalImpulseSum for? Is it just the sum of the impulses of the two bodies? And when I read in Erin Catto’s papers he made a large matrix containing all contraints at the same time. Is this a similiar method to the one you’re describing? By the way great website, took me a great step forward:)

    • Allen Chou says:

      1. Throughout the iterations within one time step, sometimes you will get a negative normal impulse for one contact as a result of resolving other contacts., which will pull penetrating objects even closer; this is not what we want. The variable normalImpulseSum is for keeping track of the total normal impulse for a contact throughout the iterations to ensure that it never becomes negative.

      2. Theoretically, we will get the optimal results by solving all constraints at once in a big matrix. But this is not possible due to limits on computational power. So we use the Sequential Impulse approach, where we solve one to a few constraints at a time, and the we iterate through all constraints multiple times in a single time step.

  7. Xavi says:

    Hi Allen, i have tried to implement it, but does not work, can i see your implementation please?

    • Allen Chou says:

      The only implementation I have is the one I did for my school project at DigiPen, which I am not allowed to disclose because it belongs to DigiPen. However, you can check out Erin Catto’s implementation in Box2D. I used it as a reference.

      Or, you can show your implementation using online paste tools like Pastebin so I can take a look at what’s wrong.

  8. Xavi says:

    Hi, according to this method, every thing will be solved by velocity changes, but i’m thinking of where is the position constraint solver?
    don’t we need to apply Baumgarte directly on our bodies position?

    • Allen Chou says:

      By applying Baumgarte stabilization to solve velocity constraints, we are feeding back positional errors into velocity changes. This has the effect of correcting the positional errors over a few time steps, instead of instantaneously. To apply instant positional correction, you basically integrate the position using the velocity change computed from Baumgarte stabilization and the delta time. This approach can potentially give you faster positional correction, but might introduce more jitter if you have too many conflicting positional constraints. I personally prefer just using Baumgarte stabilization to solve for velocity constraints. It takes multiple frames to correct positional errors, but it’s visually smoother and more stable.

  9. Randy Gaul says:

    Hi there Erin, I imagine this is because the constraint space for contact constraints is relative to the collision normal itself. Would this be why the orientation of the collision normal in world space is irrelevant?

  10. Erin Catto says:

    Hi Allen. I like your site and you have a nice blog! Also congratulations on your position at Naughty Dog!
    When you differentiate C, did you consider that the normal vector is not constant? It can rotate with time. But somehow it doesn’t matter that you skipped it. Why?

    • Allen Chou says:

      Wow, it’s Erin Catto himself! I am a big fan of your work, and I love your presentations at GDC and DigiPen.

      Actually, I didn’t consider the fact that the normal vector is not constant when I derived the constraint equation, and everything works quite fine for me. Now that you mentioned it, I should probably go back and check my derivation…

      [Edit]
      On a second thought, a new normal is computed upon every new contact data generation. Normals are treated as constant only when caching (warm starting) is at work, which happens due to frame coherency, so the normals shouldn’t change too much. Do you think it still matters?

      • Nick Kovac says:

        Hi Allen, thanks a lot for writing this blog. It’s awesome and I’ve learned a lot. I was wondering about this issue too… The neglected term is basically the penetration depth vector dotted with the derivative of the normal vector. If you look at Erin Catto’s paper “iterative dynamics with temporal coherence”, he mentions that this term is usually neglected as an approximation because the penetration depth is usually small. I have two questions about that… firstly, since we usually use discrete collision detection, why is this assumption valid? Can’t the penetration depth be arbitrary? And also, since the chain rule guarantees C dot=Jv, doesn’t that mean we should still be able to simplify the entire expression into this form, including the neglected term?

  11. MrMac says:

    Best articles on the subject I have come across! I will need to implement constraints in a project I’m working on and thanks to these articles I’m starting to understand how it all works. Thank you very much. Keep writing them!

Leave a Reply to Allen Chou Cancel reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.