From adb3a19257eeaa7ad94346058971299537faa61d Mon Sep 17 00:00:00 2001
From: Affinator <Affinator@users.noreply.github.com>
Date: Tue, 30 Jan 2024 09:43:41 +0100
Subject: [PATCH 1/5] Implement cast_ray_predicate to allow filtering the
 colliders with a function (#297)

As I was writing a xpbd backend for bevy_mod_picking, I found that a ray cast with a predicate was missing to implement the functionality efficiently.

I was inspired by the predicate from bevy_rapier: https://github.com/dimforge/bevy_rapier/blob/c6bcce4695d596a7a9c8e91748d4dbb3d31f6d13/src/pipeline/query_filter.rs#L12

# Objective

- Implement a ray cast function with the support for a predicate function to filter the colliders

## Solution

- Added support for a predicate starting at SpatialQuery and down the pipeline
- I added a small (and fun to play with) example to illustrate one possible use case
- I chose not to change existing functionality. Therefore there is some code duplication in QueryPipelineAsCompositeShapeWithPredicate. This could be unified by introducing an Option in QueryPipelineAsCompositeShape, but that would create breaking changes at many places.

---------

Co-authored-by: hendrikd <hendrik@decke.online>
Co-authored-by: Joona Aalto <jondolf.dev@gmail.com>
---
 .../examples/cast_ray_predicate.rs            | 200 ++++++++++++++++++
 src/plugins/collision/mod.rs                  |   4 +-
 src/plugins/spatial_query/pipeline.rs         |  98 +++++++++
 src/plugins/spatial_query/system_param.rs     |  62 ++++++
 4 files changed, 362 insertions(+), 2 deletions(-)
 create mode 100644 crates/bevy_xpbd_3d/examples/cast_ray_predicate.rs

diff --git a/crates/bevy_xpbd_3d/examples/cast_ray_predicate.rs b/crates/bevy_xpbd_3d/examples/cast_ray_predicate.rs
new file mode 100644
index 00000000..95e61466
--- /dev/null
+++ b/crates/bevy_xpbd_3d/examples/cast_ray_predicate.rs
@@ -0,0 +1,200 @@
+#![allow(clippy::unnecessary_cast)]
+
+use bevy::{pbr::NotShadowReceiver, prelude::*};
+use bevy_xpbd_3d::{math::*, prelude::*};
+use examples_common_3d::XpbdExamplePlugin;
+
+fn main() {
+    App::new()
+        .add_plugins((DefaultPlugins, XpbdExamplePlugin))
+        .insert_resource(ClearColor(Color::rgb(0.05, 0.05, 0.1)))
+        .insert_resource(Msaa::Sample4)
+        .add_systems(Startup, setup)
+        .add_systems(Update, (movement, reset_colors, raycast).chain())
+        .run();
+}
+
+/// The acceleration used for movement.
+#[derive(Component)]
+struct MovementAcceleration(Scalar);
+
+#[derive(Component)]
+struct RayIndicator;
+
+/// If to be ignored by raycast
+#[derive(Component)]
+struct OutOfGlass(bool);
+
+const CUBE_COLOR: Color = Color::rgba(0.2, 0.7, 0.9, 1.0);
+const CUBE_COLOR_GLASS: Color = Color::rgba(0.2, 0.7, 0.9, 0.5);
+
+fn setup(
+    mut commands: Commands,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+    mut meshes: ResMut<Assets<Mesh>>,
+) {
+    let cube_mesh = meshes.add(Mesh::from(shape::Cube { size: 1.0 }));
+
+    // Ground
+    commands.spawn((
+        PbrBundle {
+            mesh: cube_mesh.clone(),
+            material: materials.add(Color::rgb(0.7, 0.7, 0.8).into()),
+            transform: Transform::from_xyz(0.0, -2.0, 0.0).with_scale(Vec3::new(100.0, 1.0, 100.0)),
+            ..default()
+        },
+        RigidBody::Static,
+        Collider::cuboid(1.0, 1.0, 1.0),
+    ));
+
+    let cube_size = 2.0;
+
+    // Spawn cube stacks
+    for x in -1..2 {
+        for y in -1..2 {
+            for z in -1..2 {
+                let position = Vec3::new(x as f32, y as f32 + 5.0, z as f32) * (cube_size + 0.05);
+                let material: StandardMaterial = if x == -1 {
+                    CUBE_COLOR_GLASS.into()
+                } else {
+                    CUBE_COLOR.into()
+                };
+                commands.spawn((
+                    PbrBundle {
+                        mesh: cube_mesh.clone(),
+                        material: materials.add(material.clone()),
+                        transform: Transform::from_translation(position)
+                            .with_scale(Vec3::splat(cube_size as f32)),
+                        ..default()
+                    },
+                    RigidBody::Dynamic,
+                    Collider::cuboid(1.0, 1.0, 1.0),
+                    MovementAcceleration(10.0),
+                    OutOfGlass(x == -1),
+                ));
+            }
+        }
+    }
+
+    // raycast indicator
+    commands.spawn((
+        PbrBundle {
+            mesh: cube_mesh.clone(),
+            material: materials.add(Color::rgb(1.0, 0.0, 0.0).into()),
+            transform: Transform::from_xyz(-500.0, 2.0, 0.0)
+                .with_scale(Vec3::new(1000.0, 0.1, 0.1)),
+            ..default()
+        },
+        RayIndicator,
+        NotShadowReceiver,
+    ));
+
+    // Directional light
+    commands.spawn(DirectionalLightBundle {
+        directional_light: DirectionalLight {
+            illuminance: 20_000.0,
+            shadows_enabled: true,
+            ..default()
+        },
+        transform: Transform::default().looking_at(Vec3::new(-1.0, -2.5, -1.5), Vec3::Y),
+        ..default()
+    });
+
+    // Camera
+    commands.spawn(Camera3dBundle {
+        transform: Transform::from_translation(Vec3::new(0.0, 12.0, 40.0))
+            .looking_at(Vec3::Y * 5.0, Vec3::Y),
+        ..default()
+    });
+}
+
+fn movement(
+    time: Res<Time>,
+    keyboard_input: Res<Input<KeyCode>>,
+    mut query: Query<(&MovementAcceleration, &mut LinearVelocity)>,
+) {
+    // Precision is adjusted so that the example works with
+    // both the `f32` and `f64` features. Otherwise you don't need this.
+    let delta_time = time.delta_seconds_f64().adjust_precision();
+
+    for (movement_acceleration, mut linear_velocity) in &mut query {
+        let up = keyboard_input.any_pressed([KeyCode::W, KeyCode::Up]);
+        let down = keyboard_input.any_pressed([KeyCode::S, KeyCode::Down]);
+        let left = keyboard_input.any_pressed([KeyCode::A, KeyCode::Left]);
+        let right = keyboard_input.any_pressed([KeyCode::D, KeyCode::Right]);
+
+        let horizontal = right as i8 - left as i8;
+        let vertical = down as i8 - up as i8;
+        let direction =
+            Vector::new(horizontal as Scalar, 0.0, vertical as Scalar).normalize_or_zero();
+
+        // Move in input direction
+        if direction != Vector::ZERO {
+            linear_velocity.x += direction.x * movement_acceleration.0 * delta_time;
+            linear_velocity.z += direction.z * movement_acceleration.0 * delta_time;
+        }
+    }
+}
+
+fn reset_colors(
+    mut materials: ResMut<Assets<StandardMaterial>>,
+    cubes: Query<(&Handle<StandardMaterial>, &OutOfGlass)>,
+) {
+    for (material_handle, out_of_glass) in cubes.iter() {
+        if let Some(material) = materials.get_mut(material_handle) {
+            if out_of_glass.0 {
+                material.base_color = CUBE_COLOR_GLASS;
+            } else {
+                material.base_color = CUBE_COLOR;
+            }
+        }
+    }
+}
+
+fn raycast(
+    query: SpatialQuery,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+    cubes: Query<(&Handle<StandardMaterial>, &OutOfGlass)>,
+    mut indicator_transform: Query<&mut Transform, With<RayIndicator>>,
+) {
+    let origin = Vector {
+        x: -200.0,
+        y: 2.0,
+        z: 0.0,
+    };
+    let direction = Vector {
+        x: 1.0,
+        y: 0.0,
+        z: 0.0,
+    };
+
+    let mut ray_indicator_transform = indicator_transform.single_mut();
+
+    if let Some(ray_hit_data) = query.cast_ray_predicate(
+        origin,
+        direction,
+        Scalar::MAX,
+        true,
+        SpatialQueryFilter::new(),
+        &|entity| {
+            if let Ok((_, out_of_glass)) = cubes.get(entity) {
+                return !out_of_glass.0; // only look at cubes not out of glass
+            }
+            true // if the collider has no OutOfGlass component, then check it nevertheless
+        },
+    ) {
+        // set color of hit object to red
+        if let Ok((material_handle, _)) = cubes.get(ray_hit_data.entity) {
+            if let Some(material) = materials.get_mut(material_handle) {
+                material.base_color = Color::RED;
+            }
+        }
+
+        // set length of ray indicator to look more like a laser
+        let contact_point = (origin + direction * ray_hit_data.time_of_impact).x;
+        let target_scale = 1000.0 + contact_point * 2.0;
+        ray_indicator_transform.scale.x = target_scale as f32;
+    } else {
+        ray_indicator_transform.scale.x = 2000.0;
+    }
+}
diff --git a/src/plugins/collision/mod.rs b/src/plugins/collision/mod.rs
index ae7a7efe..150dd5c2 100644
--- a/src/plugins/collision/mod.rs
+++ b/src/plugins/collision/mod.rs
@@ -229,8 +229,8 @@ impl Collisions {
     /// The order of the entities does not matter.
     pub fn remove_collision_pair(&mut self, entity1: Entity, entity2: Entity) -> Option<Contacts> {
         self.0
-            .remove(&(entity1, entity2))
-            .or_else(|| self.0.remove(&(entity2, entity1)))
+            .swap_remove(&(entity1, entity2))
+            .or_else(|| self.0.swap_remove(&(entity2, entity1)))
     }
 
     /// Removes all collisions that involve the given entity.
diff --git a/src/plugins/spatial_query/pipeline.rs b/src/plugins/spatial_query/pipeline.rs
index 36f39095..df32cf51 100644
--- a/src/plugins/spatial_query/pipeline.rs
+++ b/src/plugins/spatial_query/pipeline.rs
@@ -58,6 +58,19 @@ impl SpatialQueryPipeline {
         }
     }
 
+    pub(crate) fn as_composite_shape_with_predicate<'a>(
+        &'a self,
+        query_filter: SpatialQueryFilter,
+        predicate: &'a dyn Fn(Entity) -> bool,
+    ) -> QueryPipelineAsCompositeShapeWithPredicate {
+        QueryPipelineAsCompositeShapeWithPredicate {
+            pipeline: self,
+            colliders: &self.colliders,
+            query_filter,
+            predicate,
+        }
+    }
+
     /// Updates the associated acceleration structures with a new set of entities.
     pub fn update<'a>(
         &mut self,
