Use global coordinates in Contact points

src/collision.rs

@@ -1,7 +1,5 @@
 //! Collision events, contact data and helpers.
 
-use parry::shape::SharedShape;
-
 use crate::prelude::*;
 
 /// An event that is sent for each contact pair during the narrow phase.
@@ -23,14 +21,10 @@ pub struct Contact {
     pub entity1: Entity,
     /// Second entity in the contact.
     pub entity2: Entity,
-    /// Local contact point 1 in local coordinates.
-    pub local_r1: Vector,
-    /// Local contact point 2 in local coordinates.
-    pub local_r2: Vector,
-    /// Local contact point 1 in world coordinates.
-    pub world_r1: Vector,
-    /// Local contact point 2 in world coordinates.
-    pub world_r2: Vector,
+    /// Contact point on the first entity in global coordinates.
+    pub point1: Vector,
+    /// Contact point on the second entity in global coordinates.
+    pub point2: Vector,
     /// Contact normal from contact point 1 to 2.
     pub normal: Vector,
     /// Penetration depth.
@@ -40,34 +34,27 @@ pub struct Contact {
 /// Computes one pair of contact points between two shapes.
 #[allow(clippy::too_many_arguments)]
 pub(crate) fn compute_contact(
-    ent1: Entity,
-    ent2: Entity,
-    pos1: Vector,
-    pos2: Vector,
-    local_com1: Vector,
-    local_com2: Vector,
-    rot1: &Rotation,
-    rot2: &Rotation,
-    shape1: &SharedShape,
-    shape2: &SharedShape,
+    entity1: Entity,
+    entity2: Entity,
+    position1: Vector,
+    position2: Vector,
+    rotation1: &Rotation,
+    rotation2: &Rotation,
+    collider1: &Collider,
+    collider2: &Collider,
 ) -> Option<Contact> {
     if let Ok(Some(contact)) = parry::query::contact(
-        &utils::make_isometry(pos1, rot1),
-        shape1.0.as_ref(),
-        &utils::make_isometry(pos2, rot2),
-        shape2.0.as_ref(),
+        &utils::make_isometry(position1, rotation1),
+        collider1.get_shape().0.as_ref(),
+        &utils::make_isometry(position2, rotation2),
+        collider2.get_shape().0.as_ref(),
         0.0,
     ) {
-        let world_r1 = Vector::from(contact.point1) - pos1 + local_com1;
-        let world_r2 = Vector::from(contact.point2) - pos2 + local_com2;
-
         return Some(Contact {
-            entity1: ent1,
-            entity2: ent2,
-            local_r1: rot1.inverse().rotate(world_r1),
-            local_r2: rot2.inverse().rotate(world_r2),
-            world_r1,
-            world_r2,
+            entity1,
+            entity2,
+            point1: contact.point1.into(),
+            point2: contact.point2.into(),
             normal: contact.normal1.into(),
             penetration: -contact.dist,
         });

src/constraints/penetration.rs

@@ -14,6 +14,14 @@ pub struct PenetrationConstraint {
     pub entity2: Entity,
     /// Data associated with the contact.
     pub contact: Contact,
+    /// 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.
+    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,
     /// Lagrange multiplier for the normal force.
     pub normal_lagrange: Scalar,
     /// Lagrange multiplier for the tangential force.
@@ -45,11 +53,20 @@ impl XpbdConstraint<2> for PenetrationConstraint {
 }
 
 impl PenetrationConstraint {
-    pub fn new(entity1: Entity, entity2: Entity, contact: Contact) -> Self {
+    pub fn new(body1: &RigidBodyQueryItem, body2: &RigidBodyQueryItem, contact: Contact) -> Self {
+        let world_r1 = contact.point1 - body1.position.0 + body1.center_of_mass.0;
+        let world_r2 = contact.point2 - body2.position.0 + body2.center_of_mass.0;
+        let local_r1 = body1.rotation.inverse().rotate(world_r1);
+        let local_r2 = body2.rotation.inverse().rotate(world_r2);
+
         Self {
-            entity1,
-            entity2,
+            entity1: body1.entity,
+            entity2: body2.entity,
             contact,
+            local_r1,
+            local_r2,
+            world_r1,
+            world_r2,
             normal_lagrange: 0.0,
             tangent_lagrange: 0.0,
             compliance: 0.0,
@@ -69,14 +86,12 @@ impl PenetrationConstraint {
         let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.normal_lagrange;
-        let r1 = self.contact.world_r1;
-        let r2 = self.contact.world_r2;
+        let r1 = self.world_r1;
+        let r2 = self.world_r2;
 
         // Compute contact positions at the current state
-        let p1 = body1.position.0 - body1.center_of_mass.0
-            + body1.rotation.rotate(self.contact.local_r1);
-        let p2 = body2.position.0 - body2.center_of_mass.0
-            + body2.rotation.rotate(self.contact.local_r2);
+        let p1 = body1.position.0 + body1.rotation.rotate(self.local_r1);
+        let p2 = body2.position.0 + body2.rotation.rotate(self.local_r2);
 
         // Compute penetration depth
         let penetration = (p1 - p2).dot(normal);
@@ -117,18 +132,14 @@ impl PenetrationConstraint {
         let normal = self.contact.normal;
         let compliance = self.compliance;
         let lagrange = self.tangent_lagrange;
-        let r1 = self.contact.world_r1;
-        let r2 = self.contact.world_r2;
+        let r1 = self.world_r1;
+        let r2 = self.world_r2;
 
         // Compute contact positions at the current state and before substep integration
-        let p1 = body1.position.0 - body1.center_of_mass.0
-            + body1.rotation.rotate(self.contact.local_r1);
-        let p2 = body2.position.0 - body2.center_of_mass.0
-            + body2.rotation.rotate(self.contact.local_r2);
-        let prev_p1 = body1.previous_position.0 - body1.center_of_mass.0
-            + body1.previous_rotation.rotate(self.contact.local_r1);
-        let prev_p2 = body2.previous_position.0 - body2.center_of_mass.0
-            + body2.previous_rotation.rotate(self.contact.local_r2);
+        let p1 = body1.position.0 + body1.rotation.rotate(self.local_r1);
+        let p2 = body2.position.0 + body2.rotation.rotate(self.local_r2);
+        let prev_p1 = body1.previous_position.0 + body1.previous_rotation.rotate(self.local_r1);
+        let prev_p2 = body2.previous_position.0 + body2.previous_rotation.rotate(self.local_r2);
 
         // Compute relative motion of the contact points and get the tangential component
         let delta_p = (p1 - prev_p1) - (p2 - prev_p2);

src/plugins/solver.rs

@@ -129,12 +129,10 @@ fn penetration_constraints(
                 *ent2,
                 body1.position.0,
                 body2.position.0,
-                body1.center_of_mass.0,
-                body2.center_of_mass.0,
                 &body1.rotation,
                 &body2.rotation,
-                collider1.get_shape(),
-                collider2.get_shape(),
+                collider1,
+                collider2,
             ) {
                 collision_events.push(Collision(contact));
 
@@ -162,7 +160,7 @@ fn penetration_constraints(
 
                 // Create and solve constraint if both colliders are solid
                 if sensor1.is_none() && sensor2.is_none() {
-                    let mut constraint = PenetrationConstraint::new(*ent1, *ent2, contact);
+                    let mut constraint = PenetrationConstraint::new(&body1, &body2, contact);
                     constraint.solve([&mut body1, &mut body2], sub_dt.0);
                     penetration_constraints.0.push(constraint);
                 }
@@ -364,8 +362,6 @@ fn solve_vel(
         let Contact {
             entity1,
             entity2,
-            world_r1: r1,
-            world_r2: r2,
             normal,
             ..
         } = constraint.contact;
@@ -379,21 +375,27 @@ fn solve_vel(
             let pre_solve_contact_vel1 = compute_contact_vel(
                 body1.pre_solve_linear_velocity.0,
                 body1.pre_solve_angular_velocity.0,
-                r1,
+                constraint.world_r1,
             );
             let pre_solve_contact_vel2 = compute_contact_vel(
                 body2.pre_solve_linear_velocity.0,
                 body2.pre_solve_angular_velocity.0,
-                r2,
+                constraint.world_r2,
             );
             let pre_solve_relative_vel = pre_solve_contact_vel1 - pre_solve_contact_vel2;
             let pre_solve_normal_vel = normal.dot(pre_solve_relative_vel);
 
             // Compute relative normal and tangential velocities at the contact point (equation 29)
-            let contact_vel1 =
-                compute_contact_vel(body1.linear_velocity.0, body1.angular_velocity.0, r1);
-            let contact_vel2 =
-                compute_contact_vel(body2.linear_velocity.0, body2.angular_velocity.0, r2);
+            let contact_vel1 = compute_contact_vel(
+                body1.linear_velocity.0,
+                body1.angular_velocity.0,
+                constraint.world_r1,
+            );
+            let contact_vel2 = compute_contact_vel(
+                body2.linear_velocity.0,
+                body2.angular_velocity.0,
+                constraint.world_r2,
+            );
             let relative_vel = contact_vel1 - contact_vel2;
             let normal_vel = normal.dot(relative_vel);
             let tangent_vel = relative_vel - normal * normal_vel;
@@ -402,8 +404,10 @@ fn solve_vel(
             let inv_inertia2 = body2.world_inv_inertia().0;
 
             // Compute generalized inverse masses
-            let w1 = constraint.compute_generalized_inverse_mass(&body1, r1, normal);
-            let w2 = constraint.compute_generalized_inverse_mass(&body2, r2, normal);
+            let w1 =
+                constraint.compute_generalized_inverse_mass(&body1, constraint.world_r1, normal);
+            let w2 =
+                constraint.compute_generalized_inverse_mass(&body2, constraint.world_r2, normal);
 
             // Compute dynamic friction
             let friction_impulse = get_dynamic_friction(
@@ -429,11 +433,13 @@ fn solve_vel(
             let p = delta_v / (w1 + w2);
             if body1.rb.is_dynamic() {
                 body1.linear_velocity.0 += p / body1.mass.0;
-                body1.angular_velocity.0 += compute_delta_ang_vel(inv_inertia1, r1, p);
+                body1.angular_velocity.0 +=
+                    compute_delta_ang_vel(inv_inertia1, constraint.world_r1, p);
             }
             if body2.rb.is_dynamic() {
                 body2.linear_velocity.0 -= p / body2.mass.0;
-                body2.angular_velocity.0 -= compute_delta_ang_vel(inv_inertia2, r2, p);
+                body2.angular_velocity.0 -=
+                    compute_delta_ang_vel(inv_inertia2, constraint.world_r2, p);
             }
         }
     }