Store impulses in contacts and refactor contact data (#324)

# Objective

Having access to contact impulses can be very useful for several tasks, like implementing destructable objects or determining how much damage a hit should deal. Currently, however, this is not really possible.

There should be a way to access contact impulses.

## Solution

Add properties for normal and tangent (i.e. friction) impulses to `ContactData`. These are computed for `PenetrationConstraint`s in the constraint solver and stored in `Collisions` in the new `SubstepSet::StoreImpulses` system set.

The impulses can be accessed in `Collision` events or using the `Collisions` resource. Note that the impulses in `Colision` events are currently *only from the last substep*.

Impulses are stored instead of forces for a few reasons:

- It's more efficient internally, as it skips some operations
- Impulses might make more sense for impacts
- It's what e.g. Box2D and Unity use

Computing the corresponding force is simple however, as you just need to divide by the (substep) delta time. For this, the contact types also have helpers like `normal_force` and `tangent_force`.

---

## Changelog

- Added `SubstepSet::StoreImpulses` with a `store_contact_impulses` system
- Added normal and tangent impulses and contact index to `ContactData`
- Added total normal and tangent impulses to `Contacts`
- Added manifold index to `ContactManifold`
- Added collider entities and the contact manifold index to `PenetrationConstraint`
- Added `SingleContact` struct, which is used by the `contact` query
- Removed force properties from `PenetrationConstraint` in favor of impulses
- Changed contact normal debug rendering to scale based on the impulse by default
  - Added `ContactGizmoScale` enum for configuring the scaling

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 {