Store local contact normals and transform them into world-space at each solve

src/collision.rs

@@ -43,22 +43,19 @@ pub struct ContactManifold {
     pub entity2: Entity,
     /// The contacts in this manifold.
     pub contacts: Vec<ContactData>,
-    /// A world-space contact normal shared by all contacts in this manifold.
+    /// A contact normal shared by all contacts in this manifold,
+    /// expressed in the local space of the first entity.
     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.
-    pub local_point2: Vector,
-    /// Contact point on the first entity in global coordinates.
     pub point1: Vector,
-    /// Contact point on the second entity in global coordinates.
+    /// Contact point on the second entity in local coordinates.
     pub point2: Vector,
-    /// Contact normal from contact point 1 to 2 in world coordinates.
+    /// A contact normal expressed in the local space of the first entity.
     pub normal: Vector,
     /// Penetration depth.
     pub penetration: Scalar,
@@ -67,6 +64,25 @@ pub struct ContactData {
     pub(crate) convex: bool,
 }
 
+impl ContactData {
+    /// Returns the global contact point on the first entity,
+    /// transforming the local point by the given entity position and rotation.
+    pub fn global_point1(&self, position: &Position, rotation: &Rotation) -> Vector {
+        position.0 + rotation.rotate(self.point1)
+    }
+
+    /// Returns the global contact point on the second entity,
+    /// transforming the local point by the given entity position and rotation.
+    pub fn global_point2(&self, position: &Position, rotation: &Rotation) -> Vector {
+        position.0 + rotation.rotate(self.point2)
+    }
+
+    /// Returns the world-space contact normal pointing towards the exterior of the first entity.
+    pub fn global_normal(&self, rotation: &Rotation) -> Vector {
+        rotation.rotate(self.normal)
+    }
+}
+
 /// Computes one pair of contact points between two shapes.
 #[allow(clippy::too_many_arguments)]
 pub(crate) fn compute_contacts(
@@ -103,16 +119,14 @@ pub(crate) fn compute_contacts(
                 .map(|manifold| ContactManifold {
                     entity1,
                     entity2,
-                    normal: rotation1.rotate(manifold.local_n1.into()),
+                    normal: 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()),
+                            point1: contact.local_p1.into(),
+                            point2: contact.local_p2.into(),
+                            normal: manifold.local_n1.into(),
                             penetration: -contact.dist,
                             convex,
                         })
@@ -134,11 +148,9 @@ pub(crate) fn compute_contacts(
             )
             .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()),
+                point1: contact.point1.into(),
+                point2: contact.point2.into(),
+                normal: contact.normal1.into(),
                 penetration: -contact.dist,
                 convex,
             });

src/constraints/penetration.rs

@@ -15,13 +15,9 @@ pub struct PenetrationConstraint {
     /// Data associated with the contact.
     pub contact: ContactData,
     /// Vector from the first entity's center of mass to the contact point in local coordinates.
-    pub local_r1: Vector,
+    pub r1: Vector,
     /// Vector from the second entity's center of mass to the contact point in local coordinates.
-    pub local_r2: Vector,
-    /// Vector from the first entity's center of mass to the contact position in world coordinates.
-    pub world_r1: Vector,
-    /// Vector from the second entity's center of mass to the contact position in world coordinates.
-    pub world_r2: Vector,
+    pub r2: Vector,
     /// Lagrange multiplier for the normal force.
     pub normal_lagrange: Scalar,
     /// Lagrange multiplier for the tangential force.
@@ -57,13 +53,11 @@ impl XpbdConstraint<2> for PenetrationConstraint {
         // 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);
+            let p1 =
+                body1.position.0 + body1.accumulated_translation.0 + body1.rotation.rotate(self.r1);
+            let p2 =
+                body2.position.0 + body2.accumulated_translation.0 + body2.rotation.rotate(self.r2);
+            self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation));
         }
 
         // If penetration depth is under 0, skip the collision
@@ -72,6 +66,20 @@ impl XpbdConstraint<2> for PenetrationConstraint {
         }
 
         self.solve_contact(body1, body2, dt);
+
+        if self.contact.convex {
+            let p1 =
+                body1.position.0 + body1.accumulated_translation.0 + body1.rotation.rotate(self.r1);
+            let p2 =
+                body2.position.0 + body2.accumulated_translation.0 + body2.rotation.rotate(self.r2);
+            self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation));
+        }
+
+        // If penetration depth is under 0, skip the collision
+        if self.contact.penetration <= Scalar::EPSILON {
+            return;
+        }
+
         self.solve_friction(body1, body2, dt);
     }
 }
