From 6578d97ac89d1f087fbfd8ce9325ea73e9a27669 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 11 Jun 2025 17:34:27 +0200 Subject: [PATCH 1/3] add minimal reproduction test Co-authored-by: brendan c <2380975+bcolloran@users.noreply.github.com> --- .../contact_constraint/two_body_constraint.rs | 90 +++++++++++++++++++ src/geometry/contact_pair.rs | 11 +++ 2 files changed, 101 insertions(+) diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 9adcd5453..5f9161e77 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -531,3 +531,93 @@ where [tangent1, bitangent1] } + +#[cfg(feature = "dim2")] +#[cfg(test)] +mod test { + use std::f32::NAN; + + use approx::assert_relative_ne; + use na::vector; + use na::Vector; + + use crate::prelude::{ + BroadPhaseMultiSap, CCDSolver, ColliderBuilder, ColliderSet, ImpulseJointSet, + IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, + Real, RigidBodyBuilder, RigidBodySet, + }; + + #[test] + fn tangent_not_nan_818() { + /* + * World + */ + + let mut bodies = RigidBodySet::new(); + 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(); + + /* + * The ground + */ + let ground_size = 20.0; + let ground_height = 1.0; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -3.0]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height) + .rotation((0.25 as crate::math::Real).to_radians()) + .friction(0.9); + colliders.insert_with_parent(collider, handle, &mut bodies); + + /* + * A tilted capsule that cannot rotate. + */ + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 4.0]) + .lock_rotations(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(2.0, 1.0).friction(0.9); + colliders.insert_with_parent(collider, handle, &mut bodies); + + // Steps + let gravity = Vector::y() * -9.81; + + for i in 0..51 { + if i % 50 == 0 { + let pairs: Vec<_> = nf.contact_pairs().collect(); + println!("contact pairs: {:?}", pairs); + for contact in pairs { + for manifold in contact.manifolds.iter() { + for point in manifold.points.iter() { + assert!( + point.data.tangent_impulse.index(0).is_normal(), + "tangent impulse from a contact pair point data should be normal." + ); + } + } + } + } + pipeline.step( + &gravity, + &IntegrationParameters::default(), + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut CCDSolver::new(), + None, + &(), + &(), + ); + } + } +} diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 3f569b200..b8f1f1833 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -130,6 +130,17 @@ pub struct ContactPair { pub(crate) workspace: Option, } +impl std::fmt::Debug for ContactPair { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("ContactPair") + .field("collider1", &self.collider1) + .field("collider2", &self.collider2) + .field("manifolds", &self.manifolds) + .field("has_any_active_contact", &self.has_any_active_contact) + .finish() + } +} + impl ContactPair { pub(crate) fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { Self { From d3de7813a0465456233d5568dc6cc1193b00959f Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 11 Jun 2025 18:06:08 +0200 Subject: [PATCH 2/3] set impulse_accumulator to zero in constraint builder Co-authored-by: SlivoviyBarsik <71325326+SlivoviyBarsik@users.noreply.github.com> --- .../contact_constraint/one_body_constraint.rs | 1 + .../contact_constraint/two_body_constraint.rs | 37 ++++++++----------- 2 files changed, 16 insertions(+), 22 deletions(-) diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 5e6e86b3c..2f1f723b9 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -162,6 +162,7 @@ impl OneBodyConstraintBuilder { { constraint.elements[k].tangent_part.impulse = manifold_point.warmstart_tangent_impulse; + constraint.elements[k].tangent_part.impulse_accumulator = na::zero(); for j in 0..DIM - 1 { let gcross2 = mprops2 diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 5f9161e77..85cd9c0c5 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -535,17 +535,12 @@ where #[cfg(feature = "dim2")] #[cfg(test)] mod test { - use std::f32::NAN; - - use approx::assert_relative_ne; - use na::vector; - use na::Vector; - use crate::prelude::{ BroadPhaseMultiSap, CCDSolver, ColliderBuilder, ColliderSet, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, - Real, RigidBodyBuilder, RigidBodySet, + RigidBodyBuilder, RigidBodySet, }; + use na::{vector, Vector}; #[test] fn tangent_not_nan_818() { @@ -588,21 +583,7 @@ mod test { // Steps let gravity = Vector::y() * -9.81; - for i in 0..51 { - if i % 50 == 0 { - let pairs: Vec<_> = nf.contact_pairs().collect(); - println!("contact pairs: {:?}", pairs); - for contact in pairs { - for manifold in contact.manifolds.iter() { - for point in manifold.points.iter() { - assert!( - point.data.tangent_impulse.index(0).is_normal(), - "tangent impulse from a contact pair point data should be normal." - ); - } - } - } - } + for _ in 0..50 { pipeline.step( &gravity, &IntegrationParameters::default(), @@ -619,5 +600,17 @@ mod test { &(), ); } + let pairs: Vec<_> = nf.contact_pairs().collect(); + println!("contact pairs: {:?}", pairs); + for contact in pairs { + for manifold in contact.manifolds.iter() { + for point in manifold.points.iter() { + assert!( + point.data.tangent_impulse.index(0).is_finite(), + "tangent impulse from a contact pair point data should be normal." + ); + } + } + } } } From e4b17a332f63a4808fb57b9eb614744d7f967c6f Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 11 Jun 2025 18:07:14 +0200 Subject: [PATCH 3/3] moved test in a better place --- src/dynamics/solver/contact_constraint/mod.rs | 83 +++++++++++++++++++ .../contact_constraint/two_body_constraint.rs | 83 ------------------- 2 files changed, 83 insertions(+), 83 deletions(-) diff --git a/src/dynamics/solver/contact_constraint/mod.rs b/src/dynamics/solver/contact_constraint/mod.rs index 09099dd8a..7cecc11df 100644 --- a/src/dynamics/solver/contact_constraint/mod.rs +++ b/src/dynamics/solver/contact_constraint/mod.rs @@ -27,3 +27,86 @@ mod two_body_constraint; mod two_body_constraint_element; #[cfg(feature = "simd-is-enabled")] mod two_body_constraint_simd; + +#[cfg(feature = "dim2")] +#[cfg(test)] +mod test { + use crate::prelude::{ + BroadPhaseMultiSap, CCDSolver, ColliderBuilder, ColliderSet, ImpulseJointSet, + IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, + RigidBodyBuilder, RigidBodySet, + }; + use na::{vector, Vector}; + + #[test] + fn tangent_not_nan_818() { + /* + * World + */ + + let mut bodies = RigidBodySet::new(); + 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(); + + /* + * The ground + */ + let ground_size = 20.0; + let ground_height = 1.0; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -3.0]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height) + .rotation((0.25 as crate::math::Real).to_radians()) + .friction(0.9); + colliders.insert_with_parent(collider, handle, &mut bodies); + + /* + * A tilted capsule that cannot rotate. + */ + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 4.0]) + .lock_rotations(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(2.0, 1.0).friction(0.9); + colliders.insert_with_parent(collider, handle, &mut bodies); + + // Steps + let gravity = Vector::y() * -9.81; + + for _ in 0..50 { + pipeline.step( + &gravity, + &IntegrationParameters::default(), + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut CCDSolver::new(), + None, + &(), + &(), + ); + } + let pairs: Vec<_> = nf.contact_pairs().collect(); + println!("contact pairs: {:?}", pairs); + for contact in pairs { + for manifold in contact.manifolds.iter() { + for point in manifold.points.iter() { + assert!( + point.data.tangent_impulse.index(0).is_finite(), + "tangent impulse from a contact pair point data should be normal." + ); + } + } + } + } +} diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 85cd9c0c5..9adcd5453 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -531,86 +531,3 @@ where [tangent1, bitangent1] } - -#[cfg(feature = "dim2")] -#[cfg(test)] -mod test { - use crate::prelude::{ - BroadPhaseMultiSap, CCDSolver, ColliderBuilder, ColliderSet, ImpulseJointSet, - IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, - RigidBodyBuilder, RigidBodySet, - }; - use na::{vector, Vector}; - - #[test] - fn tangent_not_nan_818() { - /* - * World - */ - - let mut bodies = RigidBodySet::new(); - 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(); - - /* - * The ground - */ - let ground_size = 20.0; - let ground_height = 1.0; - - let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -3.0]); - let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height) - .rotation((0.25 as crate::math::Real).to_radians()) - .friction(0.9); - colliders.insert_with_parent(collider, handle, &mut bodies); - - /* - * A tilted capsule that cannot rotate. - */ - let rigid_body = RigidBodyBuilder::dynamic() - .translation(vector![0.0, 4.0]) - .lock_rotations(); - let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(2.0, 1.0).friction(0.9); - colliders.insert_with_parent(collider, handle, &mut bodies); - - // Steps - let gravity = Vector::y() * -9.81; - - for _ in 0..50 { - pipeline.step( - &gravity, - &IntegrationParameters::default(), - &mut islands, - &mut bf, - &mut nf, - &mut bodies, - &mut colliders, - &mut impulse_joints, - &mut multibody_joints, - &mut CCDSolver::new(), - None, - &(), - &(), - ); - } - let pairs: Vec<_> = nf.contact_pairs().collect(); - println!("contact pairs: {:?}", pairs); - for contact in pairs { - for manifold in contact.manifolds.iter() { - for point in manifold.points.iter() { - assert!( - point.data.tangent_impulse.index(0).is_finite(), - "tangent impulse from a contact pair point data should be normal." - ); - } - } - } - } -}