Use contact manifolds instead of single contacts for collisions

[Jondolf]

Currently, each collision only uses one contact. When a box is on the ground, the contact point oscillates between the corners, which can often cause significant instability and positional drift.

This PR changes this behaviour by computing multiple contact points using contact manifolds. This allows all four corners of a cube to be in contact at the same time, which essentially eliminates drifting, closing #85.

Comparison

Before:

2023-07-19.21-10-34.mp4

After:

after.mp4

As you can see, the cubes now land in a much more stable way, drifting is almost nonexistent, and explosions or small jumps are significantly reduced.

Caveats and future work

Small jumps

Edit: Fixed this for convex colliders, but non-convex colliders are still buggy. Read my comment below this post.

Despite drifting being essentially removed and explosions being heavily reduced, there are still occasional jumps, which might be because the constraint only uses the initial penetration depths (from Parry) instead of computing them based on the current state, which would cause the applied correction to be too large.

This seems to be mitigated by manually computing the penetration depth based on contact points computed at the current state, just like the XPBD paper recommends:

let p1 = body1.position.0
    + body1.accumulated_translation.0
    + body1.rotation.rotate(self.local_r1);
let p2 = body2.position.0
    + body2.accumulated_translation.0
    + body2.rotation.rotate(self.local_r2);
let penetration = (p1 - p2).dot(normal);

However, this causes problems with trimesh colliders, so I didn't add it yet. This will have to be investigated later.

Non-convex colliders

These changes only fix convex-convex collisions, and non-convex colliders (like trimeshes) still have the old behaviour. Read my comment below this post.

Performance

As you might expect, using multiple contact points instead of one can be more expensive, but the difference seems to actually be quite small.

Parry supports reusing the previous contact manifold to speed up the computation, so it could even be faster than the old method if implemented correctly. I didn't implement this yet due to facing more problems, but it will have to be tried again, as it should give a significant performance boost.

API change

Collisions are no longer just singular contacts, so I added a Contacts struct for a collision between two entities. It contains the entities and a Vec<ContactManifold>, where each contact manifold contains the entities, a normal, and the contacts, which are represented as Vec<Contact>.

This is quite a few added layers, so it would be nice to add simple methods to e.g. get all contacts as a flat structure or to compute the deepest contact.

[Jondolf]

I made a few more changes. I noticed that this causes convex meshes (like the cubes) to sink into non-convex meshes (like trimeshes), so I had to revert it so that collisions involving non-convex meshes keep the old single contact point behaviour. This means that collisions against trimeshes will still be a bit buggy for now, but collisions between convex shapes should be much better. I will open an issue for this later.

Additionally, I changed the penetration constraint to compute the penetration depth for convex colliders based on the current state like I described in my previous message. This seems to eliminate pretty much all of the remaining explosiveness, as I'm no longer seeing any jumping cubes. However, for non-convex colliders the problem still persists as I said.

Finally, I also changed the computation of r1 and r2 to take into account the current pose of the bodies instead of using the original world_r1 and world_r2. I'm not 100% sure if this is how it should be implemented, but other people and the original paper seem to suggest so. I'm not seeing a noticeable difference, but I believe it's now correct.

I'll probably merge these changes this evening and open an issue for the non-convex collider case.