dimforge · Jakkestt · Feb 28, 2025
diff --git a/src/plugin/context/mod.rs b/src/plugin/context/mod.rs
@@ -808,8 +808,7 @@ impl RapierContextSimulation {
             &EventWriter<ContactForceEvent>,
         )>,
         hooks: &dyn PhysicsHooks,
-        time: &Time,
-        sim_to_render_time: &mut SimulationToRenderTime,
+        time: &Time<Virtual>,
         mut interpolation_query: Option<
             &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>,
         >,
@@ -837,50 +836,36 @@ impl RapierContextSimulation {
                 time_scale,
                 substeps,
             } => {
-                self.integration_parameters.dt = dt;
+                let mut substep_integration_parameters = self.integration_parameters;
+                substep_integration_parameters.dt = dt / (substeps as Real) * time_scale;
 
-                sim_to_render_time.diff += time.delta_secs();
-
-                while sim_to_render_time.diff > 0.0 {
-                    // NOTE: in this comparison we do the same computations we
-                    // will do for the next `while` iteration test, to make sure we
-                    // don't get bit by potential float inaccuracy.
-                    if sim_to_render_time.diff - dt <= 0.0 {
-                        if let Some(interpolation_query) = interpolation_query.as_mut() {
-                            // This is the last simulation step to be executed in the loop
-                            // Update the previous state transforms
-                            for (handle, mut interpolation) in interpolation_query.iter_mut() {
-                                if let Some(body) = rigidbody_set.bodies.get(handle.0) {
-                                    interpolation.start = Some(*body.position());
-                                    interpolation.end = None;
-                                }
-                            }
-                        }
-                    }
+                for _ in 0..substeps {
+                    self.pipeline.step(
+                        &gravity.into(),
+                        &substep_integration_parameters,
+                        &mut self.islands,
+                        &mut self.broad_phase,
+                        &mut self.narrow_phase,
+                        &mut rigidbody_set.bodies,
+                        &mut colliders.colliders,
+                        &mut joints.impulse_joints,
+                        &mut joints.multibody_joints,
+                        &mut self.ccd_solver,
+                        None,
+                        hooks,
+                        event_handler,
+                    );
+                    executed_steps += 1;
+                }
 
-                    let mut substep_integration_parameters = self.integration_parameters;
-                    substep_integration_parameters.dt = dt / (substeps as Real) * time_scale;
-
-                    for _ in 0..substeps {
-                        self.pipeline.step(
-                            &gravity.into(),
-                            &substep_integration_parameters,
-                            &mut self.islands,
-                            &mut self.broad_phase,
-                            &mut self.narrow_phase,
-                            &mut rigidbody_set.bodies,
-                            &mut colliders.colliders,
-                            &mut joints.impulse_joints,
-                            &mut joints.multibody_joints,
-                            &mut self.ccd_solver,
-                            None,
-                            hooks,
-                            event_handler,
-                        );
-                        executed_steps += 1;
+                if let Some(interpolation_query) = interpolation_query.as_mut() {
+                    // Update the previous state transforms
+                    for (handle, mut interpolation) in interpolation_query.iter_mut() {
+                        if let Some(body) = rigidbody_set.bodies.get(handle.0) {
+                            interpolation.start = interpolation.end;
+                            interpolation.end = Some(*body.position());
+                        }
                     }
-
-                    sim_to_render_time.diff -= dt;
                 }
             }
             TimestepMode::Variable {

diff --git a/src/plugin/context/systemparams/rapier_context_systemparam.rs b/src/plugin/context/systemparams/rapier_context_systemparam.rs
@@ -148,7 +148,6 @@ mod simulation {
     use crate::control::CharacterCollision;
     use crate::control::MoveShapeOptions;
     use crate::control::MoveShapeOutput;
-    use crate::plugin::context::SimulationToRenderTime;
     use crate::plugin::ContactPairView;
     use crate::plugin::TimestepMode;
     use crate::prelude::Collider;
@@ -210,8 +209,7 @@ mod simulation {
                 &EventWriter<ContactForceEvent>,
             )>,
             hooks: &dyn PhysicsHooks,
-            time: &Time,
-            sim_to_render_time: &mut SimulationToRenderTime,
+            time: &Time<Virtual>,
             interpolation_query: Option<
                 &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>,
             >,
@@ -225,7 +223,6 @@ mod simulation {
                 events,
                 hooks,
                 time,
-                sim_to_render_time,
                 interpolation_query,
             )
         }

diff --git a/src/plugin/plugin.rs b/src/plugin/plugin.rs
@@ -298,14 +298,26 @@ where
         if self.default_system_setup {
             app.configure_sets(
                 self.schedule,
+                (PhysicsSet::SyncBackend, PhysicsSet::StepSimulation)
+                    .chain()
+                    .before(TransformSystem::TransformPropagate),
+            );
+            app.add_systems(
+                PostUpdate,
                 (
-                    PhysicsSet::SyncBackend,
-                    PhysicsSet::StepSimulation,
-                    PhysicsSet::Writeback,
+                    systems::update_colliding_entities,
+                    systems::writeback_rigid_bodies,
+                    systems::writeback_mass_properties
+                        .ambiguous_with(systems::writeback_rigid_bodies),
                 )
                     .chain()
                     .before(TransformSystem::TransformPropagate),
             );
+            // app.configure_sets(
+            //     PostUpdate,
+            //     (PhysicsSet::Writeback).before(TransformSystem::TransformPropagate),
+            // );
+            app.add_systems(FixedPreUpdate, systems::update_rigid_bodies);
             app.configure_sets(
                 self.schedule,
                 RapierTransformPropagateSet.in_set(PhysicsSet::SyncBackend),
@@ -320,7 +332,7 @@ where
                 }
             };
             add_systems_if_enabled(PhysicsSet::SyncBackend);
-            add_systems_if_enabled(PhysicsSet::Writeback);
+            // add_systems_if_enabled(PhysicsSet::Writeback);
             add_systems_if_enabled(PhysicsSet::StepSimulation);
 
             app.init_resource::<TimestepMode>();
@@ -329,7 +341,7 @@ where
             if self.schedule.as_dyn_eq().dyn_eq(FixedUpdate.as_dyn_eq()) {
                 let config = app.world_mut().resource::<TimestepMode>();
                 match config {
-                    TimestepMode::Fixed { .. } => {}
+                    TimestepMode::Fixed { .. } | TimestepMode::Interpolated { .. } => {}
                     mode => {
                         warn!("TimestepMode is set to `{:?}`, it is recommended to use `TimestepMode::Fixed` if you have the physics in `FixedUpdate`", mode);
                     }

diff --git a/src/plugin/systems/mod.rs b/src/plugin/systems/mod.rs
@@ -18,7 +18,6 @@ pub use writeback::*;
 
 use crate::dynamics::{RapierRigidBodyHandle, TransformInterpolation};
 use crate::pipeline::{CollisionEvent, ContactForceEvent};
-use crate::plugin::context::SimulationToRenderTime;
 use crate::plugin::{RapierConfiguration, TimestepMode};
 use crate::prelude::{BevyPhysicsHooks, BevyPhysicsHooksAdapter};
 use bevy::ecs::system::{StaticSystemParam, SystemParamItem};
@@ -39,11 +38,10 @@ pub fn step_simulation<Hooks>(
         &mut RapierContextJoints,
         &mut RapierRigidBodySet,
         &RapierConfiguration,
-        &mut SimulationToRenderTime,
     )>,
     timestep_mode: Res<TimestepMode>,
     hooks: StaticSystemParam<Hooks>,
-    time: Res<Time>,
+    time: Res<Time<Virtual>>,
     mut collision_events: EventWriter<CollisionEvent>,
     mut contact_force_events: EventWriter<ContactForceEvent>,
     mut interpolation_query: Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>,
@@ -60,7 +58,6 @@ pub fn step_simulation<Hooks>(
         mut joints,
         mut rigidbody_set,
         config,
-        mut sim_to_render_time,
     ) in context.iter_mut()
     {
         let context = &mut *context;
@@ -76,7 +73,6 @@ pub fn step_simulation<Hooks>(
                 Some((&collision_events, &contact_force_events)),
                 &hooks_adapter,
                 &time,
-                &mut sim_to_render_time,
                 Some(&mut interpolation_query),
             );
         } else {

diff --git a/src/plugin/systems/rigid_body.rs b/src/plugin/systems/rigid_body.rs
@@ -1,10 +1,10 @@
 use crate::dynamics::RapierRigidBodyHandle;
+use crate::dynamics::RigidBody;
 use crate::plugin::context::systemparams::RAPIER_CONTEXT_EXPECT_ERROR;
 use crate::plugin::context::{
     DefaultRapierContext, RapierContextColliders, RapierContextEntityLink, RapierRigidBodySet,
 };
 use crate::plugin::{configuration::TimestepMode, RapierConfiguration};
-use crate::{dynamics::RigidBody, plugin::context::SimulationToRenderTime};
 use crate::{prelude::*, utils};
 use bevy::prelude::*;
 use rapier::dynamics::{RigidBodyBuilder, RigidBodyHandle, RigidBodyType};
@@ -204,7 +204,7 @@ pub fn apply_rigid_body_user_changes(
         // Use an Option<bool> to avoid running the check twice.
         let mut transform_changed = None;
 
-        if let Some(interpolation) = interpolation.as_deref_mut() {
+        if interpolation.as_deref_mut().is_some() {
             transform_changed = transform_changed.or_else(|| {
                 Some(transform_changed_fn(
                     &handle.0,
@@ -213,13 +213,6 @@ pub fn apply_rigid_body_user_changes(
                     &rigidbody_set.last_body_transform_set,
                 ))
             });
-
-            if transform_changed == Some(true) {
-                // Reset the interpolation so we don’t overwrite
-                // the user’s input.
-                interpolation.start = None;
-                interpolation.end = None;
-            }
         }
         // TODO: avoid to run multiple times the mutable deref ?
         if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) {
@@ -404,12 +397,12 @@ pub fn writeback_rigid_bodies(
     mut rigid_body_sets: Query<&mut RapierRigidBodySet>,
     timestep_mode: Res<TimestepMode>,
     config: Query<&RapierConfiguration>,
-    sim_to_render_time: Query<&SimulationToRenderTime>,
     global_transforms: Query<&GlobalTransform>,
     mut writeback: Query<
         RigidBodyWritebackComponents,
         (With<RigidBody>, Without<RigidBodyDisabled>),
     >,
+    fixed_time: Res<Time<Fixed>>,
 ) {
     for (handle, link, parent, transform, mut interpolation, mut velocity, mut sleeping) in
         writeback.iter_mut()
@@ -426,23 +419,16 @@ pub fn writeback_rigid_bodies(
             .get_mut(link.0)
             .expect(RAPIER_CONTEXT_EXPECT_ERROR)
             .into_inner();
-        let sim_to_render_time = sim_to_render_time
-            .get(link.0)
-            .expect("Could not get `SimulationToRenderTime`");
         // 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(rb) = rigid_body_set.bodies.get(handle) {
             let mut interpolated_pos = utils::iso_to_transform(rb.position());
 
-            if let TimestepMode::Interpolated { dt, .. } = *timestep_mode {
+            if let TimestepMode::Interpolated { .. } = *timestep_mode {
                 if let Some(interpolation) = interpolation.as_deref_mut() {
-                    if interpolation.end.is_none() {
-                        interpolation.end = Some(*rb.position());
-                    }
-
                     if let Some(interpolated) =
-                        interpolation.lerp_slerp((dt + sim_to_render_time.diff) / dt)
+                        interpolation.lerp_slerp(fixed_time.overstep_fraction())
                     {
                         interpolated_pos = utils::iso_to_transform(&interpolated);
                     }
@@ -494,7 +480,7 @@ pub fn writeback_rigid_bodies(
                         // 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.rotation = new_rotation;
                         transform.translation = new_translation;
                     }
 
@@ -519,7 +505,7 @@ pub fn writeback_rigid_bodies(
                         // 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.rotation = interpolated_pos.rotation;
                         transform.translation = interpolated_pos.translation;
                     }
 
@@ -587,6 +573,7 @@ pub fn init_rigid_bodies(
         builder = builder.enabled(disabled.is_none());
 
         if let Some(transform) = transform {
+            info!("{:?}", transform);
             builder = builder.position(utils::transform_to_iso(&transform.compute_transform()));
         }
 
@@ -727,3 +714,28 @@ pub fn apply_initial_rigid_body_impulses(
         }
     }
 }
+
+/// Make sure to set the position of the rigid body to the end of the interpolation
+/// at the start of the fixed update so that we don't translate on the interpolated
+/// position
+pub fn update_rigid_bodies(
+    mut rigid_body_sets: Query<&mut RapierRigidBodySet>,
+    mut rigid_bodies: Query<
+        (
+            &RapierRigidBodyHandle,
+            &RapierContextEntityLink,
+            &mut Transform,
+        ),
+        (With<RigidBody>, Without<RigidBodyDisabled>),
+    >,
+) {
+    for (handle, link, mut transform) in &mut rigid_bodies {
+        let rigid_body_set = rigid_body_sets
+            .get_mut(link.0)
+            .expect(RAPIER_CONTEXT_EXPECT_ERROR)
+            .into_inner();
+        if let Some(rb) = rigid_body_set.bodies.get(handle.0) {
+            transform.translation = rb.position().translation.into();
+        }
+    }
+}