Experiment with incremental island computation.

4 files changed, 74 insertions, 40 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 866c3a5..207cc1c 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -1,6 +1,6 @@
 //! Physics pipeline structures.
 
-use crate::dynamics::{JointSet, RigidBodySet};
+use crate::dynamics::{IslandSet, JointSet, RigidBodySet};
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter,
     IntersectionPairFilter, NarrowPhase,
@@ -42,13 +42,14 @@ impl CollisionPipeline {
         prediction_distance: Real,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
+        islands: &mut IslandSet,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         contact_pair_filter: Option<&dyn ContactPairFilter>,
         proximity_pair_filter: Option<&dyn IntersectionPairFilter>,
         events: &dyn EventHandler,
     ) {
-        bodies.maintain(colliders);
+        bodies.maintain(islands, colliders);
         self.broadphase_collider_pairs.clear();
 
         broad_phase.update_aabbs(prediction_distance, bodies, colliders);
@@ -56,7 +57,7 @@ impl CollisionPipeline {
         self.broad_phase_events.clear();
         broad_phase.find_pairs(&mut self.broad_phase_events);
 
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events);
 
         narrow_phase.compute_contacts(
             prediction_distance,
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index 287de9d..f47c1f5 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -1,11 +1,11 @@
 //! Structure for combining the various physics components to perform an actual simulation.
 
-pub use collision_pipeline::CollisionPipeline;
+// pub use collision_pipeline::CollisionPipeline;
 pub use event_handler::{ChannelEventCollector, EventHandler};
 pub use physics_pipeline::PhysicsPipeline;
 pub use query_pipeline::QueryPipeline;
 
-mod collision_pipeline;
+// mod collision_pipeline;
 mod event_handler;
 mod physics_pipeline;
 mod query_pipeline;
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index b5f123d..70474a7 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -3,7 +3,7 @@
 use crate::counters::Counters;
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, IslandSet, JointSet, RigidBodyHandle, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
@@ -31,6 +31,7 @@ pub struct PhysicsPipeline {
     joint_constraint_indices: Vec<Vec<ContactManifoldIndex>>,
     broadphase_collider_pairs: Vec<ColliderPair>,
     broad_phase_events: Vec<BroadPhasePairEvent>,
+    bodies_with_changed_sleep_state: Vec<RigidBodyHandle>,
     solvers: Vec<IslandSolver>,
 }
 
@@ -56,6 +57,7 @@ impl PhysicsPipeline {
             joint_constraint_indices: Vec::new(),
             broadphase_collider_pairs: Vec::new(),
             broad_phase_events: Vec::new(),
+            bodies_with_changed_sleep_state: Vec::new(),
         }
     }
 
@@ -66,6 +68,7 @@ impl PhysicsPipeline {
         integration_parameters: &IntegrationParameters,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
+        islands: &mut IslandSet,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
@@ -74,7 +77,7 @@ impl PhysicsPipeline {
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
-        bodies.maintain(colliders);
+        bodies.maintain(islands, colliders);
         broad_phase.maintain(colliders);
         narrow_phase.maintain(colliders, bodies);
 
@@ -93,6 +96,7 @@ impl PhysicsPipeline {
         //        let t = instant::now();
         broad_phase.update_aabbs(
             integration_parameters.prediction_distance,
+            islands,
             bodies,
             colliders,
         );
@@ -103,7 +107,7 @@ impl PhysicsPipeline {
         broad_phase.find_pairs(&mut self.broad_phase_events);
         //        println!("Find pairs time: {}", instant::now() - t);
 
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events);
         self.counters.cd.broad_phase_time.pause();
 
         //        println!("Num contact pairs: {}", pairs.len());
@@ -113,67 +117,78 @@ impl PhysicsPipeline {
         //        let t = instant::now();
         narrow_phase.compute_contacts(
             integration_parameters.prediction_distance,
+            islands,
             bodies,
             colliders,
             contact_pair_filter,
             events,
         );
-        narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
+        narrow_phase.compute_intersections(
+            islands,
+            bodies,
+            colliders,
+            proximity_pair_filter,
+            events,
+        );
         //        println!("Compute contact time: {}", instant::now() - t);
 
         self.counters.stages.island_construction_time.start();
-        bodies.update_active_set_with_contacts(
-            colliders,
-            narrow_phase,
-            joints.joint_graph(),
-            integration_parameters.min_island_size,
-        );
+        // bodies.update_active_set_with_contacts(
+        //     colliders,
+        //     narrow_phase,
+        //     joints.joint_graph(),
+        //     integration_parameters.min_island_size,
+        // );
         self.counters.stages.island_construction_time.pause();
 
-        if self.manifold_indices.len() < bodies.num_islands() {
+        if self.manifold_indices.len() < islands.num_active_islands() {
             self.manifold_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_active_islands(), Vec::new());
         }
 
-        if self.joint_constraint_indices.len() < bodies.num_islands() {
+        if self.joint_constraint_indices.len() < islands.num_active_islands() {
             self.joint_constraint_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_active_islands(), Vec::new());
         }
 
         let mut manifolds = Vec::new();
         narrow_phase.sort_and_select_active_contacts(
+            islands,
             bodies,
             &mut manifolds,
             &mut self.manifold_indices,
         );
-        joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
+        joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices);
 
         self.counters.cd.narrow_phase_time.pause();
         self.counters.stages.collision_detection_time.pause();
 
         self.counters.stages.update_time.start();
-        bodies.foreach_active_dynamic_body_mut_internal(|_, b| {
-            b.update_world_mass_properties();
-            b.integrate_accelerations(integration_parameters.dt, *gravity)
-        });
+        for handle in islands.active_bodies() {
+            if let Some(rb) = bodies.get_mut(handle) {
+                rb.update_world_mass_properties();
+                rb.integrate_accelerations(integration_parameters.dt, *gravity)
+            }
+        }
         self.counters.stages.update_time.pause();
 
         self.counters.solver.reset();
         self.counters.stages.solver_time.start();
-        if self.solvers.len() < bodies.num_islands() {
+        if self.solvers.len() < islands.num_active_islands() {
             self.solvers
-                .resize_with(bodies.num_islands(), || IslandSolver::new());
+                .resize_with(islands.num_active_islands(), || IslandSolver::new());
         }
 
         #[cfg(not(feature = "parallel"))]
         {
             enable_flush_to_zero!();
 
-            for island_id in 0..bodies.num_islands() {
+            for island_id in 0..islands.num_active_islands() {
                 self.solvers[island_id].solve_island(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
+                    islands,
                     bodies,
                     &mut manifolds[..],
                     &self.manifold_indices[island_id],
@@ -189,7 +204,7 @@ impl PhysicsPipeline {
             use rayon::prelude::*;
             use std::sync::atomic::Ordering;
 
-            let num_islands = bodies.num_islands();
+            let num_islands = islands.num_active_islands();
             let solvers = &mut self.solvers[..num_islands];
             let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
             let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _);
@@ -227,15 +242,29 @@ impl PhysicsPipeline {
 
         // Update colliders positions and kinematic bodies positions.
         // FIXME: do this in the solver?
-        bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
-                rb.linvel = na::zero();
-                rb.angvel = na::zero();
-            } else {
+        for handle in islands.active_bodies() {
+            if let Some(rb) = bodies.get_mut(handle) {
                 rb.update_predicted_position(integration_parameters.dt);
+                rb.update_colliders_positions(colliders);
+
+                // TODO: do this in the solver?
+                let prev_sleep_state = rb.can_sleep();
+                rb.update_energy();
+
+                if prev_sleep_state != rb.can_sleep() {
+                    self.bodies_with_changed_sleep_state.push(handle);
+                }
             }
+        }
+
+        for handle in self.bodies_with_changed_sleep_state.drain(..) {
+            islands.body_sleep_state_changed(&bodies[handle]);
+        }
 
+        bodies.foreach_active_kinematic_body_mut_internal(|_, rb| {
+            rb.position = rb.predicted_position;
+            rb.linvel = na::zero();
+            rb.angvel = na::zero();
             rb.update_colliders_positions(colliders);
         });
 
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 8cc6a60..6b33e49 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::RigidBodySet;
+use crate::dynamics::{IslandSet, RigidBodySet};
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
     RayIntersection, SimdQuadTree,
@@ -109,7 +109,7 @@ impl QueryPipeline {
     }
 
     /// Update the acceleration structure on the query pipeline.
-    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+    pub fn update(&mut self, islands: &IslandSet, bodies: &RigidBodySet, colliders: &ColliderSet) {
         if !self.tree_built {
             let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
             self.quadtree.clear_and_rebuild(data, self.dilation_factor);
@@ -118,10 +118,14 @@ impl QueryPipeline {
             return;
         }
 
-        for (_, body) in bodies
-            .iter_active_dynamic()
-            .chain(bodies.iter_active_kinematic())
-        {
+        for handle in islands.active_bodies() {
+            if let Some(body) = bodies.get(handle) {
+                for handle in &body.colliders {
+                    self.quadtree.pre_update(*handle)
+                }
+            }
+        }
+        for (_, body) in bodies.iter_active_kinematic() {
             for handle in &body.colliders {
                 self.quadtree.pre_update(*handle)
             }