diff options
Diffstat (limited to 'src')
44 files changed, 733 insertions, 258 deletions
diff --git a/src/dynamics/island_set.rs b/src/dynamics/island_set.rs new file mode 100644 index 0000000..a0da207 --- /dev/null +++ b/src/dynamics/island_set.rs @@ -0,0 +1,249 @@ +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::NarrowPhase; +use crate::utils; +use downcast_rs::__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, + } + } + + 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[..] + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct IslandSet { + active_islands: Vec<usize>, + islands: Vec<Island>, + to_update: VecDeque<usize>, +} + +impl IslandSet { + pub fn new() -> Self { + IslandSet { + active_islands: vec![], + islands: vec![], + to_update: VecDeque::new(), + } + } + + 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() + } + + 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 incremental_update(&mut self, narrow_phase: &NarrowPhase) { + // if let Some(island) = self.to_update.pop_front() { + // // Next island to update. + // } + } + + pub fn is_island_sleeping(&self, island_id: usize) -> bool { + self.islands[island_id].is_sleeping() + } + + pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) { + assert_eq!(body.island_id, crate::INVALID_USIZE); + + if !body.is_dynamic() { + return; + } + + let new_island_id = handle.into_raw_parts().0; + + if self.islands.len() <= new_island_id { + self.islands.resize(new_island_id + 1, Island::new()); + } + + body.island_id = new_island_id; + body.island_offset = self.islands[new_island_id].bodies.len(); + self.islands[new_island_id].bodies.push(handle); + + // NOTE: `body_sleep_state_changed` must be called afterwards. + } + + 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; + } + + let island = &mut self.islands[body.island_id]; + + if body.can_sleep() { + island.wake_up_count -= 1; + + 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); + + 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 { + 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!"); + } + } + + pub fn contact_started( + &mut self, + bodies: &mut RigidBodySet, + mut h1: RigidBodyHandle, + mut h2: RigidBodyHandle, + ) { + let body1 = &bodies[h1]; + let body2 = &bodies[h2]; + + if !body1.is_dynamic() || !body2.is_dynamic() { + // Non-dynamic bodies don't transmit forces. + // So we can ignore their contact for island computation. + return; + } + + assert_ne!(body1.island_id, crate::INVALID_USIZE); + assert_ne!(body2.island_id, crate::INVALID_USIZE); + + let mut island_id1 = body1.island_id; + let mut island_id2 = body2.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. + if island2.len() > island1.len() { + std::mem::swap(&mut island1, &mut island2); + std::mem::swap(&mut island_id1, &mut island_id2); + } + + // Now merge island2 (the smallest island) into island1 (the bigger one). + island1.bodies.reserve(island2.len()); + + for handle in island2.bodies.drain(..) { + let body = &mut bodies[handle]; + body.island_id = island_id1; + body.island_offset = island1.len(); + island1.bodies.push(handle); + } + + // 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); + } + } + } +} diff --git a/src/dynamics/island_set2.rs b/src/dynamics/island_set2.rs new file mode 100644 index 0000000..1c38b7f --- /dev/null +++ b/src/dynamics/island_set2.rs @@ -0,0 +1,133 @@ +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::NarrowPhase; +use crate::utils; +use downcast_rs::__std::collections::VecDeque; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct Island { + bodies: Vec<RigidBodyHandle>, +} + +impl Island { + pub fn new() -> Self { + Self { bodies: vec![] } + } + + pub fn len(&self) -> usize { + self.bodies.len() + } + + pub fn is_sleeping(&self) -> bool { + self.wake_up_count == 0 + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct IslandSet2 { + inactive_islands: Vec<Island>, + active_island: Island, +} + +impl IslandSet2 { + pub fn new() -> Self { + IslandSet2 { + inactive_islands: vec![], + active_island: Island::new(), + } + } + + pub fn num_active_islands(&self) -> usize { + 1 + } + + pub fn active_island(&self, i: usize) -> &Island { + &self.active_island + } + + pub fn active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a { + self.active_island.bodies.iter() + } + + pub fn contact_stopped( + &mut self, + bodies: &RigidBodySet, + h1: RigidBodyHandle, + _h2: RigidBodyHandle, + ) { + } + + pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {} + + pub fn is_island_sleeping(&self, island_id: usize) -> bool { + island_id != crate::INVALID_USIZE + } + + pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) { + assert_eq!(body.island_id, crate::INVALID_USIZE); + + if !body.is_dynamic() { + return; + } + + if body.can_sleep() { + let new_island_id = handle.into_raw_parts().0; + + if self.islands.len() <= new_island_id { + self.islands.resize(new_island_id + 1, Island::new()); + } + + body.island_id = new_island_id; + body.island_offset = self.islands[new_island_id].bodies.len(); + self.islands[new_island_id].bodies.push(handle); + } else { + body.island_offset = self.active_island.len(); + self.active_island.push(handle); + } + } + + 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; + } + + let island = &mut self.islands[body.island_id]; + + if body.can_sleep() { + island.wake_up_count -= 1; + + 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); + + 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 { + 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; + } + } + + pub fn contact_started( + &mut self, + bodies: &mut RigidBodySet, + mut h1: RigidBodyHandle, + mut h2: RigidBodyHandle, + ) { + } +} diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index a87532a..cad9869 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -2,7 +2,7 @@ use super::Joint; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; use crate::data::arena::Arena; -use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{IslandSet, JointParams, RigidBodyHandle, RigidBodySet}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -178,10 +178,11 @@ impl JointSet { // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. pub(crate) fn select_active_interactions( &self, + islands: &IslandSet, bodies: &RigidBodySet, out: &mut Vec<Vec<JointIndex>>, ) { - for out_island in &mut out[..bodies.num_islands()] { + for out_island in &mut out[..islands.num_active_islands()] { out_island.clear(); } @@ -196,9 +197,9 @@ impl JointSet { && (!rb2.is_dynamic() || !rb2.is_sleeping()) { let island_index = if !rb1.is_dynamic() { - rb2.active_island_id + islands.islands()[rb2.island_id].active_island_id() } else { - rb1.active_island_id + islands.islands()[rb1.island_id].active_island_id() }; out[island_index].push(i); diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 8c38dc2..2ff6f5c 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -12,6 +12,8 @@ pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; pub use parry::mass_properties::MassProperties; // #[cfg(not(feature = "parallel"))] pub use self::coefficient_combine_rule::CoefficientCombineRule; +pub use self::island_set::{Island, IslandSet}; + pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; #[cfg(not(feature = "parallel"))] @@ -21,6 +23,8 @@ pub(crate) use self::solver::ParallelIslandSolver; mod coefficient_combine_rule; mod integration_parameters; +mod island_set; +// mod island_set2; mod joint; mod rigid_body; mod rigid_body_set; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 5fb6183..f2c3807 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -83,6 +83,8 @@ pub struct RigidBody { pub activation: ActivationStatus, pub(crate) joint_graph_index: RigidBodyGraphIndex, pub(crate) active_island_id: usize, + pub(crate) island_offset: usize, + pub(crate) island_id: usize, pub(crate) active_set_id: usize, pub(crate) active_set_offset: usize, pub(crate) active_set_timestamp: u32, @@ -113,6 +115,8 @@ impl RigidBody { colliders: Vec::new(), activation: ActivationStatus::new_active(), joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), + island_id: crate::INVALID_USIZE, + island_offset: crate::INVALID_USIZE, active_island_id: 0, active_set_id: 0, active_set_offset: 0, @@ -127,6 +131,8 @@ impl RigidBody { pub(crate) fn reset_internal_references(&mut self) { self.colliders = Vec::new(); self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index(); + self.island_id = crate::INVALID_USIZE; + self.island_offset = 0; // crate::INVALID_USIZE; self.active_island_id = 0; self.active_set_id = 0; self.active_set_offset = 0; @@ -298,6 +304,10 @@ impl RigidBody { self.activation.sleeping } + pub fn can_sleep(&self) -> bool { + self.activation.can_sleep() + } + /// Is the velocity of this body not zero? pub fn is_moving(&self) -> bool { !self.linvel.is_zero() || !self.angvel.is_zero() @@ -873,4 +883,8 @@ impl ActivationStatus { pub fn is_active(&self) -> bool { self.energy != 0.0 } + + pub fn can_sleep(&self) -> bool { + self.energy <= Self::default_threshold() + } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 36cf4d3..01ef0ac 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,6 +2,7 @@ use rayon::prelude::*; use crate::data::arena::Arena; +use crate::dynamics::island_set::IslandSet; use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges}; use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase}; use parry::partitioning::IndexedData; @@ -169,9 +170,9 @@ impl RigidBodySet { Some(rb) } - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len() - 1 - } + // pub(crate) fn num_islands(&self) -> usize { + // self.active_islands.len() - 1 + // } /// Forces the specified rigid-body to wake up if it is dynamic. /// @@ -273,57 +274,57 @@ impl RigidBodySet { .filter_map(move |h| Some((*h, bodies.get(h.0)?))) } - /// Iter through all the active dynamic rigid-bodies on this set. - pub fn iter_active_dynamic<'a>( - &'a self, - ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> { - let bodies: &'a _ = &self.bodies; - self.active_dynamic_set - .iter() - .filter_map(move |h| Some((*h, bodies.get(h.0)?))) - } - - #[cfg(not(feature = "parallel"))] - pub(crate) fn iter_active_island<'a>( - &'a self, - island_id: usize, - ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> { - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - let bodies: &'a _ = &self.bodies; - self.active_dynamic_set[island_range] - .iter() - .filter_map(move |h| Some((*h, bodies.get(h.0)?))) - } - - #[inline(always)] - pub(crate) fn foreach_active_body_mut_internal( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_dynamic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } + // /// Iter through all the active dynamic rigid-bodies on this set. + // pub fn iter_active_dynamic<'a>( + // &'a self, + // ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> { + // let bodies: &'a _ = &self.bodies; + // self.active_dynamic_set + // .iter() + // .filter_map(move |h| Some((*h, bodies.get(h.0)?))) + // } + // + // #[cfg(not(feature = "parallel"))] + // pub(crate) fn iter_active_island<'a>( + // &'a self, + // island_id: usize, + // ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> { + // let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + // let bodies: &'a _ = &self.bodies; + // self.active_dynamic_set[island_range] + // .iter() + // .filter_map(move |h| Some((*h, bodies.get(h.0)?))) + // } - for handle in &self.active_kinematic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } + // #[inline(always)] + // pub(crate) fn foreach_active_body_mut_internal( + // &mut self, + // mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + // ) { + // for handle in &self.active_dynamic_set { + // if let Some(rb) = self.bodies.get_mut(handle.0) { + // f(*handle, rb) + // } + // } + // + // for handle in &self.active_kinematic_set { + // if let Some(rb) = self.bodies.get_mut(handle.0) { + // f(*handle, rb) + // } + // } + // } - #[inline(always)] - pub(crate) fn foreach_active_dynamic_body_mut_internal( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_dynamic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } + // #[inline(always)] + // pub(crate) fn foreach_active_dynamic_body_mut_internal( + // &mut self, + // mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + // ) { + // for handle in &self.active_dynamic_set { + // if let Some(rb) = self.bodies.get_mut(handle.0) { + // f(*handle, rb) + // } + // } + // } #[inline(always)] pub(crate) fn foreach_active_kinematic_body_mut_internal( @@ -337,60 +338,36 @@ impl RigidBodySet { } } - #[inline(always)] - #[cfg(not(feature = "parallel"))] - pub(crate) fn foreach_active_island_body_mut_internal( - &mut self, - island_id: usize, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - for handle in &self.active_dynamic_set[island_range] { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } - - #[cfg(feature = "parallel")] - #[inline(always)] - #[allow(dead_code)] - pub(crate) fn foreach_active_island_body_mut_internal_parallel( - &mut self, - i |