@@ -83,20 +91,15 @@ impl PenetrationConstraint {
         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;
-
-        let world_r1 = body1.rotation.rotate(local_r1);
-        let world_r2 = body2.rotation.rotate(local_r2);
+        let r1 = contact.point1 - body1.center_of_mass.0;
+        let r2 = contact.point2 - body2.center_of_mass.0;
 
         Self {
             entity1: body1.entity,
             entity2: body2.entity,
             contact,
-            local_r1,
-            local_r2,
-            world_r1,
-            world_r2,
+            r1,
+            r2,
             normal_lagrange: 0.0,
             tangent_lagrange: 0.0,
             compliance: 0.0,
@@ -113,12 +116,12 @@ impl PenetrationConstraint {
         dt: Scalar,
     ) {
         // Shorter aliases
-        let penetration = self.contact.penetration;
-        let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.normal_lagrange;
-        let r1 = body1.rotation.rotate(self.local_r1);
-        let r2 = body2.rotation.rotate(self.local_r2);
+        let penetration = self.contact.penetration;
+        let normal = self.contact.global_normal(&body1.rotation);
+        let r1 = body1.rotation.rotate(self.r1);
+        let r2 = body2.rotation.rotate(self.r2);
 
         // Compute generalized inverse masses
         let w1 = self.compute_generalized_inverse_mass(body1, r1, normal);
@@ -147,20 +150,20 @@ impl PenetrationConstraint {
         dt: Scalar,
     ) {
         // Shorter aliases
-        let penetration = self.contact.penetration;
-        let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.tangent_lagrange;
-        let r1 = body1.rotation.rotate(self.local_r1);
-        let r2 = body2.rotation.rotate(self.local_r2);
+        let penetration = self.contact.penetration;
+        let normal = self.contact.global_normal(&body1.rotation);
+        let r1 = body1.rotation.rotate(self.r1);
+        let r2 = body2.rotation.rotate(self.r2);
 
         // Compute relative motion of the contact points and get the tangential component
         let delta_p1 =
             body1.position.0 - body1.previous_position.0 + body1.accumulated_translation.0 + r1
-                - body1.previous_rotation.rotate(self.local_r1);
+                - body1.previous_rotation.rotate(self.r1);
         let delta_p2 =
             body2.position.0 - body2.previous_position.0 + body2.accumulated_translation.0 + r2
-                - body2.previous_rotation.rotate(self.local_r2);
+                - body2.previous_rotation.rotate(self.r2);
         let delta_p = delta_p1 - delta_p2;
         let delta_p_tangent = delta_p - delta_p.dot(normal) * normal;
 

src/plugins/debug/mod.rs

@@ -139,6 +139,7 @@ fn debug_render_aabbs(
 }
 
 fn debug_render_contacts(
+    colliders: Query<(&Position, &Rotation), With<Collider>>,
     mut collisions: EventReader<Collision>,
     mut debug_renderer: PhysicsDebugRenderer,
     config: Res<PhysicsDebugConfig>,
@@ -147,10 +148,17 @@ fn debug_render_contacts(
         return;
     };
     for Collision(contacts) in collisions.iter() {
+        let Ok((position1, rotation1)) = colliders.get(contacts.entity1) else {
+            continue;
+        };
+        let Ok((position2, rotation2)) = colliders.get(contacts.entity2) else {
+            continue;
+        };
+
         for manifold in contacts.manifolds.iter() {
             for contact in manifold.contacts.iter() {
-                let p1 = contact.point1;
-                let p2 = contact.point2;
+                let p1 = contact.global_point1(position1, rotation1);
+                let p2 = contact.global_point2(position2, rotation2);
                 #[cfg(feature = "2d")]
                 let len = 5.0;
                 #[cfg(feature = "3d")]

src/plugins/solver.rs

@@ -382,9 +382,9 @@ fn solve_vel(
                 continue;
             }
 
-            let normal = constraint.contact.normal;
-            let r1 = body1.rotation.rotate(constraint.local_r1);
-            let r2 = body2.rotation.rotate(constraint.local_r2);
+            let normal = constraint.contact.global_normal(&body1.rotation);
+            let r1 = body1.rotation.rotate(constraint.r1);
+            let r2 = body2.rotation.rotate(constraint.r2);
 
             // Compute pre-solve relative normal velocities at the contact point (used for restitution)
             let pre_solve_contact_vel1 = compute_contact_vel(