Make kinematic bodies properly wake up dynamic bodies.

examples3d/platform3.rs

@@ -64,7 +64,13 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Setup a callback to control the platform.
      */
+    let mut count = 0;
     testbed.add_callback(move |_, physics, _, _, time| {
+        count += 1;
+        if count % 100 > 50 {
+            return;
+        }
+
         if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
             let mut next_pos = platform.position;
 

src/dynamics/joint/joint_set.rs

@@ -202,8 +202,8 @@ impl JointSet {
                 }
 
                 // Wake up the attached bodies.
-                bodies.wake_up(h1);
+                bodies.wake_up(h1, true);
-                bodies.wake_up(h2);
+                bodies.wake_up(h2, true);
             }
 
             if let Some(other) = self.joint_graph.remove_node(deleted_id) {

src/dynamics/rigid_body.rs

@@ -181,10 +181,13 @@ impl RigidBody {
     }
 
     /// Wakes up this rigid body if it is sleeping.
-    pub fn wake_up(&mut self) {
+    ///
+    /// If `strong` is `true` then it is assured that the rigid-body will
+    /// remain awake during multiple subsequent timesteps.
+    pub fn wake_up(&mut self, strong: bool) {
         self.activation.sleeping = false;
 
-        if self.activation.energy == 0.0 && self.is_dynamic() {
+        if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
             self.activation.energy = self.activation.threshold.abs() * 2.0;
         }
     }
@@ -198,9 +201,18 @@ impl RigidBody {
 
     /// Is this rigid body sleeping?
     pub fn is_sleeping(&self) -> bool {
+        // TODO: should we:
+        // - return false for static bodies.
+        // - return true for non-sleeping dynamic bodies.
+        // - return true only for kinematic bodies with non-zero velocity?
         self.activation.sleeping
     }
 
+    /// Is the velocity of this body not zero?
+    pub fn is_moving(&self) -> bool {
+        !self.linvel.is_zero() || !self.angvel.is_zero()
+    }
+
     fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
         let com = &self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);

src/dynamics/rigid_body_set.rs

@@ -3,8 +3,9 @@ use rayon::prelude::*;
 
 use crate::data::arena::Arena;
 use crate::dynamics::{BodyStatus, Joint, RigidBody};
-use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
+use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
 use crossbeam::channel::{Receiver, Sender};
+use num::Zero;
 use std::ops::{Deref, DerefMut, Index, IndexMut};
 
 /// A mutable reference to a rigid-body.
@@ -197,11 +198,14 @@ impl RigidBodySet {
     }
 
     /// Forces the specified rigid-body to wake up if it is dynamic.
-    pub fn wake_up(&mut self, handle: RigidBodyHandle) {
+    ///
+    /// If `strong` is `true` then it is assured that the rigid-body will
+    /// remain awake during multiple subsequent timesteps.
+    pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
         if let Some(rb) = self.bodies.get_mut(handle) {
             // TODO: what about kinematic bodies?
             if rb.is_dynamic() {
-                rb.wake_up();
+                rb.wake_up(strong);
 
                 if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
                     rb.active_set_id = self.active_dynamic_set.len();
@@ -447,6 +451,45 @@ impl RigidBodySet {
             }
         }
 
+        // Read all the contacts and push objects touching touching this rigid-body.
+        #[inline(always)]
+        fn push_contacting_colliders(
+            rb: &RigidBody,
+            colliders: &ColliderSet,
+            contact_graph: &InteractionGraph<ContactPair>,
+            stack: &mut Vec<ColliderHandle>,
+        ) {
+            for collider_handle in &rb.colliders {
+                let collider = &colliders[*collider_handle];
+
+                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
+                    for manifold in &inter.2.manifolds {
+                        if manifold.num_active_contacts() > 0 {
+                            let other =
+                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
+                            let other_body = colliders[other].parent;
+                            stack.push(other_body);
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        // Now iterate on all active kinematic bodies and push all the bodies
+        // touching them to the stack so they can be woken up.
+        for h in self.active_kinematic_set.iter() {
+            let rb = &self.bodies[*h];
+
+            if !rb.is_moving() {
+                // If the kinematic body does not move, it does not have
+                // to wake up any dynamic body.
+                continue;
+            }
+
+            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
+        }
+
         //        println!("Selection: {}", instant::now() - t);
 
         //        let t = instant::now();
@@ -465,7 +508,9 @@ impl RigidBodySet {
                 // We already visited this body and its neighbors.
                 // Also, we don't propagate awake state through static bodies.
                 continue;
-            } else if self.stack.len() < island_marker {
+            }
+
+            if self.stack.len() < island_marker {
                 if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
                     >= min_island_size
                 {
@@ -476,29 +521,16 @@ impl RigidBodySet {
                 island_marker = self.stack.len();
             }
 
-            rb.wake_up();
+            rb.wake_up(false);
             rb.active_island_id = self.active_islands.len() - 1;
             rb.active_set_id = self.active_dynamic_set.len();
             rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
             rb.active_set_timestamp = self.active_set_timestamp;
             self.active_dynamic_set.push(handle);
 
-            // Read all the contacts and push objects touching this one.
+            // Transmit the active state to all the rigid-bodies with colliders
-            for collider_handle in &rb.colliders {
+            // in contact or joined with this collider.
-                let collider = &colliders[*collider_handle];
+            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
-
-                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
-                    for manifold in &inter.2.manifolds {
-                        if manifold.num_active_contacts() > 0 {
-                            let other =
-                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
-                            let other_body = colliders[other].parent;
-                            self.stack.push(other_body);
-                            break;
-                        }
-                    }
-                }
-            }
 
             for inter in joint_graph.interactions_with(rb.joint_graph_index) {
                 let other = crate::utils::other_handle((inter.0, inter.1), handle);

src/geometry/narrow_phase.rs

@@ -87,11 +87,11 @@ impl NarrowPhase {
                 // Wake up every body in contact with the deleted collider.
                 for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
                     if let Some(parent) = colliders.get(a).map(|c| c.parent) {
-                        bodies.wake_up(parent)
+                        bodies.wake_up(parent, true)
                     }
 
                     if let Some(parent) = colliders.get(b).map(|c| c.parent) {
-                        bodies.wake_up(parent)
+                        bodies.wake_up(parent, true)
                     }
                 }
 
@@ -119,6 +119,7 @@ impl NarrowPhase {
     pub(crate) fn register_pairs(
         &mut self,
         colliders: &mut ColliderSet,
+        bodies: &mut RigidBodySet,
         broad_phase_events: &[BroadPhasePairEvent],
         events: &dyn EventHandler,
     ) {
@@ -218,9 +219,13 @@ impl NarrowPhase {
                                 .contact_graph
                                 .remove_edge(co1.contact_graph_index, co2.contact_graph_index);
 
-                            // Emit a contact stopped event if we had a proximity before removing the edge.
+                            // Emit a contact stopped event if we had a contact before removing the edge.
+                            // Also wake up the dynamic bodies that were in contact.
                             if let Some(ctct) = contact_pair {
                                 if ctct.has_any_active_contact() {
+                                    bodies.wake_up(co1.parent, true);
+                                    bodies.wake_up(co2.parent, true);
+
                                     events.handle_contact_event(ContactEvent::Stopped(
                                         pair.collider1,
                                         pair.collider2,
@@ -250,8 +255,7 @@ impl NarrowPhase {
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) {
-            {
                 // No need to update this contact because nothing moved.
                 return;
             }
@@ -359,7 +363,8 @@ impl NarrowPhase {
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()))
+                || (!rb1.is_dynamic() && !rb2.is_dynamic())
             {
                 // No need to update this contact because nothing moved.
                 return;

src/pipeline/collision_pipeline.rs

@@ -50,7 +50,7 @@ impl CollisionPipeline {
         self.broad_phase_events.clear();
         broad_phase.find_pairs(&mut self.broad_phase_events);
 
-        narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
 
         narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events);
         narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events);

src/pipeline/physics_pipeline.rs

@@ -75,6 +75,15 @@ impl PhysicsPipeline {
         self.counters.step_started();
         bodies.maintain_active_set();
 
+        // Update kinematic bodies velocities.
+        // TODO: what is the best place for this? It should at least be
+        // located before the island computation because we test the velocity
+        // there to determine if this kinematic body should wake-up dynamic
+        // bodies it is touching.
+        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
+            body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
+        });
+
         self.counters.stages.collision_detection_time.start();
         self.counters.cd.broad_phase_time.start();
         self.broadphase_collider_pairs.clear();
@@ -91,7 +100,7 @@ impl PhysicsPipeline {
         broad_phase.find_pairs(&mut self.broad_phase_events);
         //        println!("Find pairs time: {}", instant::now() - t);
 
-        narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
         self.counters.cd.broad_phase_time.pause();
 
         //        println!("Num contact pairs: {}", pairs.len());
@@ -122,11 +131,6 @@ impl PhysicsPipeline {
         );
         self.counters.stages.island_construction_time.pause();
 
-        // Update kinematic bodies velocities.
-        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
-        });
-
         if self.manifold_indices.len() < bodies.num_islands() {
             self.manifold_indices
                 .resize(bodies.num_islands(), Vec::new());
@@ -261,7 +265,7 @@ impl PhysicsPipeline {
 
         if let Some(parent) = bodies.get_mut_internal(collider.parent) {
             parent.remove_collider_internal(handle, &collider);
-            bodies.wake_up(collider.parent);
+            bodies.wake_up(collider.parent, true);
         }
 
         Some(collider)
@@ -303,6 +307,37 @@ mod test {
     use crate::math::Vector;
     use crate::pipeline::PhysicsPipeline;
 
+    #[test]
+    fn kinematic_and_static_contact_crash() {
+        let mut colliders = ColliderSet::new();
+        let mut joints = JointSet::new();
+        let mut pipeline = PhysicsPipeline::new();
+        let mut bf = BroadPhase::new();
+        let mut nf = NarrowPhase::new();
+        let mut bodies = RigidBodySet::new();
+
+        let rb = RigidBodyBuilder::new_static().build();
+        let h1 = bodies.insert(rb.clone());
+        let co = ColliderBuilder::ball(10.0).build();
+        colliders.insert(co.clone(), h1, &mut bodies);
+
+        // The same but with a kinematic body.
+        let rb = RigidBodyBuilder::new_kinematic().build();
+        let h2 = bodies.insert(rb.clone());
+        colliders.insert(co, h2, &mut bodies);
+
+        pipeline.step(
+            &Vector::zeros(),
+            &IntegrationParameters::default(),
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut joints,
+            &(),
+        );
+    }
+
     #[test]
     fn rigid_body_removal_before_step() {
         let mut colliders = ColliderSet::new();

src_testbed/engine.rs

@@ -609,7 +609,7 @@ impl GraphicsManager {
         }
     }
 
-    pub fn draw(&mut self, colliders: &ColliderSet, window: &mut Window) {
+    pub fn draw(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) {
         // use kiss3d::camera::Camera;
         // println!(
         //     "camera eye {:?}, at: {:?}",
@@ -618,6 +618,20 @@ impl GraphicsManager {
         // );
         for (_, ns) in self.b2sn.iter_mut() {
             for n in ns.iter_mut() {
+                /*
+                if let Some(co) = colliders.get(n.collider()) {
+                    let bo = &bodies[co.parent()];
+
+                    if bo.is_dynamic() {
+                        if bo.is_sleeping() {
+                            n.set_color(Point3::new(1.0, 0.0, 0.0));
+                        } else {
+                            n.set_color(Point3::new(0.0, 1.0, 0.0));
+                        }
+                    }
+                }
+                 */
+
                 n.update(colliders);
                 n.draw(window);
             }

src_testbed/testbed.rs

@@ -824,7 +824,7 @@ impl Testbed {
                     .physics
                     .bodies
                     .iter()
-                    .filter(|e| e.1.is_dynamic())
+                    .filter(|e| !e.1.is_static())
                     .map(|e| e.0)
                     .collect();
                 let num_to_delete = (dynamic_bodies.len() / 10).max(1);
@@ -1367,7 +1367,7 @@ impl State for Testbed {
                     }
                 } else {
                     for (_, mut body) in self.physics.bodies.iter_mut() {
-                        body.wake_up();
+                        body.wake_up(true);
                         body.activation.threshold = -1.0;
                     }
                 }
@@ -1571,7 +1571,8 @@ impl State for Testbed {
         }
 
         self.highlight_hovered_body(window);
-        self.graphics.draw(&self.physics.colliders, window);
+        self.graphics
+            .draw(&self.physics.bodies, &self.physics.colliders, window);
 
         #[cfg(feature = "fluids")]
         {
@@ -1648,26 +1649,35 @@ Fluids: {:.2}ms
         }
 
         if self.state.flags.contains(TestbedStateFlags::DEBUG) {
-            let hash_bf = md5::compute(&bincode::serialize(&self.physics.broad_phase).unwrap());
+            let bf = bincode::serialize(&self.physics.broad_phase).unwrap();
-            let hash_nf = md5::compute(&bincode::serialize(&self.physics.narrow_phase).unwrap());
+            let nf = bincode::serialize(&self.physics.narrow_phase).unwrap();
-            let hash_bodies = md5::compute(&bincode::serialize(&self.physics.bodies).unwrap());
+            let bs = bincode::serialize(&self.physics.bodies).unwrap();
-            let hash_colliders =
+            let cs = bincode::serialize(&self.physics.colliders).unwrap();
-                md5::compute(&bincode::serialize(&self.physics.colliders).unwrap());
+            let js = bincode::serialize(&self.physics.joints).unwrap();
-            let hash_joints = md5::compute(&bincode::serialize(&self.physics.joints).unwrap());
+            let hash_bf = md5::compute(&bf);
+            let hash_nf = md5::compute(&nf);
+            let hash_bodies = md5::compute(&bs);
+            let hash_colliders = md5::compute(&cs);
+            let hash_joints = md5::compute(&js);
             profile = format!(
                 r#"{}
 Hashes at frame: {}
-|_ Broad phase: {:?}
+|_ Broad phase [{:.1}KB]: {:?}
-|_ Narrow phase: {:?}
+|_ Narrow phase [{:.1}KB]: {:?}
-|_ Bodies: {:?}
+|_ Bodies [{:.1}KB]: {:?}
-|_ Colliders: {:?}
+|_ Colliders [{:.1}KB]: {:?}
-|_ Joints: {:?}"#,
+|_ Joints [{:.1}KB]: {:?}"#,
                 profile,
                 self.state.timestep_id,
+                bf.len() as f32 / 1000.0,
                 hash_bf,
+                nf.len() as f32 / 1000.0,
                 hash_nf,
+                bs.len() as f32 / 1000.0,
                 hash_bodies,
+                cs.len() as f32 / 1000.0,
                 hash_colliders,
+                js.len() as f32 / 1000.0,
                 hash_joints
             );
         }