Main Physics Update

• Update positions/rotations
• Update velocity (add external forces, eg. gravity)
• Collision detection
• Collision resolution
• Add stored impulses to velocities, reset impulses, dampen velocities

I think I've got this part right, my biggest question here is if I'm treating the angular velocity properly. The collision resolution treats body.angularVelocity as a linear vector (simply adds rotations to it), however my final rotation works by converting body.angularVelocity to a quaternion and multiplying the body.quaternion by it.

Collision Detection

• Broadphase detection
• GJK+EPA
• Add/update contacts to the contact manifold

I believe my collision detection works, but the most important question is: is what I'm returning the same thing that SI/PGS is expecting? GJK+EPA returns a normal, a penetration depth, and 1 contact point: essentially EPA finishes with a support point and a triangle in the minkowski configuration. Two contacts can be found by first retrieving the bayesian coefficients (u, v, w) with respect to the last projected point, and multiplying those coefficients with the associated support points:

contact.pointA = triangle.v1.support_a*u + triangle.v2.support_a*v + triangle.v3.support_a*w
contact.pointB = triangle.v1.support_b*u + triangle.v2.support_b*v + triangle.v3.support_b*w
contact.point = (contact.pointA + contact.pointB) * 0.5

Where our contact point is simply the midpoint of contact.pointA and contact.pointB. The new contact is added to the bodyA/bodyB manifold. If the same contact already exists (the pair of associated vertices for each support point {support_a.vertex_index, support_b.vertex_index} is the same in an existing contact) then that contact is updated with the new information. Otherwise the new contact is added. If our manifold exceeds 4 points then delete the weakest contact.

Since GJK+EPA only returns 1 contact point, I'm maintaining the other contact points by updating their penetration depth:

contact.depth = contact.normal.dot( bodyA.point(contact.point.support_a) - bodyB.point(contact.point.support_b) )

Then if the new depth < 0 (no longer penetrating) or depth > 1.0 (penetrating too far, probably fell through) simply delete the contact.

Collision Resolution

• For each contact: solve penetrations, warmstart impulses, calculate b for baumgarte stabilization
• For each contact: For each iteration: find delta lambda and add impulse

Solve Penetrations

contact.rA = contact.pointA - bodyA.position
contact.tangent = contact.rA.cross(contact.normal)
bodyA.d = bodyA.invMass + contact.normal.dot( contact.tangent.cross(contact.rA) * bodyA.invInertia )
contact.d = 1.0 / (bodyA.d + bodyB.d)
dVA = contact.normal.dot( bodyA.velocity ) + contact.tangent.dot( bodyA.angularVelocity )
immediateLinearImpulse = contact.normal * body.invMass * (contact.depth - (dVA * contact.d) - (dVB * contact.d))
immediateAngularImpulse = contact.tangent * body.invInertia * (contact.depth - (dVA * contact.d) - (dVB * contact.d))

both of the immediate impulses are directly added to each body's linear/angular velocity. I feel iffy about simply adding the angular impulse, and am not sure if I'm doing this right. Also I'm treating invInertia as a scalar value (0.5) rather than a matrix, I hope this is okay?

Warmstart Impulses

Dampen contact's stored impulse and apply to each body

contact.impulse *= warmstart // warmstart ~0.8
body.applyLinearImpulse( contact.normal * body.invMass * contact.impulse )
body.applyAngularImpulse( contact.tangent * body.invInertia * contact.impulse )

Note that the impulses don't affect the velocity until after the collision resolution.

Baumgarte Stabilization

contact.b = -baumgarte * JDotV * dt  // baumgarte ~0.2

Sequential Impulses

M = (bodyA.invMass, bodyA.invInertia, bodyB.invMass, bodyB.invInertia)
B = J*M
JDotA = J.dot(B) * contact.impulse
D = J.dot(B)
delta = (contact.b - JDotB) / D
contact.impulse = clamp(contact.impulse + delta, 0, Infinity)
 
