Flip writeback to iterate over rapier's bodies

bevy_rapier3d/Cargo.toml

@@ -46,6 +46,7 @@ serde = { version = "1", features = [ "derive" ], optional = true}
 bevy = { version = "0.11", default-features = false, features = ["x11"]}
 approx = "0.5.1"
 glam = { version = "0.24", features = [ "approx" ] }
+bevy-inspector-egui = "0.19"
 
 [package.metadata.docs.rs]
 # Enable all the features when building the docs on docs.rs

bevy_rapier3d/examples/boxes3.rs

@@ -12,6 +12,7 @@ fn main() {
             DefaultPlugins,
             RapierPhysicsPlugin::<NoUserData>::default(),
             RapierDebugRenderPlugin::default(),
+            bevy_inspector_egui::quick::WorldInspectorPlugin::default(),
         ))
         .add_systems(Startup, (setup_graphics, setup_physics))
         .run();
@@ -33,6 +34,7 @@ pub fn setup_physics(mut commands: Commands) {
     let ground_height = 0.1;
 
     commands.spawn((
+        Name::new("Ground"),
         TransformBundle::from(Transform::from_xyz(0.0, -ground_height, 0.0)),
         Collider::cuboid(ground_size, ground_height, ground_size),
     ));

bevy_rapier3d/examples/kinematic_velocity.rs

@@ -0,0 +1,32 @@
+use bevy::prelude::*;
+use bevy_rapier3d::prelude::*;
+
+fn main() {
+    App::new()
+        .add_plugins(DefaultPlugins)
+        .add_plugins(RapierPhysicsPlugin::<NoUserData>::default())
+        .add_plugins(RapierDebugRenderPlugin::default())
+        .insert_resource(RapierConfiguration::default())
+        .add_systems(Startup, setup)
+        .add_systems(Update, test)
+        .run();
+}
+
+fn test(ctx: Res<RapierContext>, query: Query<(Entity, &Transform), With<RigidBody>>) {
+    for (entity, transform) in &query {
+        info!("transform: {:.1?}", transform.translation);
+    }
+}
+
+fn setup(mut commands: Commands) {
+    commands.spawn(Camera3dBundle {
+        transform: Transform::from_xyz(-3.0, 3.0, 10.0).looking_at(Vec3::ZERO, Vec3::Y),
+        ..Default::default()
+    });
+
+    commands
+        .spawn(RigidBody::KinematicVelocityBased)
+        .insert(Collider::ball(0.5))
+        .insert(Velocity::linear(Vec3::Y * -1.81))
+        .insert(TransformBundle::from(Transform::from_xyz(0.0, 1.0, 0.0)));
+}

src/plugin/systems.rs

@@ -538,133 +538,132 @@ pub fn writeback_rigid_bodies(
     let context = &mut *context;
     let scale = context.physics_scale;
 
-    if config.physics_pipeline_active {
-        for (entity, parent, transform, mut interpolation, mut velocity, mut sleeping) in
-            writeback.iter_mut()
-        {
-            // TODO: do this the other way round: iterate through Rapier’s RigidBodySet on the active bodies,
-            // and update the components accordingly. That way, we don’t have to iterate through the entities that weren’t changed
-            // by physics (for example because they are sleeping).
-            if let Some(handle) = context.entity2body.get(&entity).copied() {
-                if let Some(rb) = context.bodies.get(handle) {
-                    let mut interpolated_pos = utils::iso_to_transform(rb.position(), scale);
+    if !config.physics_pipeline_active {
+        return;
+    }
 
-                    if let TimestepMode::Interpolated { dt, .. } = config.timestep_mode {
-                        if let Some(interpolation) = interpolation.as_deref_mut() {
-                            if interpolation.end.is_none() {
-                                interpolation.end = Some(*rb.position());
-                            }
+    for (handle, rb) in context.bodies.iter().filter(|(_, body)| body.is_enabled()) {
+        let Some(entity) = context.rigid_body_entity(handle) else {
+            continue;
+        };
 
-                            if let Some(interpolated) =
-                                interpolation.lerp_slerp((dt + sim_to_render_time.diff) / dt)
-                            {
-                                interpolated_pos = utils::iso_to_transform(&interpolated, scale);
-                            }
-                        }
-                    }
+        let Ok((_, parent, transform, mut interpolation, mut velocity, mut sleeping)) =
+            writeback.get_mut(entity)
+        else {
+            continue;
+        };
 
-                    if let Some(mut transform) = transform {
-                        // NOTE: we query the parent’s global transform here, which is a bit
-                        //       unfortunate (performance-wise). An alternative would be to
-                        //       deduce the parent’s global transform from the current entity’s
-                        //       global transform. However, this makes it nearly impossible
-                        //       (because of rounding errors) to predict the exact next value this
-                        //       entity’s global transform will get after the next transform
-                        //       propagation, which breaks our transform modification detection
-                        //       that we do to detect if the user’s transform has to be written
-                        //       into the rigid-body.
-                        if let Some(parent_global_transform) =
-                            parent.and_then(|p| global_transforms.get(**p).ok())
-                        {
-                            // We need to compute the new local transform such that:
-                            // curr_parent_global_transform * new_transform = interpolated_pos
-                            // new_transform = curr_parent_global_transform.inverse() * interpolated_pos
-                            let (_, inverse_parent_rotation, inverse_parent_translation) =
-                                parent_global_transform
-                                    .affine()
-                                    .inverse()
-                                    .to_scale_rotation_translation();
-                            let new_rotation = inverse_parent_rotation * interpolated_pos.rotation;
-
-                            #[allow(unused_mut)] // mut is needed in 2D but not in 3D.
-                            let mut new_translation = inverse_parent_rotation
-                                * interpolated_pos.translation
-                                + inverse_parent_translation;
-
-                            // In 2D, preserve the transform `z` component that may have been set by the user
-                            #[cfg(feature = "dim2")]
-                            {
-                                new_translation.z = transform.translation.z;
-                            }
+        if let Some(sleeping) = &mut sleeping {
+            // NOTE: we write the new value only if there was an
+            //       actual change, in order to not trigger bevy’s
+            //       change tracking when the values didn’t change.
+            if sleeping.sleeping != rb.is_sleeping() {
+                sleeping.sleeping = rb.is_sleeping();
+            }
+        }
 
-                            if transform.rotation != new_rotation
-                                || transform.translation != new_translation
-                            {
-                                // NOTE: we write the new value only if there was an
-                                //       actual change, in order to not trigger bevy’s
-                                //       change tracking when the values didn’t change.
-                                transform.rotation = new_rotation;
-                                transform.translation = new_translation;
-                            }
+        let mut interpolated_pos = utils::iso_to_transform(rb.position(), scale);
 
-                            // NOTE: we need to compute the result of the next transform propagation
-                            //       to make sure that our change detection for transforms is exact
-                            //       despite rounding errors.
-                            let new_global_transform =
-                                parent_global_transform.mul_transform(*transform);
-
-                            context
-                                .last_body_transform_set
-                                .insert(handle, new_global_transform);
-                        } else {
-                            // In 2D, preserve the transform `z` component that may have been set by the user
-                            #[cfg(feature = "dim2")]
-                            {
-                                interpolated_pos.translation.z = transform.translation.z;
-                            }
+        if let TimestepMode::Interpolated { dt, .. } = config.timestep_mode {
+            if let Some(interpolation) = interpolation.as_deref_mut() {
+                if interpolation.end.is_none() {
+                    interpolation.end = Some(*rb.position());
+                }
 
-                            if transform.rotation != interpolated_pos.rotation
-                                || transform.translation != interpolated_pos.translation
-                            {
-                                // NOTE: we write the new value only if there was an
-                                //       actual change, in order to not trigger bevy’s
-                                //       change tracking when the values didn’t change.
-                                transform.rotation = interpolated_pos.rotation;
-                                transform.translation = interpolated_pos.translation;
-                            }
+                if let Some(interpolated) =
+                    interpolation.lerp_slerp((dt + sim_to_render_time.diff) / dt)
+                {
+                    interpolated_pos = utils::iso_to_transform(&interpolated, scale);
+                }
+            }
+        }
 
-                            context
-                                .last_body_transform_set
-                                .insert(handle, GlobalTransform::from(interpolated_pos));
-                        }
-                    }
+        if let Some(mut transform) = transform {
+            // NOTE: we query the parent’s global transform here, which is a bit
+            //       unfortunate (performance-wise). An alternative would be to
+            //       deduce the parent’s global transform from the current entity’s
+            //       global transform. However, this makes it nearly impossible
+            //       (because of rounding errors) to predict the exact next value this
+            //       entity’s global transform will get after the next transform
+            //       propagation, which breaks our transform modification detection
+            //       that we do to detect if the user’s transform has to be written
+            //       into the rigid-body.
+            if let Some(parent_global_transform) =
+                parent.and_then(|p| global_transforms.get(**p).ok())
+            {
+                // We need to compute the new local transform such that:
+                // curr_parent_global_transform * new_transform = interpolated_pos
+                // new_transform = curr_parent_global_transform.inverse() * interpolated_pos
+                let (_, inverse_parent_rotation, inverse_parent_translation) =
+                    parent_global_transform
+                        .affine()
+                        .inverse()
+                        .to_scale_rotation_translation();
+                let new_rotation = inverse_parent_rotation * interpolated_pos.rotation;
+
+                #[allow(unused_mut)] // mut is needed in 2D but not in 3D.
+                let mut new_translation = inverse_parent_rotation * interpolated_pos.translation
+                    + inverse_parent_translation;
+
+                // In 2D, preserve the transform `z` component that may have been set by the user
+                #[cfg(feature = "dim2")]
+                {
+                    new_translation.z = transform.translation.z;
+                }
 
-                    if let Some(velocity) = &mut velocity {
-                        let new_vel = Velocity {
-                            linvel: (rb.linvel() * scale).into(),
-                            #[cfg(feature = "dim3")]
-                            angvel: (*rb.angvel()).into(),
-                            #[cfg(feature = "dim2")]
-                            angvel: rb.angvel(),
-                        };
+                if transform.rotation != new_rotation || transform.translation != new_translation {
+                    // NOTE: we write the new value only if there was an
+                    //       actual change, in order to not trigger bevy’s
+                    //       change tracking when the values didn’t change.
+                    transform.rotation = new_rotation;
+                    transform.translation = new_translation;
+                }
 
-                        // NOTE: we write the new value only if there was an
-                        //       actual change, in order to not trigger bevy’s
-                        //       change tracking when the values didn’t change.
-                        if **velocity != new_vel {
-                            **velocity = new_vel;
-                        }
-                    }
+                // NOTE: we need to compute the result of the next transform propagation
+                //       to make sure that our change detection for transforms is exact
+                //       despite rounding errors.
+                let new_global_transform = parent_global_transform.mul_transform(*transform);
 
-                    if let Some(sleeping) = &mut sleeping {
-                        // NOTE: we write the new value only if there was an
-                        //       actual change, in order to not trigger bevy’s
-                        //       change tracking when the values didn’t change.
-                        if sleeping.sleeping != rb.is_sleeping() {
-                            sleeping.sleeping = rb.is_sleeping();
-                        }
-                    }
+                context
+                    .last_body_transform_set
+                    .insert(handle, new_global_transform);
+            } else {
+                // In 2D, preserve the transform `z` component that may have been set by the user
+                #[cfg(feature = "dim2")]
+                {
+                    interpolated_pos.translation.z = transform.translation.z;
+                }
+
+                if transform.rotation != interpolated_pos.rotation
+                    || transform.translation != interpolated_pos.translation
+                {
+                    // NOTE: we write the new value only if there was an
+                    //       actual change, in order to not trigger bevy’s
+                    //       change tracking when the values didn’t change.
+                    transform.rotation = interpolated_pos.rotation;
+                    transform.translation = interpolated_pos.translation;
                 }
+
+                context
+                    .last_body_transform_set
+                    .insert(handle, GlobalTransform::from(interpolated_pos));
+            }
+        }
+
+        if let Some(velocity) = &mut velocity {
+            let new_vel = Velocity {
+                linvel: (rb.linvel() * scale).into(),
+                #[cfg(feature = "dim3")]
+                angvel: (*rb.angvel()).into(),
+                #[cfg(feature = "dim2")]
+                angvel: rb.angvel(),
+            };
+
+            // NOTE: we write the new value only if there was an
+            //       actual change, in order to not trigger bevy’s
+            //       change tracking when the values didn’t change.
+            if **velocity != new_vel {
+                **velocity = new_vel;
             }
         }
     }