diff --git a/CHANGELOG.md b/CHANGELOG.md index 4d8c214b2..05d34db15 100644 --- a/CHANGELOG.md +++ b/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: diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 2de58aecb..f475fd46d 100644 --- a/src/dynamics/integration_parameters.rs +++ b/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 diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index a7d2ec363..2ef91be0d 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/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 diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 95e408fee..7723fc30b 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -662,6 +662,8 @@ impl PhysicsPipeline { #[cfg(test)] mod test { + use na::point; + use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, RigidBodyBuilder, RigidBodySet, @@ -669,7 +671,7 @@ mod test { 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, + ¶meters, + &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()); + } + } }