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/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/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 {