diff options
Diffstat (limited to 'src/dynamics/island_set.rs')
| -rw-r--r-- | src/dynamics/island_set.rs | 383 |
1 files changed, 231 insertions, 152 deletions
diff --git a/src/dynamics/island_set.rs b/src/dynamics/island_set.rs index a0da207..71dc9eb 100644 --- a/src/dynamics/island_set.rs +++ b/src/dynamics/island_set.rs @@ -1,114 +1,69 @@ +use crate::data::Coarena; use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; -use crate::geometry::NarrowPhase; +use crate::geometry::{ColliderSet, NarrowPhase}; use crate::utils; -use downcast_rs::__std::collections::VecDeque; +use std::collections::VecDeque; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub struct Island { bodies: Vec<RigidBodyHandle>, - - // Number of bodies that are awake on this island. - // If this is equal to zero, ten the whole island is asleep. - wake_up_count: usize, - // Index in the island_set.active_islands vector. - active_island_id: usize, - dirty: bool, } impl Island { pub fn new() -> Self { - Self { - bodies: vec![], - wake_up_count: 0, - active_island_id: crate::INVALID_USIZE, - dirty: false, - } + Self { bodies: vec![] } } pub fn len(&self) -> usize { self.bodies.len() } - pub fn is_sleeping(&self) -> bool { - self.wake_up_count == 0 - } - - pub fn active_island_id(&self) -> usize { - self.active_island_id - } - pub fn bodies(&self) -> &[RigidBodyHandle] { - &self.bodies[..] + &self.bodies } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub struct IslandSet { - active_islands: Vec<usize>, + // We can't store this in the rigid-bodies because that would + // cause borrowing issues during the traversal. + traversal_body_timestamps: Coarena<u64>, islands: Vec<Island>, - to_update: VecDeque<usize>, + active_island: usize, + islands_to_extract: VecDeque<RigidBodyHandle>, + islands_extraction_queue: Vec<RigidBodyHandle>, + // NOTE: this value must never be 0, otherwise some + // invariants used during the traversal can break. + traversal_timestamp: u64, } impl IslandSet { pub fn new() -> Self { IslandSet { - active_islands: vec![], - islands: vec![], - to_update: VecDeque::new(), + traversal_body_timestamps: Coarena::new(), + islands: vec![Island::new()], + active_island: 0, + islands_to_extract: VecDeque::new(), + islands_extraction_queue: vec![], + traversal_timestamp: 1, } } - pub fn islands(&self) -> &[Island] { - &self.islands[..] - } - pub fn num_active_islands(&self) -> usize { - self.active_islands.len() - } - - pub fn active_island(&self, i: usize) -> &Island { - &self.islands[self.active_islands[i]] - } - - pub fn active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a { - let islands = &self.islands; - self.active_islands - .iter() - .copied() - .flat_map(move |i| islands[i].bodies.iter()) - .copied() + 1 } - pub fn contact_stopped( - &mut self, - bodies: &RigidBodySet, - h1: RigidBodyHandle, - _h2: RigidBodyHandle, - ) { - // // NOTE: we don't actually need h2 because they are both on the same island anyway. - // // Yet we keep the `h2` argument to show that this function properly take into account - // // both bodies. - // if let Some(island_id) = bodies.get(h1).map(|b| b.island_id) { - // if let Some(island) = self.islands.get_mut(island_id) { - // if !island.dirty { - // island.dirty = true; - // self.to_update.push_back(island_id); - // } - // } - // } + pub fn is_island_sleeping(&self, island_id: usize) -> bool { + island_id != self.active_island } - pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) { - // if let Some(island) = self.to_update.pop_front() { - // // Next island to update. - // } + pub fn active_bodies(&self) -> &[RigidBodyHandle] { + &self.islands[self.active_island].bodies } - pub fn is_island_sleeping(&self, island_id: usize) -> bool { - self.islands[island_id].is_sleeping() - } + pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {} pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) { assert_eq!(body.island_id, crate::INVALID_USIZE); @@ -117,85 +72,239 @@ impl IslandSet { return; } - let new_island_id = handle.into_raw_parts().0; + self.traversal_body_timestamps + .ensure_element_exists(handle.0, 0); - if self.islands.len() <= new_island_id { - self.islands.resize(new_island_id + 1, Island::new()); + if body.can_sleep() { + body.island_id = self.islands.len(); + body.island_offset = 0; + self.islands.push(Island { + bodies: vec![handle], + }) + } else { + let mut active_island = &mut self.islands[self.active_island]; + body.island_id = self.active_island; + body.island_offset = active_island.bodies.len(); + active_island.bodies.push(handle); } + } - body.island_id = new_island_id; - body.island_offset = self.islands[new_island_id].bodies.len(); - self.islands[new_island_id].bodies.push(handle); + pub fn contact_started( + &mut self, + bodies: &mut RigidBodySet, + mut h1: RigidBodyHandle, + mut h2: RigidBodyHandle, + ) { + let island1 = bodies[h1].island_id; + let island2 = bodies[h2].island_id; - // NOTE: `body_sleep_state_changed` must be called afterwards. + self.wake_up_island(bodies, island1); + self.wake_up_island(bodies, island2); } - pub fn body_sleep_state_changed(&mut self, body: &RigidBody) { - // Non-dynamic bodies never take part in the island computation - // since they don't transmit any forces. - if !body.is_dynamic() { - return; + pub fn body_sleep_state_changed(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle) { + let body = &mut bodies[handle]; + let island_id = body.island_id; + + if island_id == crate::INVALID_USIZE { + self.add_rigid_body(handle, body); + } else if island_id == self.active_island && body.can_sleep() { + // The body is sleeping now. + self.islands_to_extract.push_back(handle); + } else if island_id != self.active_island && !body.can_sleep() { + // The body is awake now. + self.wake_up_island(bodies, island_id); } + } - let island = &mut self.islands[body.island_id]; - - if body.can_sleep() { - island.wake_up_count -= 1; + #[inline(always)] + fn push_contacting_bodies( + handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + queue: &mut Vec<RigidBodyHandle>, + traversal_body_timestamps: &mut Coarena<u64>, + first_traversal_timestamp: u64, + traversal_timestamp: u64, + ) -> bool { + for collider_handle in &bodies[handle].colliders { + if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { + for inter in contacts { + let other = crate::utils::select_other((inter.0, inter.1), *collider_handle); + let other_body_handle = colliders[other].parent; + let other_body = &bodies[other_body_handle]; + + if other_body.is_dynamic() { + if !other_body.can_sleep() { + return false; + } + + let other_body_timestamp = + &mut traversal_body_timestamps[other_body_handle.0]; + + if *other_body_timestamp >= first_traversal_timestamp + && *other_body_timestamp < traversal_timestamp + { + // We already saw this body during another traversal this frame. + // So we don't need to continue any further. + return false; + } + + if *other_body_timestamp != traversal_timestamp { + *other_body_timestamp = traversal_timestamp; + queue.push(other_body_handle); + } + } + } + } + } - if island.wake_up_count == 0 { - // The island is sleeping now. - // Remove it from the active set. - let active_island_id_to_remove = island.active_island_id; - island.active_island_id = crate::INVALID_USIZE; - self.active_islands.swap_remove(active_island_id_to_remove); + true + } - if let Some(moved_island) = self.active_islands.get(active_island_id_to_remove) { - self.islands[*moved_island].active_island_id = active_island_id_to_remove; + pub fn update_sleeping_islands( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + narrow_phase: &NarrowPhase, + ) { + let first_traversal_timestamp = self.traversal_timestamp + 1; + let mut island_found = false; + + while let Some(handle) = self.islands_to_extract.pop_front() { + self.traversal_timestamp += 1; + island_found = self.extract_sleeping_island( + bodies, + colliders, + narrow_phase, + handle, + first_traversal_timestamp, + ) || island_found; + + // Make sure our `traversal_timestamp` remains correct + // even if it overflows (can happen in very long-running + // simulations). + if self.traversal_timestamp == u64::MAX - 1 { + // Stop the work now because the overflowing timestamp + // will cause issues with the traversal. Forcibly reset all + // the timestamps and continue next frame. + for body in bodies.iter_mut_internal() { + body.1.traversal_timestamp = 0; } + self.traversal_timestamp = 1; + break; } - } else { - if island.wake_up_count == 0 { - // The islands is waking up. - // Add it to the active set. - assert_eq!(island.active_island_id, crate::INVALID_USIZE); - island.active_island_id = self.active_islands.len(); - self.active_islands.push(body.island_id); - } - - island.wake_up_count += 1; } - if self.active_islands.len() == 0 { - dbg!("Hurray!"); + if island_found { + // Now we need to remove from the active set all the bodies + // we moved to a sleeping island. + let mut i = 0; + let mut new_len = 0; + let active_island = &mut self.islands[self.active_island]; + + while i < active_island.bodies.len() { + let handle = active_island.bodies[i]; + let body = &mut bodies[handle]; + + if body.island_id == self.active_island { + // This body still belongs to this island. + // Update its offset. + body.island_offset = new_len; + active_island.bodies[new_len] = handle; + new_len += 1; + } + + i += 1; + } + + active_island.bodies.truncate(new_len); } } - pub fn contact_started( + fn extract_sleeping_island( &mut self, bodies: &mut RigidBodySet, - mut h1: RigidBodyHandle, - mut h2: RigidBodyHandle, - ) { - let body1 = &bodies[h1]; - let body2 = &bodies[h2]; + colliders: &mut ColliderSet, + narrow_phase: &NarrowPhase, + handle: RigidBodyHandle, + first_traversal_timestamp: u64, + ) -> bool { + // Attempt to extract a sleeping island by traversing the + // contact graph starting with the given rigid-body. + if let Some(body) = bodies.get(handle) { + if body.is_dynamic() && body.can_sleep() { + // Perform a breadth-first search of the contact graph starting + // with this body. We stop the search when an active body is + // found (meaning that the island cannot sleep), or if we + // extracted the whole island. + // We do a breadth-first search in order to increase our chances + // to find a non-sleeping body quickly: if this body just started + // sleeping, chances are that a non-sleeping body is close. + self.islands_extraction_queue.clear(); + self.islands_extraction_queue.push(handle); + self.traversal_body_timestamps[handle.0] = self.traversal_timestamp; + let mut i = 0; + + while i < self.islands_extraction_queue.len() { + let handle = self.islands_extraction_queue[i]; + + let continue_traversal = Self::push_contacting_bodies( + handle, + bodies, + colliders, + narrow_phase, + &mut self.islands_extraction_queue, + &mut self.traversal_body_timestamps, + first_traversal_timestamp, + self.traversal_timestamp, + ); + + if !continue_traversal { + // This island cannot sleep yet. + return false; + } + + i += 1; + } - if !body1.is_dynamic() || !body2.is_dynamic() { - // Non-dynamic bodies don't transmit forces. - // So we can ignore their contact for island computation. - return; + // If we reached this point, then we successfully found a sleeping island. + for (k, handle) in self.islands_extraction_queue.iter().enumerate() { + let body = &mut bodies[*handle]; + body.island_id = self.islands.len(); + body.island_offset = k; + } + + // FIXME: recycle old islands. + self.islands.push(Island { + bodies: std::mem::replace(&mut self.islands_extraction_queue, Vec::new()), + }); + + return true; + } } - assert_ne!(body1.island_id, crate::INVALID_USIZE); - assert_ne!(body2.island_id, crate::INVALID_USIZE); + false + } + + pub fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) { + if island_id == crate::INVALID_USIZE { + return; + } - let mut island_id1 = body1.island_id; - let mut island_id2 = body2.island_id; + let mut island_id1 = self.active_island; + let mut island_id2 = island_id; if island_id1 != island_id2 { let (mut island1, mut island2) = utils::get2_mut(&mut self.islands, island_id1, island_id2); // Make sure island1 points to the biggest island. + // The typical scenario is that only a few object are awaking + // one big island. So in this typical scenario, we actually want + // to merge the active island into the sleeping island, and then + // mark the sleeping island as active. if island2.len() > island1.len() { std::mem::swap(&mut island1, &mut island2); std::mem::swap(&mut island_id1, &mut island_id2); @@ -213,37 +322,7 @@ impl IslandSet { // Islands can get big so let's save some memory. island2.bodies = vec![]; - - // Update the wake-up count and determine if the island is awake. - // Note that if both `wake_up_count` are zero, then the island - // won't end up in the active island set (which is what we want). - let island2_was_sleeping = island2.is_sleeping(); - - if island1.is_sleeping() && !island2_was_sleeping { - // Add the first island to the active island set. - assert_eq!(island1.active_island_id, crate::INVALID_USIZE); - island1.active_island_id = self.active_islands.len(); - self.active_islands.push(island_id1); - } - - island1.wake_up_count += island2.wake_up_count; - island2.wake_up_count = 0; - - assert!(island1.wake_up_count != 0 || island1.active_island_id == crate::INVALID_USIZE); - - if !island2_was_sleeping { - // The second island will be emptied, so we - // can remove it from the set of active islands. - let active_island_id_to_remove = island2.active_island_id; - island2.active_island_id = crate::INVALID_USIZE; - self.active_islands.swap_remove(active_island_id_to_remove); - - if let Some(moved_island) = self.active_islands.get(active_island_id_to_remove) { - self.islands[*moved_island].active_island_id = active_island_id_to_remove; - } - } else { - assert_eq!(island2.active_island_id, crate::INVALID_USIZE); - } + self.active_island = island_id1; } } } |