@@ -172,6 +185,48 @@ impl SpatialQueryPipeline {
             })
     }
 
+    /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
+    /// If there are no hits, `None` is returned.
+    ///
+    /// ## Arguments
+    ///
+    /// - `origin`: Where the ray is cast from.
+    /// - `direction`: What direction the ray is cast in.
+    /// - `max_time_of_impact`: The maximum distance that the ray can travel.
+    /// - `solid`: If true and the ray origin is inside of a collider, the hit point will be the ray origin itself.
+    /// Otherwise, the collider will be treated as hollow, and the hit point will be at the collider's boundary.
+    /// - `query_filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
+    /// - `predicate`: A function with which the colliders are filtered. Given the Entity it should return false, if the
+    /// entity should be ignored.
+    ///
+    /// See also: [`SpatialQuery::cast_ray`]
+    pub fn cast_ray_predicate(
+        &self,
+        origin: Vector,
+        direction: Vector,
+        max_time_of_impact: Scalar,
+        solid: bool,
+        query_filter: SpatialQueryFilter,
+        predicate: &dyn Fn(Entity) -> bool,
+    ) -> Option<RayHitData> {
+        let pipeline_shape = self.as_composite_shape_with_predicate(query_filter, predicate);
+        let ray = parry::query::Ray::new(origin.into(), direction.into());
+        let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
+            &pipeline_shape,
+            &ray,
+            max_time_of_impact,
+            solid,
+        );
+
+        self.qbvh
+            .traverse_best_first(&mut visitor)
+            .map(|(_, (entity_index, hit))| RayHitData {
+                entity: self.entity_from_index(entity_index),
+                time_of_impact: hit.toi,
+                normal: hit.normal.into(),
+            })
+    }
+
     /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData) until `max_hits` is reached.
     ///
     /// Note that the order of the results is not guaranteed, and if there are more hits than `max_hits`,
