test for wrong self intersection after Collider::set_parent

src/geometry/narrow_phase.rs

@@ -1164,7 +1164,7 @@ impl NarrowPhase {
 
 #[cfg(test)]
 #[cfg(feature = "f32")]
-//#[cfg(feature = "dim3")]
+#[cfg(feature = "dim3")]
 mod test {
     use na::vector;
 
@@ -1177,7 +1177,7 @@ mod test {
 
     /// Test for https://github.com/dimforge/rapier/issues/734.
     #[test]
-    pub fn reparent() {
+    pub fn collider_set_parent_depenetration() {
         let mut rigid_body_set = RigidBodySet::new();
         let mut collider_set = ColliderSet::new();
 
@@ -1186,25 +1186,21 @@ mod test {
 
         /* Create body 1, which will contain both colliders at first. */
         let rigid_body_1 = RigidBodyBuilder::dynamic()
-            .translation(vector![0.0, 10.0, 0.0])
+            .translation(vector![0.0, 0.0, 0.0])
             .build();
         let body_1_handle = rigid_body_set.insert(rigid_body_1);
 
         /* Create collider 1. Parent it to rigid body 1. */
         let collider_1_handle =
             collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set);
 
-        /* Create collider 2. Parent it to rigid body 1. */
-        //let collider_2_handle =
-        //    collider_set.insert_with_parent(collider.build(), body_2_handle, &mut rigid_body_set);
-
         /* Create collider 2. Parent it to rigid body 1. */
         let collider_2_handle =
             collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set);
 
         /* Create body 2. No attached colliders yet. */
         let rigid_body_2 = RigidBodyBuilder::dynamic()
-            .translation(vector![0.0, 10.0, 0.0])
+            .translation(vector![0.0, 0.0, 0.0])
             .build();
         let body_2_handle = rigid_body_set.insert(rigid_body_2);
 
@@ -1245,6 +1241,7 @@ mod test {
                 < 0.5f32
         );
 
+        /* Parent collider 2 to body 2. */
         collider_set.set_parent(collider_2_handle, Some(body_2_handle), &mut rigid_body_set);
 
         /* Run the game loop, stepping the simulation once per frame. */
@@ -1276,7 +1273,128 @@ mod test {
         assert!(
             (collider_1_position.translation.vector - collider_2_position.translation.vector)
                 .magnitude()
-                >= 0.5f32
+                >= 0.5f32,
+            "colliders should no longer be penetrating."
+        );
+    }
+
+    /// Test for https://github.com/dimforge/rapier/issues/734.
+    #[test]
+    pub fn collider_set_parent_no_self_intersection() {
+        let mut rigid_body_set = RigidBodySet::new();
+        let mut collider_set = ColliderSet::new();
+
+        /* Create the ground. */
+        let collider = ColliderBuilder::ball(0.5);
+
+        /* Create body 1, which will contain collider 1. */
+        let rigid_body_1 = RigidBodyBuilder::dynamic()
+            .translation(vector![0.0, 0.0, 0.0])
+            .build();
+        let body_1_handle = rigid_body_set.insert(rigid_body_1);
+
+        /* Create collider 1. Parent it to rigid body 1. */
+        let collider_1_handle =
+            collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set);
+
+        /* Create body 2, which will contain collider 2 at first. */
+        let rigid_body_2 = RigidBodyBuilder::dynamic()
+            .translation(vector![0.0, 0.0, 0.0])
+            .build();
+        let body_2_handle = rigid_body_set.insert(rigid_body_2);
+
+        /* Create collider 2. Parent it to rigid body 1. */
+        //let collider_2_handle =
+        //    collider_set.insert_with_parent(collider.build(), body_2_handle, &mut rigid_body_set);
+
+        /* Create collider 2. Parent it to rigid body 1. */
+        let collider_2_handle =
+            collider_set.insert_with_parent(collider.build(), body_2_handle, &mut rigid_body_set);
+
+        /* Create other structures necessary for the simulation. */
+        let gravity = vector![0.0, 0.0, 0.0];
+        let integration_parameters = IntegrationParameters::default();
+        let mut physics_pipeline = PhysicsPipeline::new();
+        let mut island_manager = IslandManager::new();
+        let mut broad_phase = DefaultBroadPhase::new();
+        let mut narrow_phase = NarrowPhase::new();
+        let mut impulse_joint_set = ImpulseJointSet::new();
+        let mut multibody_joint_set = MultibodyJointSet::new();
+        let mut ccd_solver = CCDSolver::new();
+        let mut query_pipeline = QueryPipeline::new();
+        let physics_hooks = ();
+        let event_handler = ();
+
+        physics_pipeline.step(
+            &gravity,
+            &integration_parameters,
+            &mut island_manager,
+            &mut broad_phase,
+            &mut narrow_phase,
+            &mut rigid_body_set,
+            &mut collider_set,
+            &mut impulse_joint_set,
+            &mut multibody_joint_set,
+            &mut ccd_solver,
+            Some(&mut query_pipeline),
+            &physics_hooks,
+            &event_handler,
+        );
+        let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos;
+        let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos;
+        assert!(
+            (collider_1_position.translation.vector - collider_2_position.translation.vector)
+                .magnitude()
+                < 0.5f32
+        );
+
+        /* Parent collider 2 to body 1. */
+        collider_set.set_parent(collider_2_handle, Some(body_1_handle), &mut rigid_body_set);
+
+        /* Run the game loop, stepping the simulation once per frame. */
+        for _ in 0..200 {
+            physics_pipeline.step(
+                &gravity,
+                &integration_parameters,
+                &mut island_manager,
+                &mut broad_phase,
+                &mut narrow_phase,
+                &mut rigid_body_set,
+                &mut collider_set,
+                &mut impulse_joint_set,
+                &mut multibody_joint_set,
+                &mut ccd_solver,
+                Some(&mut query_pipeline),
+                &physics_hooks,
+                &event_handler,
+            );
+
+            let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos;
+            let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos;
+            println!("collider 1 position: {}", collider_1_position.translation);
+            println!("collider 2 position: {}", collider_2_position.translation);
+        }
+
+        let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos;
+        let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos;
+        assert!(
+            (collider_1_position.translation.vector - collider_2_position.translation.vector)
+                .magnitude()
+                < 0.5f32,
+            "colliders should be penetrating."
+        );
+        assert!(
+            rigid_body_set
+                .get(body_1_handle)
+                .unwrap()
+                .position()
+                .translation
+                .vector
+                .magnitude()
+                // TODO: this is probably a way too big value to test, consider lowering it.
+                // In the meantime, this proves that current behaviour is incorrect.
+                < 30000f32,
+            "Body 1 should not have gone too far from origin."
         );
     }
 }