Fix tangent impulse being NaN

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,
+                &(),
+                &(),
+            );
+        }
+    }
+}

src/geometry/contact_pair.rs

@@ -130,6 +130,17 @@ pub struct ContactPair {
     pub(crate) workspace: Option<ContactManifoldsWorkspace>,
 }
 
+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 {