Store impulses in contacts and refactor contact data

src/constraints/penetration.rs

@@ -16,8 +16,14 @@ pub struct PenetrationConstraint {
     pub entity1: Entity,
     /// Second entity in the constraint.
     pub entity2: Entity,
+    /// The entity of the collider of the first body.
+    pub collider_entity1: Entity,
+    /// The entity of the collider of the second body.
+    pub collider_entity2: Entity,
     /// Data associated with the contact.
     pub contact: ContactData,
+    /// The index of the contact in the manifold.
+    pub manifold_index: usize,
     /// Vector from the first entity's center of mass to the contact point in local coordinates.
     pub r1: Vector,
     /// Vector from the second entity's center of mass to the contact point in local coordinates.
@@ -28,16 +34,10 @@ pub struct PenetrationConstraint {
     pub tangent_lagrange: Scalar,
     /// The constraint's compliance, the inverse of stiffness, has the unit meters / Newton.
     pub compliance: Scalar,
-    /// The coefficient of [dynamic friction](Friction) in this contact.
-    pub dynamic_friction_coefficient: Scalar,
-    /// The coefficient of [static friction](Friction) in this contact.
-    pub static_friction_coefficient: Scalar,
-    /// The coefficient of [restitution](Restitution) in this contact.
-    pub restitution_coefficient: Scalar,
-    /// Normal force acting along the constraint.
-    pub normal_force: Vector,
-    /// Static friction force acting along this constraint.
-    pub static_friction_force: Vector,
+    /// The effective [friction](Friction) of the contact.
+    pub friction: Friction,
+    /// The effective [restitution](Restitution) of the contact.
+    pub restitution: Restitution,
 }
 
 impl XpbdConstraint<2> for PenetrationConstraint {
@@ -70,28 +70,33 @@ impl XpbdConstraint<2> for PenetrationConstraint {
 
 impl PenetrationConstraint {
     /// Creates a new [`PenetrationConstraint`] with the given bodies and contact data.
+    ///
+    /// The `manifold_index` is the index of the contact in a [`ContactManifold`].
     pub fn new(
         body1: &RigidBodyQueryItem,
         body2: &RigidBodyQueryItem,
+        collider_entity1: Entity,
+        collider_entity2: Entity,
         contact: ContactData,
+        manifold_index: usize,
     ) -> Self {
         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,
+            collider_entity1,
+            collider_entity2,
             contact,
+            manifold_index,
             r1,
             r2,
             normal_lagrange: 0.0,
             tangent_lagrange: 0.0,
             compliance: 0.0,
-            dynamic_friction_coefficient: 0.0,
-            static_friction_coefficient: 0.0,
-            restitution_coefficient: 0.0,
-            normal_force: Vector::ZERO,
-            static_friction_force: Vector::ZERO,
+            friction: body1.friction.combine(*body2.friction),
+            restitution: body1.restitution.combine(*body2.restitution),
         }
     }
 
@@ -126,8 +131,10 @@ impl PenetrationConstraint {
         // Apply positional correction to solve overlap
         self.apply_positional_correction(body1, body2, delta_lagrange, normal, r1, r2);
 
-        // Update normal force using the equation f = lambda * n / h^2
-        self.normal_force = self.normal_lagrange * normal / dt.powi(2);
+        // Update normal impulse.
+        // f = lambda / h^2
+        // i = f * h = lambda / h
+        self.contact.normal_impulse += self.normal_lagrange / dt;
     }
 
     fn solve_friction(
@@ -170,7 +177,7 @@ impl PenetrationConstraint {
         let w = [w1, w2];
 
         // Apply static friction if |delta_x_perp| < mu_s * d
-        if sliding_len < self.static_friction_coefficient * penetration {
+        if sliding_len < self.friction.static_coefficient * penetration {
             // Compute Lagrange multiplier update for static friction
             let delta_lagrange =
                 self.compute_lagrange_update(lagrange, sliding_len, &gradients, &w, compliance, dt);
@@ -179,8 +186,10 @@ impl PenetrationConstraint {
             // Apply positional correction to handle static friction
             self.apply_positional_correction(body1, body2, delta_lagrange, tangent, r1, r2);
 
-            // Update static friction force using the equation f = lambda * n / h^2
-            self.static_friction_force = self.tangent_lagrange * tangent / dt.powi(2);
+            // Update static friction impulse.
+            // f = lambda / h^2
+            // i = f * h = lambda / h
+            self.contact.tangent_impulse += self.tangent_lagrange / dt;
         }
     }
 }

src/lib.rs

@@ -693,6 +693,8 @@ pub enum PhysicsStepSet {
 /// 4. Solve positional and angular constraints
 /// 5. Update velocities
 /// 6. Solve velocity constraints (dynamic friction and restitution)
+/// 7. Store contact impulses in [`Collisions`].
+/// 8. Apply [`AccumulatedTranslation`] to positions.
 #[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)]
 pub enum SubstepSet {
     /// Responsible for integrating Newton's 2nd law of motion,
@@ -736,6 +738,10 @@ pub enum SubstepSet {
     ///
     /// See [`SolverPlugin`].
     SolveVelocities,
+    /// Contact impulses computed by the solver are stored in contacts in [`Collisions`].
+    ///
+    /// See [`SolverPlugin`].
+    StoreImpulses,
     /// Responsible for applying translation accumulated during the substep.
     ///
     /// See [`SolverPlugin`].

src/plugins/collision/contact_query.rs

@@ -68,7 +68,7 @@ pub fn contact(
     position2: impl Into<Position>,
     rotation2: impl Into<Rotation>,
     prediction_distance: Scalar,
-) -> Result<Option<ContactData>, UnsupportedShape> {
+) -> Result<Option<SingleContact>, UnsupportedShape> {
     let rotation1: Rotation = rotation1.into();
     let rotation2: Rotation = rotation2.into();
     let isometry1 = utils::make_isometry(position1.into(), rotation1);
@@ -100,13 +100,13 @@ pub fn contact(
                 return None;
             }
 
-            Some(ContactData {
+            Some(SingleContact::new(
                 point1,
                 point2,
                 normal1,
                 normal2,
-                penetration: -contact.dist,
-            })
+                -contact.dist,
+            ))
         } else {
             None
         }
@@ -175,6 +175,9 @@ pub fn contact_manifolds(
         &mut manifolds,
         &mut None,
     );
+
+    let mut manifold_index = 0;
+
     manifolds
         .iter()
         .filter_map(|manifold| {
@@ -196,21 +199,30 @@ pub fn contact_manifolds(
                 return None;
             }
 
-            Some(ContactManifold {
+            let manifold = ContactManifold {
                 normal1,
                 normal2,
                 contacts: manifold
                     .contacts()
                     .iter()
-                    .map(|contact| ContactData {
-                        point1: subpos1.transform_point(&contact.local_p1).into(),
-                        point2: subpos2.transform_point(&contact.local_p2).into(),
-                        normal1,
-                        normal2,
-                        penetration: -contact.dist,
+                    .enumerate()
+                    .map(|(contact_index, contact)| {
+                        ContactData::new(
+                            subpos1.transform_point(&contact.local_p1).into(),
+                            subpos2.transform_point(&contact.local_p2).into(),
+                            normal1,
+                            normal2,
+                            -contact.dist,
+                            contact_index,
+                        )
                     })
                     .collect(),
-            })
+                index: manifold_index,
+            };
+
+            manifold_index += 1;
+
+            Some(manifold)
         })
         .collect()
 }

src/plugins/collision/mod.rs

@@ -266,6 +266,34 @@ pub struct Contacts {
     pub during_current_substep: bool,
     /// True if the bodies were in contact during the previous frame.
     pub during_previous_frame: bool,
+    /// The total normal impulse applied to the first body in a collision.
+    ///
+    /// To get the corresponding force, divide the impulse by `Time<Substeps>`.
+    pub total_normal_impulse: Scalar,
+    /// The total tangent impulse applied to the first body in a collision.
+    ///
+    /// To get the corresponding force, divide the impulse by `Time<Substeps>`.
+    #[doc(alias = "total_friction_impulse")]
+    pub total_tangent_impulse: Scalar,
+}
+
+impl Contacts {
+    /// The force corresponding to the total normal impulse applied over `delta_time`.
+    ///
+    /// Because contacts are solved over several substeps, `delta_time` should
+    /// typically use `Time<Substeps>`.
+    pub fn total_normal_force(&self, delta_time: Scalar) -> Scalar {
+        self.total_normal_impulse / delta_time
+    }
+
+    /// The force corresponding to the total tangent impulse applied over `delta_time`.
+    ///
+    /// Because contacts are solved over several substeps, `delta_time` should
+    /// typically use `Time<Substeps>`.
+    #[doc(alias = "total_friction_force")]
+    pub fn total_tangent_force(&self, delta_time: Scalar) -> Scalar {
+        self.total_tangent_impulse / delta_time
+    }
 }
 
 /// A contact manifold between two colliders, containing a set of contact points.
@@ -281,6 +309,8 @@ pub struct ContactManifold {
     /// A contact normal shared by all contacts in this manifold,
     /// expressed in the local space of the second entity.
     pub normal2: Vector,
+    /// The index of the manifold in the collision.
+    pub index: usize,
 }
 
 impl ContactManifold {
@@ -295,6 +325,66 @@ impl ContactManifold {
     }
 }
 
+/// Data related to a single contact between two bodies.
+///
+/// If you want a contact that belongs to a [contact manifold](ContactManifold) and has more data,
+/// see [`ContactData`].
+#[derive(Clone, Copy, Debug, PartialEq)]
+#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
+pub struct SingleContact {
+    /// Contact point on the first entity in local coordinates.
+    pub point1: Vector,
+    /// Contact point on the second entity in local coordinates.
+    pub point2: Vector,
+    /// A contact normal expressed in the local space of the first entity.
+    pub normal1: Vector,
+    /// A contact normal expressed in the local space of the second entity.
+    pub normal2: Vector,
+    /// Penetration depth.
+    pub penetration: Scalar,
+}
+
+impl SingleContact {
+    /// Creates a new [`SingleContact`]. The contact points and normals should be given in local space.
+    pub fn new(
+        point1: Vector,
+        point2: Vector,
+        normal1: Vector,
+        normal2: Vector,
+        penetration: Scalar,
+    ) -> Self {
+        Self {
+            point1,
+            point2,
+            normal1,
+            normal2,
+            penetration,
+        }
+    }
+
+    /// 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_normal1(&self, rotation: &Rotation) -> Vector {
+        rotation.rotate(self.normal1)
+    }
+
+    /// Returns the world-space contact normal pointing towards the exterior of the second entity.
+    pub fn global_normal2(&self, rotation: &Rotation) -> Vector {
+        rotation.rotate(self.normal2)
+    }
+}
+
 /// Data related to a contact between two bodies.
 #[derive(Clone, Copy, Debug, PartialEq)]
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
@@ -309,9 +399,80 @@ pub struct ContactData {
     pub normal2: Vector,
     /// Penetration depth.
     pub penetration: Scalar,
+    /// The impulse applied to the first body along the normal.
+    ///
+    /// To get the corresponding force, divide the impulse by `Time<Substeps>`.
+    ///
+    /// ## Caveats
+    ///
+    /// * This will initially be zero after collision detection and will only be computed after constraint solving.
+    /// It is recommended to access this before or after physics.
+    /// * The impulse is from a single *substep*. Each physics frame has [`SubstepCount`] substeps.
+    /// * Impulses in contact events like [`Collision`] currently use the impulse from the *last* substep.
+    ///
+    /// The total impulse for a collision including all contacts can be accessed in [`Contacts`] returned by
+    /// the [`Collision`] event or the [`Collisions`] rsource.
+    pub normal_impulse: Scalar,
+    /// The impulse applied to the first body along the tangent. This corresponds to the impulse caused by friction.
+    ///
+    /// To get the corresponding force, divide the impulse by `Time<Substeps>`.
+    ///
+    /// ## Caveats
+    ///
+    /// * This will initially be zero after collision detection and will only be computed after constraint solving.
+    /// It is recommended to access this before or after physics.
+    /// * The impulse is from a single *substep*. Each physics frame has [`SubstepCount`] substeps.
+    /// * Impulses in contact events like [`Collision`] currently use the impulse from the *last* substep.
+    ///
+    /// The total impulse for a collision including all contacts can be accessed in [`Contacts`] returned by
+    /// the [`Collision`] event or the [`Collisions`] rsource.
+    #[doc(alias = "friction_impulse")]
+    pub tangent_impulse: Scalar,
+    /// The index of the contact in a contact manifold if it is in one.
+    pub index: usize,
 }
 
 impl ContactData {
+    /// Creates a new [`ContactData`]. The contact points and normals should be given in local space.
+    ///
+    /// The given `index` is the index of the contact in a contact manifold, if it is in one.
+    pub fn new(
+        point1: Vector,
+        point2: Vector,
+        normal1: Vector,
+        normal2: Vector,
+        penetration: Scalar,
+        index: usize,
+    ) -> Self {
+        Self {
+            point1,
+            point2,
+            normal1,
+            normal2,
+            penetration,
+            normal_impulse: 0.0,
+            tangent_impulse: 0.0,
+            index,
+        }
+    }
+
+    /// The force corresponding to the normal impulse applied over `delta_time`.
+    ///
+    /// Because contacts are solved over several substeps, `delta_time` should
+    /// typically use `Time<Substeps>`.
+    pub fn normal_force(&self, delta_time: Scalar) -> Scalar {
+        self.normal_impulse / delta_time
+    }
+
+    /// The force corresponding to the tangent impulse applied over `delta_time`.
+    ///
+    /// Because contacts are solved over several substeps, `delta_time` should
+    /// typically use `Time<Substeps>`.
+    #[doc(alias = "friction_force")]
+    pub fn tangent_force(&self, delta_time: Scalar) -> Scalar {
+        self.tangent_impulse / delta_time
+    }
+
     /// 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 {