Fix contact stability for non-convex colliders

[Jondolf]

Fixes #93, #108, #158 and mostly fixes #2.

Objective

Currently, trimeshes and compound colliders use the old single contact point approach, which is often quite unstable and causes positional drift. In addition, compound colliders "explode" very easily when they are off-center.

Solution

I found that the penetration depth for contact manifolds seems to be computed incorrectly. It is relative to the center of mass and not the position of each body, so for off-center colliders, it's adding the centers of mass to the contact positions, which results in an incorrect penetration depth. Fixing this makes it possible to use contact manifolds for all collider types and compute the penetration depth manually at the start of each constraint solve.

I also noticed that contact positions for Parry's compound colliders are relative to each subshape, not the entire collider itself. Transforming the contact positions by the subshape positions fixes the issues.

Unsolved issues

Pretty much all colliders seem to be stable now, but convex hulls can sometimes have very weird collision issues in edge cases where the individual faces are large.

2023-09-17.13-14-09.mp4

Sometimes colliders pass through the ground, vanish or explode, and they often bump into the edges. The problems are epecially prevalent near edges.

The contact normals and contact points seem to be computed incorrectly for convex polyhedra sometimes. This is an issue in parry, as it also affects rapier, so I opened an issue here: dimforge/parry#166

As this seems to be an issue in parry and not bevy_xpbd, and it's only an issue in quite specific situations, I'm fine with merging this PR as it fixes a lot of much more common issues.