body.applyLinearImpulse( contact.normal * body.invMass * delta )
body.applyAngularImpulse( contact.tangent * body.invInertia * delta )

Note that contact.impulse is stored for the next time around, and the body's impulses are reset by the end of the physics engine update. Again I'm unsure about adding the angular impulse like this, and also treating the invInertia as a scalar. But more importantly, I'm not sure that my sequential solver is correct at all..

I could really use some advice, if anybody here knows what I'm doing wrong or if I misunderstood something please let me know.

Videos

With sequential impulses

Physics engine with PGS (sequential impulses) enabled, with 10 iterations. The results are....bad.

Without sequential impulses

The cube falls right through the floor

No resting contact, and completely bizzare behaviour with cubes over the edge

Even without sequential impulses the cube shouldn't fall into the floor this way! The impulses will be negative but in the video looks very weird.

I would suggest:

Disable BroadPhase;
Review your GJK and EPA;
Disable contact caching;
Check your quaternions, matrices, vectors;
Each constraint is handled separately then each delta velocity is calculated after each one;

Erin Catto slides are recommended.

Contact caching is mandatory. This is why some people spend their effort in algos such SAT that gets up to X contact points with clipping.

Just keep the local contact points (the two points in this case). You only need a point returned from EPA, normal, and penetration to get the two. This is how you calculate the 2nd point:

WorldOnB = WorldOnA + WorldNormalOutOfA • Penetration (flip sign if needed)

LocalOnA = InverseObjectAWorldMatrix • WorldOnA
LocalOnB = InverseObjectBWorldMatrix • WorldOnB

Use Bullet btPersistentManifold to calculate the biggest contact area in the direction of the gravity.

This is how the flow if you're using sequential impulses works:

BroadPhase();
NarrowPhase(); (Update World Points, etc.)
ApplyForces();

WarmStart();
ApplyImpulses(); (10 times or less);
ApplyVelocities(); (update positions);

Looks to me mostly to be buggy collision detection code.

Some notes on debugging:

• Implement frame by frame stepping controlled by a keypress
• Make sure the results are deterministic on your machine
• Render points of contact and their associated normals
• Pre-place a physics object (cube) in code so you can see the same simulation upon restart (don't use the mouse to move things)
• Check out example source code as reference (ODE has function dBoxBox, qu3e has q3BoxToBox, Cyclone Physics has some SAT code)
• Reduce variables to isolate problem. Turn off collision resolution and make sure you always generate correct contact points

Once you are sure your collision detection code is working, then you can turn on resolution and debug it in isolation.

Hey guys, I really appreciate your advice and help here. I've managed to fix up most of the bugs and drastically improve my physics engine since then. I've got a few new questions given the current state of my program..

Side note: The problems that needed fixing were

• I was applying the angular velocity to my quaternion rotation completely wrong
• My collision detection was only providing 1 valid contact point (the other one was invalid)
• I needed to maintain the contact manifold (update contact points, and remove the contact if the two points are orthogonally too far away)
• Had to completely change around my collision resolution system (only apply sequential impulses, no pre solver)
• Needed to limit the maximum physics delta time (update the physics engine repeatedly with a maximum of 20ms per step)
• Added friction

Where I'm at now is a mostly stable system, with 3 primarily noticeable bugs:

Slow resolution

If a box is forced into the ground, its collision resolution (pushed out of the ground) is solved extremely slowly. I'm assuming that the box should be shot out much faster

Swaying stack

Notice how over the course of 30 minutes, the boxes slowly move out of place and the stack is continuously swaying back and forth. After another 15 minutes the stack was close to toppling over. My guess is that I have to enable sleeping to bodies which have a small amount of velocity being applied to them. This seems really hacky, is there a better way?

Large stack

Anything more than 5 boxes results in the stack toppling over. I'm not even sure what can be done to fix this?

Any tips or insight would be really helpful. Thanks!

Could still be buggy collision detection, as I don't see you rendering contacts along with their normals in any of those videos. Could also be some incorrect math in the solver. You'll have to compare your code against a reference, either your own derivations or someone else's implementation.