@@ -715,6 +770,49 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
     }
 }
 
+pub(crate) struct QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
+    colliders: &'a HashMap<Entity, (Isometry<Scalar>, Collider, CollisionLayers)>,
+    pipeline: &'a SpatialQueryPipeline,
+    query_filter: SpatialQueryFilter,
+    predicate: &'b dyn Fn(Entity) -> bool,
+}
+
+impl<'a, 'b> TypedSimdCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
+    type PartShape = dyn Shape;
+    type PartId = u32;
+    type QbvhStorage = DefaultStorage;
+
+    fn map_typed_part_at(
+        &self,
+        shape_id: Self::PartId,
+        mut f: impl FnMut(Option<&Isometry<Scalar>>, &Self::PartShape),
+    ) {
+        if let Some((entity, (iso, shape, layers))) =
+            self.colliders
+                .get_key_value(&utils::entity_from_index_and_gen(
+                    shape_id,
+                    *self.pipeline.entity_generations.get(&shape_id).unwrap(),
+                ))
+        {
+            if self.query_filter.test(*entity, *layers) && (self.predicate)(*entity) {
+                f(Some(iso), &**shape.shape_scaled());
+            }
+        }
+    }
+
+    fn map_untyped_part_at(
+        &self,
+        shape_id: Self::PartId,
+        f: impl FnMut(Option<&Isometry<Scalar>>, &dyn Shape),
+    ) {
+        self.map_typed_part_at(shape_id, f);
+    }
+
+    fn typed_qbvh(&self) -> &parry::partitioning::GenericQbvh<Self::PartId, Self::QbvhStorage> {
+        &self.pipeline.qbvh
+    }
+}
+
 /// The result of a [point projection](spatial_query#point-projection) on a [collider](Collider).
 #[derive(Debug, Clone, PartialEq)]
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
diff --git a/src/plugins/spatial_query/system_param.rs b/src/plugins/spatial_query/system_param.rs
index cd7ac3c6..c36fc62f 100644
--- a/src/plugins/spatial_query/system_param.rs
+++ b/src/plugins/spatial_query/system_param.rs
@@ -133,6 +133,68 @@ impl<'w, 's> SpatialQuery<'w, 's> {
             .cast_ray(origin, direction, max_time_of_impact, solid, query_filter)
     }
 
+    /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
+    /// If there are no hits, `None` is returned.
+    ///
+    /// ## Arguments
+    ///
+    /// - `origin`: Where the ray is cast from.
+    /// - `direction`: What direction the ray is cast in.
+    /// - `max_time_of_impact`: The maximum distance that the ray can travel.
+    /// - `solid`: If true and the ray origin is inside of a collider, the hit point will be the ray origin itself.
+    /// Otherwise, the collider will be treated as hollow, and the hit point will be at the collider's boundary.
+    /// - `query_filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
+    /// - `predicate`: A function with which the colliders are filtered. Given the Entity it should return false, if the
+    /// entity should be ignored.
+    ///
+    /// ## Example
+    ///
+    /// ```
+    /// use bevy::prelude::*;
+    /// # #[cfg(feature = "2d")]
+    /// # use bevy_xpbd_2d::prelude::*;
+    /// # #[cfg(feature = "3d")]
+    /// use bevy_xpbd_3d::prelude::*;
+    ///
+    /// # #[cfg(all(feature = "3d", feature = "f32"))]
+    /// fn print_hits(spatial_query: SpatialQuery) {
+    ///     // Cast ray and print first hit
+    ///     if let Some(first_hit) = spatial_query.cast_ray(
+    ///         Vec3::ZERO,                    // Origin
+    ///         Vec3::X,                       // Direction
+    ///         100.0,                         // Maximum time of impact (travel distance)
+    ///         true,                          // Does the ray treat colliders as "solid"
+    ///         SpatialQueryFilter::default(), // Query filter
+    ///         &|entity| {                    // Predicate
+    ///             if let Some(value) = query.get(entity) {
+    ///                 return value == x;     // ignore if value from query is x
+    ///             }
+    ///             true                       // else check for collision
+    ///         }
+    ///     ) {
+    ///         println!("First hit: {:?}", first_hit);
+    ///     }
+    /// }
+    /// ```
+    pub fn cast_ray_predicate(
+        &self,
+        origin: Vector,
+        direction: Vector,
+        max_time_of_impact: Scalar,
+        solid: bool,
+        query_filter: SpatialQueryFilter,
+        predicate: &dyn Fn(Entity) -> bool,
+    ) -> Option<RayHitData> {
+        self.query_pipeline.cast_ray_predicate(
+            origin,
+            direction,
+            max_time_of_impact,
+            solid,
+            query_filter,
+            predicate,
+        )
+    }
+
     /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData) until `max_hits` is reached.
     ///
     /// Note that the order of the results is not guaranteed, and if there are more hits than `max_hits`,

From ba154111d1e6d7d386ea3b94571f62edd56174e5 Mon Sep 17 00:00:00 2001
From: Joona Aalto <jondolf.dev@gmail.com>
Date: Mon, 12 Feb 2024 20:14:48 +0200
Subject: [PATCH 2/5] Fix overlap warnings triggering for non-dynamic
 collisions

---
 src/plugins/solver.rs | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/plugins/solver.rs b/src/plugins/solver.rs
index 3581211f..bb4cdd47 100644
--- a/src/plugins/solver.rs
+++ b/src/plugins/solver.rs
@@ -211,7 +211,11 @@ fn penetration_constraints(
                 }
             }
 
