Merge pull request #2 from liquidev/fix/interpolation

Fix interpolation

src/plugin/context.rs

@@ -19,6 +19,8 @@ use crate::dynamics::TransformInterpolation;
 use crate::prelude::{CollisionGroups, RapierRigidBodyHandle};
 use rapier::control::CharacterAutostep;
 
+use super::interpolation_context::RapierInterpolationContext;
+
 /// The Rapier context, containing all the state of the physics engine.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Resource)]
@@ -49,6 +51,7 @@ pub struct RapierContext {
     /// The integration parameters, controlling various low-level coefficient of the simulation.
     pub integration_parameters: IntegrationParameters,
     pub(crate) physics_scale: Real,
+    pub(crate) interpolation: RapierInterpolationContext,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     pub(crate) event_handler: Option<Box<dyn EventHandler>>,
     // For transform change detection.
@@ -86,6 +89,7 @@ impl Default for RapierContext {
             query_pipeline: QueryPipeline::new(),
             integration_parameters: IntegrationParameters::default(),
             physics_scale: 1.0,
+            interpolation: RapierInterpolationContext::default(),
             event_handler: None,
             last_body_transform_set: HashMap::new(),
             entity2body: HashMap::new(),
@@ -227,13 +231,6 @@ impl RapierContext {
 
         self.integration_parameters.dt = delta_time;
 
-        for (handle, mut interpolation) in interpolation_query.iter_mut() {
-            if let Some(body) = self.bodies.get(handle.0) {
-                interpolation.start = interpolation.end;
-                interpolation.end = Some(*body.position());
-            }
-        }
-
         let mut substep_integration_parameters = self.integration_parameters;
         substep_integration_parameters.dt = delta_time / substeps as Real;
 
@@ -254,6 +251,18 @@ impl RapierContext {
                 events,
             );
         }
+
+        // NOTE: Update the interpolation data must be after all substeps.
+        for (handle, mut interpolation) in interpolation_query.iter_mut() {
+            if let Some(body) = self.bodies.get(handle.0) {
+                if self.interpolation.is_after_update() {
+                    interpolation.start = interpolation.end;
+                }
+                interpolation.end = Some(*body.position());
+            }
+        }
+
+        self.interpolation.notice_fixed_update_frame();
     }
 
     /// This method makes sure tha the rigid-body positions have been propagated to

src/plugin/interpolation_context.rs

@@ -0,0 +1,59 @@
+use std::collections::VecDeque;
+
+use bevy::time::{Fixed, Real, Time};
+
+pub(crate) struct RapierInterpolationContext {
+    // Collect lasts delta times across the presentation frames.
+    temporal_presentation_delta_time: VecDeque<f32>,
+    elapsed_time_since_last_fixed_update: f32,
+}
+
+impl RapierInterpolationContext {
+    pub fn get_lerp_percentage_for_frame(
+        &mut self,
+        fixed_time: &Time<Fixed>,
+        presentation_time: &Time<Real>,
+    ) -> f32 {
+        // NOTE: Create little margin in miliseconds which prevents constantly interpolation mode switching e.g. when
+        //       fixed delta time is almost equal to presentation delta time.
+        const INTERPOLATION_MODE_MARGIN: f32 = 0.001;
+
+        // Smooths out the presentation delta time over the last N frames.
+        self.temporal_presentation_delta_time.pop_front();
+        self.temporal_presentation_delta_time
+            .push_back(presentation_time.delta_seconds());
+        let temporal_presentation_time_delta =
+            self.temporal_presentation_delta_time.iter().sum::<f32>()
+                / self.temporal_presentation_delta_time.len() as f32;
+
+        self.elapsed_time_since_last_fixed_update += presentation_time.delta_seconds();
+
+        // NOTE: Use different interpolation modes depending where fixed delta time is greater than presentation delta
+        //       time and otherwise when is not.
+        match fixed_time.delta_seconds() + INTERPOLATION_MODE_MARGIN
+            >= temporal_presentation_time_delta
+        {
+            true => fixed_time.overstep_percentage(),
+            false => (self.elapsed_time_since_last_fixed_update
+                / presentation_time.delta_seconds())
+            .min(1.0),
+        }
+    }
+
+    pub fn notice_fixed_update_frame(&mut self) {
+        self.elapsed_time_since_last_fixed_update = 0.0;
+    }
+
+    pub fn is_after_update(&self) -> bool {
+        self.elapsed_time_since_last_fixed_update > 0.0
+    }
+}
+
+impl Default for RapierInterpolationContext {
+    fn default() -> Self {
+        Self {
+            temporal_presentation_delta_time: vec![0.0; 8].into(),
+            elapsed_time_since_last_fixed_update: 0.0,
+        }
+    }
+}

src/plugin/mod.rs

@@ -8,6 +8,7 @@ pub mod systems;
 
 mod configuration;
 mod context;
+mod interpolation_context;
 mod narrow_phase;
 #[allow(clippy::module_inception)]
 mod plugin;

src/plugin/plugin.rs

@@ -1,6 +1,7 @@
 use crate::pipeline::{CollisionEvent, ContactForceEvent};
 use crate::plugin::{systems, RapierConfiguration, RapierContext};
 use crate::prelude::*;
+use bevy::app::RunFixedUpdateLoop;
 use bevy::{
     ecs::{
         event::{event_update_system, Events},
@@ -205,31 +206,31 @@ where
         // Add each set as necessary
         if self.default_system_setup {
             app.configure_sets(
-                PreUpdate,
+                RunFixedUpdateLoop,
                 PhysicsSet::SyncBackend.before(TransformSystem::TransformPropagate),
             );
             app.configure_sets(
                 FixedUpdate,
                 PhysicsSet::StepSimulation.before(TransformSystem::TransformPropagate),
             );
             app.configure_sets(
-                PostUpdate,
+                Update,
                 PhysicsSet::Writeback.before(TransformSystem::TransformPropagate),
             );
 
             // These *must* be in the main schedule currently so that they do not miss events.
             app.add_systems(PostUpdate, (systems::sync_removals,));
 
             app.add_systems(
-                PreUpdate,
+                RunFixedUpdateLoop,
                 Self::get_systems(PhysicsSet::SyncBackend).in_set(PhysicsSet::SyncBackend),
             );
             app.add_systems(
                 FixedUpdate,
                 Self::get_systems(PhysicsSet::StepSimulation).in_set(PhysicsSet::StepSimulation),
             );
             app.add_systems(
-                PostUpdate,
+                Update,
                 Self::get_systems(PhysicsSet::Writeback).in_set(PhysicsSet::Writeback),
             );
         }

src/plugin/systems.rs

@@ -534,7 +534,8 @@ pub fn apply_joint_user_changes(
 pub fn writeback_rigid_bodies(
     mut context: ResMut<RapierContext>,
     config: Res<RapierConfiguration>,
-    time: Res<Time<Fixed>>,
+    fixed_time: Res<Time<Fixed>>,
+    presentation_time: Res<Time<bevy::time::Real>>,
     global_transforms: Query<&GlobalTransform>,
     mut writeback: Query<
         RigidBodyWritebackComponents,
@@ -543,6 +544,9 @@ pub fn writeback_rigid_bodies(
 ) {
     let context = &mut *context;
     let scale = context.physics_scale;
+    let lerp_percentage = context
+        .interpolation
+        .get_lerp_percentage_for_frame(&fixed_time, &presentation_time);
 
     if config.physics_pipeline_active {
         for (entity, parent, transform, mut interpolation, mut velocity, mut sleeping) in
@@ -560,9 +564,7 @@ pub fn writeback_rigid_bodies(
                             interpolation.end = Some(*rb.position());
                         }
 
-                        if let Some(interpolated) =
-                            interpolation.lerp_slerp(time.overstep_percentage())
-                        {
+                        if let Some(interpolated) = interpolation.lerp_slerp(lerp_percentage) {
                             interpolated_pos = utils::iso_to_transform(&interpolated, scale);
                         }
                     }
@@ -1465,7 +1467,16 @@ pub fn update_character_controls(
                 }
             }
 
-            if let Ok(mut transform) = transforms.get_mut(entity_to_move) {
+            // NOTE: Update the translation of the Rapier's rigid body without editing the Bevy's transform for
+            //       preventing noticing change in the [`systems::apply_rigid_body_user_changes`] for kinematic bodies.
+            if let Some(body) = body_handle.and_then(|h| context.bodies.get_mut(h.0)) {
+                // TODO: Use `set_next_kinematic_translation` for position based kinematic bodies? - Vixenka 29.01.2024
+                body.set_translation(
+                    body.translation() + movement.translation * physics_scale,
+                    true,
+                );
+            } else if let Ok(mut transform) = transforms.get_mut(entity_to_move) {
+                // I do not know if this else case is still needed, but it was existing in the original code. - Vixenka 29.01.2024
                 // TODO: take the parent’s GlobalTransform rotation into account?
                 transform.translation.x += movement.translation.x * physics_scale;
                 transform.translation.y += movement.translation.y * physics_scale;