Merge branch 'main' into collider-generics

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;
+    }
+}

src/components/mod.rs

@@ -329,6 +329,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))]

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 {

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`].