-            if contacts.during_current_substep && body1.rb.is_added() || body2.rb.is_added() {
+            if contacts.during_current_substep
+                && (body1.rb.is_added() || body2.rb.is_added())
+                && body1.rb.is_dynamic()
+                && body2.rb.is_dynamic()
+            {
                 // if the RigidBody entity has a name, use that for debug.
                 let debug_id1 = match name1 {
                     Some(n) => format!("{:?} ({n})", body1.entity),

From 23ca1befad3e48d7e48f11a3b02eebeed3c99e08 Mon Sep 17 00:00:00 2001
From: Joona Aalto <jondolf.dev@gmail.com>
Date: Mon, 12 Feb 2024 20:29:42 +0200
Subject: [PATCH 3/5] Fix colliders without `RigidBody` not working (#323)

# Objective

On the main branch, collisions between colliders without a `RigidBody` component are currently not detected. This is because some queries are overly strict and require components that only rigid bodies have.

## Solution

Make some components in queries optional in order to fix non-rigidbody colliders.
---
 src/plugins/collision/broad_phase.rs | 13 +++++++------
 src/plugins/sync.rs                  | 11 ++++++++---
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/plugins/collision/broad_phase.rs b/src/plugins/collision/broad_phase.rs
index d1676f4b..49761d0c 100644
--- a/src/plugins/collision/broad_phase.rs
+++ b/src/plugins/collision/broad_phase.rs
@@ -185,7 +185,7 @@ impl MapEntities for AabbIntervals {
 fn update_aabb_intervals(
     aabbs: Query<(
         &ColliderAabb,
-        &ColliderParent,
+        Option<&ColliderParent>,
         Option<&CollisionLayers>,
         Ref<Position>,
         Ref<Rotation>,
@@ -199,10 +199,11 @@ fn update_aabb_intervals(
                 aabbs.get(*collider_entity)
             {
                 *aabb = *new_aabb;
-                *collider_parent = *new_parent;
+                *collider_parent = new_parent.map_or(ColliderParent(*collider_entity), |p| *p);
                 *layers = new_layers.map_or(CollisionLayers::default(), |layers| *layers);
 
-                let is_static = rbs.get(new_parent.get()).is_ok_and(RigidBody::is_static);
+                let is_static =
+                    new_parent.is_some_and(|p| rbs.get(p.get()).is_ok_and(RigidBody::is_static));
                 *is_inactive = is_static || (!position.is_changed() && !rotation.is_changed());
 
                 true
@@ -219,7 +220,7 @@ fn add_new_aabb_intervals(
     aabbs: Query<
         (
             Entity,
-            &ColliderParent,
+            Option<&ColliderParent>,
             &ColliderAabb,
             Option<&RigidBody>,
             Option<&CollisionLayers>,
@@ -231,7 +232,7 @@ fn add_new_aabb_intervals(
     let aabbs = aabbs.iter().map(|(ent, parent, aabb, rb, layers)| {
         (
             ent,
-            *parent,
+            parent.map_or(ColliderParent(ent), |p| *p),
             *aabb,
             // Default to treating collider as immovable/static for filtering unnecessary collision checks
             layers.map_or(CollisionLayers::default(), |layers| *layers),
@@ -295,7 +296,7 @@ fn sweep_and_prune(
 /// Sorts a list iteratively using comparisons. In an ascending sort order, when a smaller value is encountered, it is moved lower in the list until it is larger than the item before it.
 ///
 /// This is relatively slow for large lists, but very efficient in cases where the list is already mostly sorted.
-fn insertion_sort<T>(items: &mut Vec<T>, comparison: fn(&T, &T) -> bool) {
+fn insertion_sort<T>(items: &mut [T], comparison: fn(&T, &T) -> bool) {
     for i in 1..items.len() {
         let mut j = i;
         while j > 0 && comparison(&items[j - 1], &items[j]) {
diff --git a/src/plugins/sync.rs b/src/plugins/sync.rs
index fd99579f..4d71ea1a 100644
--- a/src/plugins/sync.rs
+++ b/src/plugins/sync.rs
@@ -410,8 +410,8 @@ fn transform_to_position(
         &mut Position,
         Option<&AccumulatedTranslation>,
         &mut Rotation,
-        &PreviousRotation,
-        &CenterOfMass,
+        Option<&PreviousRotation>,
+        Option<&CenterOfMass>,
     )>,
 ) {
     for (
@@ -433,7 +433,12 @@ fn transform_to_position(
         let previous_transform = previous_transform.compute_transform();
         let pos = position.0
             + accumulated_translation.map_or(Vector::ZERO, |t| {
-                get_pos_translation(t, previous_rotation, &rotation, center_of_mass)
+                get_pos_translation(
+                    t,
+                    &previous_rotation.copied().unwrap_or_default(),
+                    &rotation,
+                    &center_of_mass.copied().unwrap_or_default(),
+                )
             });
 
         #[cfg(feature = "2d")]

From 7a97c56dfb12cc9cf5c702d542b0b7d5866b4189 Mon Sep 17 00:00:00 2001
From: Tim Irmler <42088586+zwazel@users.noreply.github.com>
Date: Mon, 12 Feb 2024 23:34:25 +0100
Subject: [PATCH 4/5] fix raycast does not follow entity transform without
 rigidbody (#310)

# Objective

- Fixes #288

## Solution

- If no Rotation and Position is available, fall back to GlobalTransform

## Changelog

- I've added a `From<GlobalTransform>` and `From<&GlobalTransform>` to `Position` as well as a `From<&GlobalTransform>` to `Rotation`. as a `From<GlobalTransform>` already existed.

I tested the updated `update_ray_caster_positions` system in my personal project, and it fixed the problem for me. I currently can't test the `update_shape_caster_positions` but as it is basically the same changes, i'd be surprised if it wouldn't work.

as this is my first time contributing to this crate, i'm not sure if the code is what is expected.

---------

Co-authored-by: Joona Aalto <jondolf.dev@gmail.com>
---
 src/components/mod.rs            |  38 ++++++++
 src/components/rotation.rs       |   6 ++
 src/plugins/spatial_query/mod.rs | 144 +++++++++++++++++++------------
 3 files changed, 135 insertions(+), 53 deletions(-)

diff --git a/src/components/mod.rs b/src/components/mod.rs
index 1581f011..35aa755b 100644
--- a/src/components/mod.rs
+++ b/src/components/mod.rs
@@ -331,6 +331,44 @@ impl Position {
     }
 }
 
+impl From<GlobalTransform> for Position {
+    #[cfg(feature = "2d")]
+    fn from(value: GlobalTransform) -> Self {
+        Self::from_xy(
+            value.translation().adjust_precision().x,
+            value.translation().adjust_precision().y,
+        )
+    }
+
+    #[cfg(feature = "3d")]
+    fn from(value: GlobalTransform) -> Self {
+        Self::from_xyz(
+            value.translation().adjust_precision().x,
+            value.translation().adjust_precision().y,
+            value.translation().adjust_precision().z,
+        )
+    }
+}
+
+impl From<&GlobalTransform> for Position {
+    #[cfg(feature = "2d")]
+    fn from(value: &GlobalTransform) -> Self {
+        Self::from_xy(
+            value.translation().adjust_precision().x,
+            value.translation().adjust_precision().y,
+        )
+    }
+
+    #[cfg(feature = "3d")]
+    fn from(value: &GlobalTransform) -> Self {
+        Self::from_xyz(
+            value.translation().adjust_precision().x,
+            value.translation().adjust_precision().y,
+            value.translation().adjust_precision().z,
+        )
+    }
+}
+
 /// The position of a [rigid body](RigidBody) at the start of a substep.
 #[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, DerefMut, PartialEq, From)]
 #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
diff --git a/src/components/rotation.rs b/src/components/rotation.rs
index ff838fbf..370821c8 100644
--- a/src/components/rotation.rs
+++ b/src/components/rotation.rs
@@ -277,6 +277,12 @@ impl From<GlobalTransform> for Rotation {
     }
 }
 
+impl From<&GlobalTransform> for Rotation {
+    fn from(value: &GlobalTransform) -> Self {
+        Self::from(value.compute_transform().rotation)
+    }
+}
+
 #[cfg(feature = "2d")]
 impl From<Quat> for Rotation {
     fn from(quat: Quat) -> Self {
diff --git a/src/plugins/spatial_query/mod.rs b/src/plugins/spatial_query/mod.rs
index 83cfe8cb..184a37f1 100644
--- a/src/plugins/spatial_query/mod.rs
+++ b/src/plugins/spatial_query/mod.rs
@@ -241,45 +241,64 @@ type RayCasterPositionQueryComponents = (
     Option<&'static Position>,
     Option<&'static Rotation>,
     Option<&'static Parent>,
+    Option<&'static GlobalTransform>,
 );
 
+#[allow(clippy::type_complexity)]
 fn update_ray_caster_positions(
     mut rays: Query<RayCasterPositionQueryComponents>,
-    parents: Query<(Option<&Position>, Option<&Rotation>), With<Children>>,
+    parents: Query<
+        (
+            Option<&Position>,
+            Option<&Rotation>,
+            Option<&GlobalTransform>,
+        ),
+        With<Children>,
+    >,
 ) {
-    for (mut ray, position, rotation, parent) in &mut rays {
+    for (mut ray, position, rotation, parent, transform) in &mut rays {
         let origin = ray.origin;
         let direction = ray.direction;
 
-        if let Some(position) = position {
-            ray.set_global_origin(position.0 + rotation.map_or(origin, |rot| rot.rotate(origin)));
+        let global_position = position.copied().or(transform.map(Position::from));
+        let global_rotation = rotation.copied().or(transform.map(Rotation::from));
+
+        if let Some(global_position) = global_position {
+            ray.set_global_origin(
+                global_position.0 + rotation.map_or(origin, |rot| rot.rotate(origin)),
+            );
         } else if parent.is_none() {
             ray.set_global_origin(origin);
         }
 
-        if let Some(rotation) = rotation {
-            let global_direction = rotation.rotate(ray.direction);
+        if let Some(global_rotation) = global_rotation {
+            let global_direction = global_rotation.rotate(ray.direction);
             ray.set_global_direction(global_direction);
         } else if parent.is_none() {
             ray.set_global_direction(direction);
         }
 
-        if let Some(parent) = parent {
-            if let Ok((parent_position, parent_rotation)) = parents.get(parent.get()) {
-                if position.is_none() {
-                    if let Some(position) = parent_position {
-                        let rotation = rotation.map_or(
-                            parent_rotation.map_or(Rotation::default(), |rot| *rot),
-                            |rot| *rot,
-                        );
-                        ray.set_global_origin(position.0 + rotation.rotate(origin));
-                    }
+        if let Some(Ok((parent_position, parent_rotation, parent_transform))) =
+            parent.map(|p| parents.get(p.get()))
+        {
+            let parent_position = parent_position
+                .copied()
+                .or(parent_transform.map(Position::from));
+            let parent_rotation = parent_rotation
+                .copied()
+                .or(parent_transform.map(Rotation::from));
+
+            // Apply parent transformations
+            if global_position.is_none() {
+                if let Some(position) = parent_position {
+                    let rotation = global_rotation.unwrap_or(parent_rotation.unwrap_or_default());
+                    ray.set_global_origin(position.0 + rotation.rotate(origin));
                 }
-                if rotation.is_none() {
-                    if let Some(rotation) = parent_rotation {
-                        let global_direction = rotation.rotate(ray.direction);
-                        ray.set_global_direction(global_direction);
-                    }
+            }
+            if global_rotation.is_none() {
+                if let Some(rotation) = parent_rotation {
+                    let global_direction = rotation.rotate(ray.direction);
+                    ray.set_global_direction(global_direction);
                 }
             }
         }
@@ -291,34 +310,48 @@ type ShapeCasterPositionQueryComponents = (
     Option<&'static Position>,
     Option<&'static Rotation>,
     Option<&'static Parent>,
+    Option<&'static GlobalTransform>,
 );
 
+#[allow(clippy::type_complexity)]
 fn update_shape_caster_positions(
     mut shape_casters: Query<ShapeCasterPositionQueryComponents>,
-    parents: Query<(Option<&Position>, Option<&Rotation>), With<Children>>,
+    parents: Query<
+        (
+            Option<&Position>,
+            Option<&Rotation>,
+            Option<&GlobalTransform>,
+        ),
+        With<Children>,
+    >,
 ) {
-    for (mut shape_caster, position, rotation, parent) in &mut shape_casters {
+    for (mut shape_caster, position, rotation, parent, transform) in &mut shape_casters {
         let origin = shape_caster.origin;
         let shape_rotation = shape_caster.shape_rotation;
         let direction = shape_caster.direction;
 
-        if let Some(position) = position {
-            shape_caster
-                .set_global_origin(position.0 + rotation.map_or(origin, |rot| rot.rotate(origin)));
+        let global_position = position.copied().or(transform.map(Position::from));
+        let global_rotation = rotation.copied().or(transform.map(Rotation::from));
+
+        if let Some(global_position) = global_position {
+            shape_caster.set_global_origin(
+                global_position.0 + rotation.map_or(origin, |rot| rot.rotate(origin)),
+            );
         } else if parent.is_none() {
             shape_caster.set_global_origin(origin);
         }
 
-        if let Some(rotation) = rotation {
-            let global_direction = rotation.rotate(shape_caster.direction);
+        if let Some(global_rotation) = global_rotation {
+            let global_direction = global_rotation.rotate(shape_caster.direction);
             shape_caster.set_global_direction(global_direction);
             #[cfg(feature = "2d")]
             {
-                shape_caster.set_global_shape_rotation(shape_rotation + rotation.as_radians());
+                shape_caster
+                    .set_global_shape_rotation(shape_rotation + global_rotation.as_radians());
             }
             #[cfg(feature = "3d")]
             {
-                shape_caster.set_global_shape_rotation(shape_rotation + rotation.0);
+                shape_caster.set_global_shape_rotation(shape_rotation + global_rotation.0);
             }
         } else if parent.is_none() {
             shape_caster.set_global_direction(direction);
@@ -332,30 +365,35 @@ fn update_shape_caster_positions(
             }
         }
 
-        if let Some(parent) = parent {
-            if let Ok((parent_position, parent_rotation)) = parents.get(parent.get()) {
-                if position.is_none() {
-                    if let Some(position) = parent_position {
-                        let rotation = rotation.map_or(
-                            parent_rotation.map_or(Rotation::default(), |rot| *rot),
-                            |rot| *rot,
-                        );
-                        shape_caster.set_global_origin(position.0 + rotation.rotate(origin));
-                    }
+        if let Some(Ok((parent_position, parent_rotation, parent_transform))) =
+            parent.map(|p| parents.get(p.get()))
+        {
+            let parent_position = parent_position
+                .copied()
+                .or(parent_transform.map(Position::from));
+            let parent_rotation = parent_rotation
+                .copied()
+                .or(parent_transform.map(Rotation::from));
+
+            // Apply parent transformations
+            if global_position.is_none() {
+                if let Some(position) = parent_position {
+                    let rotation = global_rotation.unwrap_or(parent_rotation.unwrap_or_default());
+                    shape_caster.set_global_origin(position.0 + rotation.rotate(origin));
                 }
-                if rotation.is_none() {
-                    if let Some(rotation) = parent_rotation {
-                        let global_direction = rotation.rotate(shape_caster.direction);
-                        shape_caster.set_global_direction(global_direction);
-                        #[cfg(feature = "2d")]
-                        {
-                            shape_caster
-                                .set_global_shape_rotation(shape_rotation + rotation.as_radians());
-                        }
-                        #[cfg(feature = "3d")]
-                        {
-                            shape_caster.set_global_shape_rotation(shape_rotation + rotation.0);
-                        }
+            }
+            if global_rotation.is_none() {
+                if let Some(rotation) = parent_rotation {
+                    let global_direction = rotation.rotate(shape_caster.direction);
+                    shape_caster.set_global_direction(global_direction);
+                    #[cfg(feature = "2d")]
+                    {
+                        shape_caster
+                            .set_global_shape_rotation(shape_rotation + rotation.as_radians());
+                    }
+                    #[cfg(feature = "3d")]
+                    {
+                        shape_caster.set_global_shape_rotation(shape_rotation + rotation.0);
                     }
                 }
             }

From 715b5b033efe973b8e49a0585e53903947817e2b Mon Sep 17 00:00:00 2001
From: Joona Aalto <jondolf.dev@gmail.com>
Date: Sat, 17 Feb 2024 13:49:36 +0200
Subject: [PATCH 5/5] 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         |  49 +++++---
 src/lib.rs                             |   6 +
 src/plugins/collision/contact_query.rs |  36 ++++--
 src/plugins/collision/mod.rs           | 161 +++++++++++++++++++++++++
 src/plugins/collision/narrow_phase.rs  |   5 +
 src/plugins/debug/configuration.rs     |  34 +++++-
 src/plugins/debug/mod.rs               |  24 +++-
 src/plugins/setup/mod.rs               |   1 +
 src/plugins/solver.rs                  |  70 ++++++++---
 9 files changed, 331 insertions(+), 55 deletions(-)

diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs
index b5deac51..24fd6769 100644
--- a/src/constraints/penetration.rs
+++ b/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,10 +70,15 @@ 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;
@@ -81,17 +86,17 @@ impl PenetrationConstraint {
         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;
         }
     }
 }
diff --git a/src/lib.rs b/src/lib.rs
index e546b4cf..b8e78fa5 100644
--- a/src/lib.rs
+++ b/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`].
diff --git a/src/plugins/collision/contact_query.rs b/src/plugins/collision/contact_query.rs
index 5050c2fc..d6ba310f 100644
--- a/src/plugins/collision/contact_query.rs
+++ b/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()
 }
diff --git a/src/plugins/collision/mod.rs b/src/plugins/collision/mod.rs
index 150dd5c2..c99eb6e2 100644
--- a/src/plugins/collision/mod.rs
+++ b/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 {
diff --git a/src/plugins/collision/narrow_phase.rs b/src/plugins/collision/narrow_phase.rs
index cc07b82f..2bb64c72 100644
--- a/src/plugins/collision/narrow_phase.rs
+++ b/src/plugins/collision/narrow_phase.rs
@@ -197,6 +197,8 @@ fn process_collision_pair<F>(
                 *rotation2,
                 narrow_phase_config.prediction_distance,
             ),
+            total_normal_impulse: 0.0,
+            total_tangent_impulse: 0.0,
         };
 
         if !contacts.manifolds.is_empty() {
@@ -213,6 +215,9 @@ pub fn reset_collision_states(
     query: Query<(Option<&RigidBody>, Has<Sleeping>)>,
 ) {
     for contacts in collisions.get_internal_mut().values_mut() {
+        contacts.total_normal_impulse = 0.0;
+        contacts.total_tangent_impulse = 0.0;
+
         if let Ok([(rb1, sleeping1), (rb2, sleeping2)]) =
             query.get_many([contacts.entity1, contacts.entity2])
         {
diff --git a/src/plugins/debug/configuration.rs b/src/plugins/debug/configuration.rs
index f0d14c36..579a8e94 100644
--- a/src/plugins/debug/configuration.rs
+++ b/src/plugins/debug/configuration.rs
@@ -52,6 +52,8 @@ pub struct PhysicsDebugConfig {
     pub contact_point_color: Option<Color>,
     /// The color of the contact normals. If `None`, the contact normals will not be rendered.
     pub contact_normal_color: Option<Color>,
+    /// The scale used for contact normals.
+    pub contact_normal_scale: ContactGizmoScale,
     /// The color of the lines drawn from the centers of bodies to their joint anchors.
     pub joint_anchor_color: Option<Color>,
     /// The color of the lines drawn between joint anchors, indicating the separation.
@@ -88,6 +90,7 @@ impl Default for PhysicsDebugConfig {
             sleeping_color_multiplier: Some([1.0, 1.0, 0.4, 1.0]),
             contact_point_color: None,
             contact_normal_color: None,
+            contact_normal_scale: ContactGizmoScale::default(),
             joint_anchor_color: Some(Color::PINK),
             joint_separation_color: Some(Color::RED),
             raycast_color: Some(Color::RED),
@@ -102,6 +105,21 @@ impl Default for PhysicsDebugConfig {
     }
 }
 
+/// The scale used for contact normals rendered using gizmos.
+#[derive(Reflect)]
+pub enum ContactGizmoScale {
+    /// The length of the rendered contact normal is constant.
+    Constant(Scalar),
+    /// The length of the rendered contact normal is scaled by the contact force and the given scaling factor.
+    Scaled(Scalar),
+}
+
+impl Default for ContactGizmoScale {
+    fn default() -> Self {
+        Self::Scaled(0.025)
+    }
+}
+
 impl PhysicsDebugConfig {
     /// Creates a [`PhysicsDebugConfig`] configuration with all rendering options enabled.
     pub fn all() -> Self {
@@ -116,6 +134,7 @@ impl PhysicsDebugConfig {
             sleeping_color_multiplier: Some([1.0, 1.0, 0.4, 1.0]),
             contact_point_color: Some(Color::CYAN),
             contact_normal_color: Some(Color::RED),
+            contact_normal_scale: ContactGizmoScale::default(),
             joint_anchor_color: Some(Color::PINK),
             joint_separation_color: Some(Color::RED),
             raycast_color: Some(Color::RED),
@@ -141,6 +160,7 @@ impl PhysicsDebugConfig {
             sleeping_color_multiplier: None,
             contact_point_color: None,
             contact_normal_color: None,
+            contact_normal_scale: ContactGizmoScale::default(),
             joint_anchor_color: None,
             joint_separation_color: None,
             raycast_color: None,
@@ -233,12 +253,24 @@ impl PhysicsDebugConfig {
         self
     }
 
-    /// Sets the contact color.
+    /// Sets the contact point color.
     pub fn with_contact_point_color(mut self, color: Color) -> Self {
         self.contact_point_color = Some(color);
         self
     }
 
+    /// Sets the contact normal color.
+    pub fn with_contact_normal_color(mut self, color: Color) -> Self {
+        self.contact_normal_color = Some(color);
+        self
+    }
+
+    /// Sets the contact normal scale.
+    pub fn with_contact_normal_scale(mut self, scale: ContactGizmoScale) -> Self {
+        self.contact_normal_scale = scale;
+        self
+    }
+
     /// Sets the colors used for debug rendering joints.
     pub fn with_joint_colors(anchor_color: Option<Color>, separation_color: Option<Color>) -> Self {
         Self {
diff --git a/src/plugins/debug/mod.rs b/src/plugins/debug/mod.rs
index f8f1aca4..dc385003 100644
--- a/src/plugins/debug/mod.rs
+++ b/src/plugins/debug/mod.rs
@@ -281,6 +281,7 @@ fn debug_render_contacts(
     mut collisions: EventReader<Collision>,
     mut debug_renderer: PhysicsDebugRenderer,
     config: Res<PhysicsDebugConfig>,
+    time: Res<Time<Substeps>>,
 ) {
     if config.contact_point_color.is_none() && config.contact_normal_color.is_none() {
         return;
@@ -322,15 +323,30 @@ fn debug_render_contacts(
 
                 // Draw contact normals
                 if let Some(color) = config.contact_normal_color {
+                    // Use dimmer color for second normal
+                    let mut color_dim = color.as_hsla();
+                    color_dim.set_l(color_dim.l() * 0.5);
+
+                    // The length of the normal arrows
+                    let length = match config.contact_normal_scale {
+                        ContactGizmoScale::Constant(length) => length,
+                        ContactGizmoScale::Scaled(scale) => {
+                            scale * contacts.total_normal_impulse
+                                / time.delta_seconds_f64().adjust_precision()
+                        }
+                    };
+
                     #[cfg(feature = "2d")]
                     {
-                        debug_renderer.draw_arrow(p1, p1 + normal1 * 30.0, 8.0, 8.0, color);
-                        debug_renderer.draw_arrow(p2, p2 + normal2 * 30.0, 8.0, 8.0, color);
+                        debug_renderer.draw_arrow(p1, p1 + normal1 * length, 8.0, 8.0, color);
+                        debug_renderer.draw_arrow(p2, p2 + normal2 * length, 8.0, 8.0, color);
                     }
+
                     #[cfg(feature = "3d")]
                     {
-                        debug_renderer.draw_arrow(p1, p1 + normal1 * 0.5, 0.1, 0.1, color);
-                        debug_renderer.draw_arrow(p2, p2 + normal2 * 0.5, 0.1, 0.1, color);
+                        debug_renderer.draw_arrow(p1, p1 + normal1 * length, 0.1, 0.1, color);
+
+                        debug_renderer.draw_arrow(p2, p2 + normal2 * length, 0.1, 0.1, color_dim);
                     }
                 }
             }
diff --git a/src/plugins/setup/mod.rs b/src/plugins/setup/mod.rs
index a382639a..2c6c7db9 100644
--- a/src/plugins/setup/mod.rs
+++ b/src/plugins/setup/mod.rs
@@ -173,6 +173,7 @@ impl Plugin for PhysicsSetupPlugin {
                     SubstepSet::SolveUserConstraints,
                     SubstepSet::UpdateVelocities,
                     SubstepSet::SolveVelocities,
+                    SubstepSet::StoreImpulses,
                     SubstepSet::ApplyTranslation,
                 )
                     .chain(),
diff --git a/src/plugins/solver.rs b/src/plugins/solver.rs
index bb4cdd47..89352753 100644
--- a/src/plugins/solver.rs
+++ b/src/plugins/solver.rs
@@ -70,6 +70,8 @@ impl Plugin for SolverPlugin {
                 .in_set(SubstepSet::SolveVelocities),
         );
 
+        substeps.add_systems(store_contact_impulses.in_set(SubstepSet::StoreImpulses));
+
         substeps.add_systems(apply_translation.in_set(SubstepSet::ApplyTranslation));
     }
 }
@@ -167,15 +169,14 @@ fn penetration_constraints(
                 .friction
                 .unwrap_or(body1.friction)
                 .combine(*collider2.friction.unwrap_or(body2.friction));
-            let restitution_coefficient = collider1
+            let restitution = collider1
                 .restitution
                 .unwrap_or(body1.restitution)
-                .combine(*collider2.restitution.unwrap_or(body2.restitution))
-                .coefficient;
+                .combine(*collider2.restitution.unwrap_or(body2.restitution));
 
             // Create and solve penetration constraints for each contact.
-            for contact_manifold in contacts.manifolds.iter() {
-                for contact in contact_manifold.contacts.iter() {
+            for (manifold_index, manifold) in contacts.manifolds.iter().enumerate() {
+                for contact in manifold.contacts.iter() {
                     // Add collider transforms to local contact points
                     let contact = ContactData {
                         point1: collider1.transform.map_or(contact.point1, |t| {
@@ -194,10 +195,16 @@ fn penetration_constraints(
                     };
 
                     let mut constraint = PenetrationConstraint {
-                        dynamic_friction_coefficient: friction.dynamic_coefficient,
-                        static_friction_coefficient: friction.static_coefficient,
-                        restitution_coefficient,
-                        ..PenetrationConstraint::new(&body1, &body2, contact)
+                        friction,
+                        restitution,
+                        ..PenetrationConstraint::new(
+                            &body1,
+                            &body2,
+                            *collider_entity1,
+                            *collider_entity2,
+                            contact,
+                            manifold_index,
+                        )
                     };
                     constraint.solve([&mut body1, &mut body2], delta_secs);
                     penetration_constraints.0.push(constraint);
@@ -422,13 +429,13 @@ fn update_ang_vel(
 #[allow(clippy::type_complexity)]
 fn solve_vel(
     mut bodies: Query<RigidBodyQuery, Without<Sleeping>>,
-    penetration_constraints: Res<PenetrationConstraints>,
+    mut penetration_constraints: ResMut<PenetrationConstraints>,
     gravity: Res<Gravity>,
     time: Res<Time>,
 ) {
     let delta_secs = time.delta_seconds_adjusted();
 
-    for constraint in penetration_constraints.0.iter() {
+    for constraint in penetration_constraints.0.iter_mut() {
         if let Ok([mut body1, mut body2]) = bodies.get_many_mut(constraint.entities()) {
             if !body1.rb.is_dynamic() && !body2.rb.is_dynamic() {
                 continue;
@@ -479,29 +486,32 @@ fn solve_vel(
             let restitution_speed = compute_restitution(
                 normal_speed,
                 pre_solve_normal_speed,
-                constraint.restitution_coefficient,
+                constraint.restitution.coefficient,
                 gravity.0,
                 delta_secs,
             );
             if restitution_speed.abs() > Scalar::EPSILON {
                 let w1 = constraint.compute_generalized_inverse_mass(&body1, r1, normal);
                 let w2 = constraint.compute_generalized_inverse_mass(&body2, r2, normal);
-                p += restitution_speed / (w1 + w2) * normal;
+                let restitution_impulse = restitution_speed / (w1 + w2);
+                p += restitution_impulse * normal;
+                constraint.contact.normal_impulse += restitution_impulse;
             }
 
             // Compute dynamic friction
             if tangent_speed > Scalar::EPSILON {
-                let tangent_dir = tangent_vel / tangent_speed;
-                let w1 = constraint.compute_generalized_inverse_mass(&body1, r1, tangent_dir);
-                let w2 = constraint.compute_generalized_inverse_mass(&body2, r2, tangent_dir);
+                let tangent = tangent_vel / tangent_speed;
+                let w1 = constraint.compute_generalized_inverse_mass(&body1, r1, tangent);
+                let w2 = constraint.compute_generalized_inverse_mass(&body2, r2, tangent);
                 let friction_impulse = compute_dynamic_friction(
                     tangent_speed,
                     w1 + w2,
-                    constraint.dynamic_friction_coefficient,
+                    constraint.friction.dynamic_coefficient,
                     constraint.normal_lagrange,
                     delta_secs,
                 );
-                p += friction_impulse * tangent_dir;
+                p += friction_impulse * tangent;
+                constraint.contact.tangent_impulse += friction_impulse;
             }
 
             if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
@@ -588,6 +598,30 @@ pub fn joint_damping<T: Joint>(
     }
 }
 
+fn store_contact_impulses(
+    constraints: Res<PenetrationConstraints>,
+    mut collisions: ResMut<Collisions>,
+) {
+    for constraint in constraints.0.iter() {
+        let Some(collision) =
+            collisions.get_mut(constraint.collider_entity1, constraint.collider_entity2)
+        else {
+            continue;
+        };
+        if let Some(Some(contact)) = collision
+            .manifolds
+            .get_mut(constraint.manifold_index)
+            .map(|m| m.contacts.get_mut(constraint.contact.index))
+        {
+            contact.normal_impulse = constraint.contact.normal_impulse;
+            contact.tangent_impulse = constraint.contact.tangent_impulse;
+
+            collision.total_normal_impulse += contact.normal_impulse.abs();
+            collision.total_tangent_impulse += contact.tangent_impulse.abs();
+        }
+    }
+}
+
 #[allow(clippy::type_complexity)]
 fn apply_translation(
     mut bodies: Query<