Merge pull request #90 from Jondolf/contact-manifolds

Use contact manifolds instead of single contacts for collisions

crates/bevy_xpbd_3d/examples/basic_kinematic_character.rs

@@ -134,14 +134,18 @@ fn kinematic_collision(
     mut bodies: Query<(&RigidBody, &mut Position)>,
 ) {
     // Iterate through collisions and move the kinematic body to resolve penetration
-    for Collision(contact) in collision_event_reader.iter() {
+    for Collision(contacts) in collision_event_reader.iter() {
         if let Ok([(rb1, mut position1), (rb2, mut position2)]) =
-            bodies.get_many_mut([contact.entity1, contact.entity2])
+            bodies.get_many_mut([contacts.entity1, contacts.entity2])
         {
-            if rb1.is_kinematic() && !rb2.is_kinematic() {
-                position1.0 -= contact.normal * contact.penetration;
-            } else if rb2.is_kinematic() && !rb1.is_kinematic() {
-                position2.0 += contact.normal * contact.penetration;
+            for manifold in contacts.manifolds.iter() {
+                for contact in manifold.contacts.iter() {
+                    if rb1.is_kinematic() && !rb2.is_kinematic() {
+                        position1.0 -= contact.normal * contact.penetration;
+                    } else if rb2.is_kinematic() && !rb1.is_kinematic() {
+                        position2.0 += contact.normal * contact.penetration;
+                    }
+                }
             }
         }
     }

src/collision.rs

@@ -1,12 +1,12 @@
 //! Collision events, contact data and helpers.
 
-use parry::query::QueryDispatcher;
+use parry::query::{PersistentQueryDispatcher, QueryDispatcher};
 
 use crate::prelude::*;
 
 /// A [collision event](Collider#collision-events) that is sent for each contact pair during the narrow phase.
 #[derive(Event, Clone, Debug, PartialEq)]
-pub struct Collision(pub Contact);
+pub struct Collision(pub Contacts);
 
 /// A [collision event](Collider#collision-events) that is sent when two entities start colliding.
 #[derive(Event, Clone, Debug, PartialEq)]
@@ -16,13 +16,40 @@ pub struct CollisionStarted(pub Entity, pub Entity);
 #[derive(Event, Clone, Debug, PartialEq)]
 pub struct CollisionEnded(pub Entity, pub Entity);
 
-/// Data related to a contact between two bodies.
-#[derive(Clone, Copy, Debug, PartialEq)]
-pub struct Contact {
+/// All contacts between two colliders.
+///
+/// The contacts are stored in contact manifolds.
+/// Each manifold contains one or more contact points, and each contact
+/// in a given manifold shares the same contact normal.
+#[derive(Clone, Debug, PartialEq)]
+pub struct Contacts {
+    /// First entity in the contact.
+    pub entity1: Entity,
+    /// Second entity in the contact.
+    pub entity2: Entity,
+    /// A list of contact manifolds between two colliders.
+    /// Each manifold contains one or more contact points, but each contact
+    /// in a given manifold shares the same contact normal.
+    pub manifolds: Vec<ContactManifold>,
+}
+
+/// A contact manifold between two colliders, containing a set of contact points.
+/// Each contact in a manifold shares the same contact normal.
+#[derive(Clone, Debug, PartialEq)]
+pub struct ContactManifold {
     /// First entity in the contact.
     pub entity1: Entity,
     /// Second entity in the contact.
     pub entity2: Entity,
+    /// The contacts in this manifold.
+    pub contacts: Vec<ContactData>,
+    /// A world-space contact normal shared by all contacts in this manifold.
+    pub normal: Vector,
+}
+
+/// Data related to a contact between two bodies.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ContactData {
     /// Contact point on the first entity in local coordinates.
     pub local_point1: Vector,
     /// Contact point on the second entity in local coordinates.
@@ -35,11 +62,14 @@ pub struct Contact {
     pub normal: Vector,
     /// Penetration depth.
     pub penetration: Scalar,
+    /// True if both colliders are convex. Currently, contacts between
+    /// convex and non-convex colliders have to be handled differently.
+    pub(crate) convex: bool,
 }
 
 /// Computes one pair of contact points between two shapes.
 #[allow(clippy::too_many_arguments)]
-pub(crate) fn compute_contact(
+pub(crate) fn compute_contacts(
     entity1: Entity,
     entity2: Entity,
     position1: Vector,
@@ -48,27 +78,82 @@ pub(crate) fn compute_contact(
     rotation2: &Rotation,
     collider1: &Collider,
     collider2: &Collider,
-) -> Option<Contact> {
+) -> Contacts {
     let isometry1 = utils::make_isometry(position1, rotation1);
     let isometry2 = utils::make_isometry(position2, rotation2);
     let isometry12 = isometry1.inv_mul(&isometry2);
+    let convex = collider1.is_convex() && collider2.is_convex();
 
-    if let Ok(Some(contact)) = parry::query::DefaultQueryDispatcher.contact(
-        &isometry12,
-        collider1.get_shape().0.as_ref(),
-        collider2.get_shape().0.as_ref(),
-        0.0,
-    ) {
-        return Some(Contact {
+    if convex {
+        // Todo: Reuse manifolds from previous frame to improve performance
+        let mut manifolds: Vec<parry::query::ContactManifold<(), ()>> = vec![];
+        let _ = parry::query::DefaultQueryDispatcher.contact_manifolds(
+            &isometry12,
+            collider1.get_shape().0.as_ref(),
+            collider2.get_shape().0.as_ref(),
+            0.0,
+            &mut manifolds,
+            &mut None,
+        );
+        Contacts {
+            entity1,
+            entity2,
+            manifolds: manifolds
+                .clone()
+                .into_iter()
+                .map(|manifold| ContactManifold {
+                    entity1,
+                    entity2,
+                    normal: rotation1.rotate(manifold.local_n1.into()),
+                    contacts: manifold
+                        .contacts()
+                        .iter()
+                        .map(|contact| ContactData {
+                            local_point1: contact.local_p1.into(),
+                            local_point2: contact.local_p2.into(),
+                            point1: position1 + rotation1.rotate(contact.local_p1.into()),
+                            point2: position2 + rotation2.rotate(contact.local_p2.into()),
+                            normal: rotation1.rotate(manifold.local_n1.into()),
+                            penetration: -contact.dist,
+                            convex,
+                        })
+                        .filter(|contact| contact.penetration > 0.0)
+                        .collect(),
+                })
+                .collect(),
+        }
+    } else {
+        // For some reason, convex colliders sink into non-convex colliders
+        // if we use contact manifolds, so we have to compute a single contact point instead.
+        // Todo: Find out why this is and use contact manifolds for both types of colliders.
+        let contact = parry::query::DefaultQueryDispatcher
+            .contact(
+                &isometry12,
+                collider1.get_shape().0.as_ref(),
+                collider2.get_shape().0.as_ref(),
+                0.0,
+            )
+            .unwrap()
+            .map(|contact| ContactData {
+                local_point1: contact.point1.into(),
+                local_point2: contact.point2.into(),
+                point1: position1 + rotation1.rotate(contact.point1.into()),
+                point2: position2 + rotation2.rotate(contact.point2.into()),
+                normal: rotation1.rotate(contact.normal1.into()),
+                penetration: -contact.dist,
+                convex,
+            });
+        Contacts {
             entity1,
             entity2,
-            local_point1: contact.point1.into(),
-            local_point2: contact.point2.into(),
-            point1: position1 + rotation1.rotate(contact.point1.into()),
-            point2: position2 + rotation2.rotate(contact.point2.into()),
-            normal: rotation1.rotate(contact.normal1.into()),
-            penetration: -contact.dist,
-        });
+            manifolds: contact.map_or(vec![], |contact| {
+                vec![ContactManifold {
+                    entity1,
+                    entity2,
+                    contacts: vec![contact],
+                    normal: contact.normal,
+                }]
+            }),
+        }
     }
-    None
 }

src/constraints/penetration.rs

@@ -13,7 +13,7 @@ pub struct PenetrationConstraint {
     /// Second entity in the constraint.
     pub entity2: Entity,
     /// Data associated with the contact.
-    pub contact: Contact,
+    pub contact: ContactData,
     /// Vector from the first entity's center of mass to the contact point in local coordinates.
     pub local_r1: Vector,
     /// Vector from the second entity's center of mass to the contact point in local coordinates.
@@ -47,14 +47,42 @@ impl XpbdConstraint<2> for PenetrationConstraint {
     /// Solves overlap between two bodies.
     fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
         let [body1, body2] = bodies;
+
+        // For convex-convex collider contacts, we can compute the penetration at the current state
+        // using the contact points like the XPBD paper suggests, which reduces explosiveness.
+        //
+        // However, non-convex colliders cause convex colliders to sink into them unless we use
+        // the penetration depth provided by Parry.
+        //
+        // Todo: Figure out why this is and use the method below for all collider types in order to fix
+        // explosions for all contacts.
+        if self.contact.convex {
+            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);
+            self.contact.penetration = (p1 - p2).dot(self.contact.normal);
+        }
+
+        // If penetration depth is under 0, skip the collision
+        if self.contact.penetration <= Scalar::EPSILON {
+            return;
+        }
+
         self.solve_contact(body1, body2, dt);
         self.solve_friction(body1, body2, dt);
     }
 }
 
 impl PenetrationConstraint {
     /// Creates a new [`PenetrationConstraint`] with the given bodies and contact data.
-    pub fn new(body1: &RigidBodyQueryItem, body2: &RigidBodyQueryItem, contact: Contact) -> Self {
+    pub fn new(
+        body1: &RigidBodyQueryItem,
+        body2: &RigidBodyQueryItem,
+        contact: ContactData,
+    ) -> Self {
         let local_r1 = contact.local_point1 - body1.center_of_mass.0;
         let local_r2 = contact.local_point2 - body2.center_of_mass.0;
 
@@ -89,13 +117,8 @@ impl PenetrationConstraint {
         let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.normal_lagrange;
-        let r1 = self.world_r1;
-        let r2 = self.world_r2;
-
-        // If penetration depth is under 0, skip the collision
-        if penetration <= 0.0 {
-            return;
-        }
+        let r1 = body1.rotation.rotate(self.local_r1);
+        let r2 = body2.rotation.rotate(self.local_r2);
 
         // Compute generalized inverse masses
         let w1 = self.compute_generalized_inverse_mass(body1, r1, normal);
@@ -128,8 +151,8 @@ impl PenetrationConstraint {
         let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.tangent_lagrange;
-        let r1 = self.world_r1;
-        let r2 = self.world_r2;
+        let r1 = body1.rotation.rotate(self.local_r1);
+        let r2 = body2.rotation.rotate(self.local_r2);
 
         // Compute relative motion of the contact points and get the tangential component
         let delta_p1 =

src/plugins/debug/mod.rs

@@ -146,23 +146,27 @@ fn debug_render_contacts(
     let Some(color) = config.contact_color else {
         return;
     };
-    for Collision(contact) in collisions.iter() {
-        let p1 = contact.point1;
-        let p2 = contact.point2;
-        #[cfg(feature = "2d")]
-        let len = 5.0;
-        #[cfg(feature = "3d")]
-        let len = 0.3;
-
-        debug_renderer.draw_line(p1 - Vector::X * len, p1 + Vector::X * len, color);
-        debug_renderer.draw_line(p1 - Vector::Y * len, p1 + Vector::Y * len, color);
-        #[cfg(feature = "3d")]
-        debug_renderer.draw_line(p1 - Vector::Z * len, p1 + Vector::Z * len, color);
-
-        debug_renderer.draw_line(p2 - Vector::X * len, p2 + Vector::X * len, color);
-        debug_renderer.draw_line(p2 - Vector::Y * len, p2 + Vector::Y * len, color);
-        #[cfg(feature = "3d")]
-        debug_renderer.draw_line(p2 - Vector::Z * len, p2 + Vector::Z * len, color);
+    for Collision(contacts) in collisions.iter() {
+        for manifold in contacts.manifolds.iter() {
+            for contact in manifold.contacts.iter() {
+                let p1 = contact.point1;
+                let p2 = contact.point2;
+                #[cfg(feature = "2d")]
+                let len = 5.0;
+                #[cfg(feature = "3d")]
+                let len = 0.3;
+
+                debug_renderer.draw_line(p1 - Vector::X * len, p1 + Vector::X * len, color);
+                debug_renderer.draw_line(p1 - Vector::Y * len, p1 + Vector::Y * len, color);
+                #[cfg(feature = "3d")]
+                debug_renderer.draw_line(p1 - Vector::Z * len, p1 + Vector::Z * len, color);
+
+                debug_renderer.draw_line(p2 - Vector::X * len, p2 + Vector::X * len, color);
+                debug_renderer.draw_line(p2 - Vector::Y * len, p2 + Vector::Y * len, color);
+                #[cfg(feature = "3d")]
+                debug_renderer.draw_line(p2 - Vector::Z * len, p2 + Vector::Z * len, color);
+            }
+        }
     }
 }