Fix delta_time being 0 resulting in incorrect simulation (#660)

* Add failing test

* fix tests

* better fix

* add changelog

* fix propagated to `contact_cfm_factor`

* PR feedback

* more PR feedbacks

CHANGELOG.md

@@ -1,3 +1,9 @@
+## Unreleased
+
+### Fix
+
+- Fix `NaN` values appearing in bodies translation and rotation after a simulation step with a delta time equal to 0 ([#660](https://github.com/dimforge/rapier/pull/660)).
+
 ## v0.20.0 (9 June 2024)
 
 This release introduces two new crates:

src/dynamics/integration_parameters.rs

@@ -181,7 +181,12 @@ impl IntegrationParameters {
     /// [`Self::contact_damping_ratio`] and the substep length.
     pub fn contact_cfm_factor(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
-        let inv_erp_minus_one = 1.0 / self.contact_erp() - 1.0;
+        // The logic is similar to [`Self::joint_cfm_coeff`].
+        let contact_erp = self.contact_erp();
+        if contact_erp == 0.0 {
+            return 0.0;
+        }
+        let inv_erp_minus_one = 1.0 / contact_erp - 1.0;
 
         // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
         //     / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
@@ -220,8 +225,12 @@ impl IntegrationParameters {
     /// [`Self::joint_damping_ratio`] and the substep length.
     pub fn joint_cfm_coeff(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
-        // The logic is similar to `Self::cfm_factor`.
-        let inv_erp_minus_one = 1.0 / self.joint_erp() - 1.0;
+        // The logic is similar to `Self::contact_cfm_factor`.
+        let joint_erp = self.joint_erp();
+        if joint_erp == 0.0 {
+            return 0.0;
+        }
+        let inv_erp_minus_one = 1.0 / joint_erp - 1.0;
         inv_erp_minus_one * inv_erp_minus_one
             / ((1.0 + inv_erp_minus_one)
                 * 4.0

src/dynamics/rigid_body_set.rs

@@ -23,7 +23,7 @@ impl BodyPair {
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Clone, Default)]
+#[derive(Clone, Default, Debug)]
 /// A set of rigid bodies that can be handled by a physics pipeline.
 pub struct RigidBodySet {
     // NOTE: the pub(crate) are needed by the broad phase

src/pipeline/physics_pipeline.rs

@@ -662,14 +662,16 @@ impl PhysicsPipeline {
 
 #[cfg(test)]
 mod test {
+    use na::point;
+
     use crate::dynamics::{
         CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, RigidBodyBuilder,
         RigidBodySet,
     };
     use crate::geometry::{BroadPhaseMultiSap, ColliderBuilder, ColliderSet, NarrowPhase};
     use crate::math::Vector;
     use crate::pipeline::PhysicsPipeline;
-    use crate::prelude::{MultibodyJointSet, RigidBodyType};
+    use crate::prelude::{MultibodyJointSet, RevoluteJointBuilder, RigidBodyType};
 
     #[test]
     fn kinematic_and_fixed_contact_crash() {
@@ -937,4 +939,68 @@ mod test {
         // Expect body to now be in active_dynamic_set
         assert!(islands.active_dynamic_set.contains(&h));
     }
+
+    #[test]
+    fn joint_step_delta_time_0() {
+        let mut colliders = ColliderSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
+        let mut multibody_joints = MultibodyJointSet::new();
+        let mut pipeline = PhysicsPipeline::new();
+        let mut bf = BroadPhaseMultiSap::new();
+        let mut nf = NarrowPhase::new();
+        let mut islands = IslandManager::new();
+
+        let mut bodies = RigidBodySet::new();
+
+        // Initialize bodies
+        let rb = RigidBodyBuilder::fixed().additional_mass(1.0).build();
+        let h = bodies.insert(rb.clone());
+        let rb_dynamic = RigidBodyBuilder::dynamic().additional_mass(1.0).build();
+        let h_dynamic = bodies.insert(rb_dynamic.clone());
+
+        // Add joint
+        #[cfg(feature = "dim2")]
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(point![0.0, 1.0])
+            .local_anchor2(point![0.0, -3.0]);
+        #[cfg(feature = "dim3")]
+        let joint = RevoluteJointBuilder::new(Vector::z_axis())
+            .local_anchor1(point![0.0, 1.0, 0.0])
+            .local_anchor2(point![0.0, -3.0, 0.0]);
+        impulse_joints.insert(h, h_dynamic, joint, true);
+
+        let mut parameters = IntegrationParameters::default();
+        parameters.dt = 0.0;
+        // Step once
+        let gravity = Vector::y() * -9.81;
+        pipeline.step(
+            &gravity,
+            &parameters,
+            &mut islands,
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut impulse_joints,
+            &mut multibody_joints,
+            &mut CCDSolver::new(),
+            None,
+            &(),
+            &(),
+        );
+        let translation = bodies[h_dynamic].translation();
+        let rotation = bodies[h_dynamic].rotation();
+        assert!(translation.x.is_finite());
+        assert!(translation.y.is_finite());
+        #[cfg(feature = "dim2")]
+        assert!(rotation.is_finite());
+        #[cfg(feature = "dim3")]
+        {
+            assert!(translation.z.is_finite());
+            assert!(rotation.i.is_finite());
+            assert!(rotation.j.is_finite());
+            assert!(rotation.k.is_finite());
+            assert!(rotation.w.is_finite());
+        }
+    }
 }