From cf77d17d9e6c425b1899c03db8e07f265259791b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 3 Feb 2021 16:33:08 +0100 Subject: Experiment with incremental island computation. --- src/dynamics/island_set.rs | 249 +++++++++++++++++++++ src/dynamics/island_set2.rs | 133 +++++++++++ src/dynamics/joint/joint_set.rs | 9 +- src/dynamics/mod.rs | 4 + src/dynamics/rigid_body.rs | 14 ++ src/dynamics/rigid_body_set.rs | 185 +++++++-------- src/dynamics/solver/interaction_groups.rs | 31 +-- src/dynamics/solver/island_solver.rs | 23 +- .../joint_constraint/ball_position_constraint.rs | 8 +- .../ball_position_constraint_wide.rs | 6 +- .../joint_constraint/ball_velocity_constraint.rs | 6 +- .../ball_velocity_constraint_wide.rs | 6 +- .../joint_constraint/fixed_position_constraint.rs | 6 +- .../joint_constraint/fixed_velocity_constraint.rs | 6 +- .../fixed_velocity_constraint_wide.rs | 6 +- .../prismatic_position_constraint.rs | 6 +- .../prismatic_velocity_constraint.rs | 6 +- .../prismatic_velocity_constraint_wide.rs | 6 +- .../revolute_position_constraint.rs | 6 +- .../revolute_velocity_constraint.rs | 6 +- .../revolute_velocity_constraint_wide.rs | 6 +- src/dynamics/solver/parallel_island_solver.rs | 6 +- src/dynamics/solver/position_constraint.rs | 4 +- src/dynamics/solver/position_constraint_wide.rs | 4 +- src/dynamics/solver/position_ground_constraint.rs | 2 +- .../solver/position_ground_constraint_wide.rs | 2 +- src/dynamics/solver/position_solver.rs | 22 +- src/dynamics/solver/solver_constraints.rs | 12 +- src/dynamics/solver/velocity_constraint.rs | 4 +- src/dynamics/solver/velocity_constraint_wide.rs | 4 +- ...ocity_constraint_wide_with_manifold_friction.rs | 4 +- .../velocity_constraint_with_manifold_friction.rs | 4 +- src/dynamics/solver/velocity_ground_constraint.rs | 2 +- .../solver/velocity_ground_constraint_wide.rs | 2 +- ...round_constraint_wide_with_manifold_friction.rs | 2 +- ...ity_ground_constraint_with_manifold_friction.rs | 2 +- src/dynamics/solver/velocity_solver.rs | 21 +- src/geometry/broad_phase_multi_sap.rs | 10 +- src/geometry/narrow_phase.rs | 28 ++- src/pipeline/collision_pipeline.rs | 7 +- src/pipeline/mod.rs | 4 +- src/pipeline/physics_pipeline.rs | 87 ++++--- src/pipeline/query_pipeline.rs | 16 +- src/utils.rs | 14 ++ 44 files changed, 733 insertions(+), 258 deletions(-) create mode 100644 src/dynamics/island_set.rs create mode 100644 src/dynamics/island_set2.rs (limited to 'src') 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, + + // 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, + islands: Vec, + to_update: VecDeque, +} + +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 + '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, +} + +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, + 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 + '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>, ) { - 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 { - 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 { - 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 { + // 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 { + // 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, - island_id: usize, - f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, - ) { - use std::sync::atomic::Ordering; - - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _); - self.active_dynamic_set[island_range] - .par_iter() - .for_each_init( - || bodies.load(Ordering::Relaxed), - |bodies, handle| { - let bodies: &mut Arena = unsafe { std::mem::transmute(*bodies) }; - if let Some(rb) = bodies.get_mut(handle.0) { - f(*handle, rb) - } - }, - ); - } + // #[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) + // } + // } + // } // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] { // &self.active_dynamic_set // } - pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { - self.active_islands[island_id]..self.active_islands[island_id + 1] - } - - pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { - &self.active_dynamic_set[self.active_island_range(island_id)] - } + // pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { + // self.active_islands[island_id]..self.active_islands[island_id + 1] + // } + // + // pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { + // &self.active_dynamic_set[self.active_island_range(island_id)] + // } // Utility function to avoid some borrowing issue in the `maintain` method. fn maintain_one( + islands: &mut IslandSet, colliders: &mut ColliderSet, handle: RigidBodyHandle, rb: &mut RigidBody, @@ -425,13 +402,22 @@ impl RigidBodySet { active_dynamic_set.push(handle); } + if rb.changes.contains(RigidBodyChanges::SLEEP) { + if rb.island_id == crate::INVALID_USIZE { + islands.add_rigid_body(handle, rb); + } + + islands.body_sleep_state_changed(rb); + } + rb.changes = RigidBodyChanges::empty(); } - pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) { + pub(crate) fn maintain(&mut self, islands: &mut IslandSet, colliders: &mut ColliderSet) { if self.modified_all_bodies { for (handle, rb) in self.bodies.iter_mut() { Self::maintain_one( + islands, colliders, RigidBodyHandle(handle), rb, @@ -447,6 +433,7 @@ impl RigidBodySet { for handle in self.modified_bodies.drain(..) { if let Some(rb) = self.bodies.get_mut(handle.0) { Self::maintain_one( + islands, colliders, handle, rb, diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 21cc642..fef7565 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{BodyPair, IslandSet, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { @@ -83,24 +83,23 @@ impl ParallelInteractionGroups { match (rb1.is_static(), rb2.is_static()) { (false, false) => { - let color_mask = - bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset]; + let color_mask = bcolors[rb1.island_offset] | bcolors[rb2.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.active_set_offset] |= 1 << *color; - bcolors[rb2.active_set_offset] |= 1 << *color; + bcolors[rb1.island_offset] |= 1 << *color; + bcolors[rb2.island_offset] |= 1 << *color; } (true, false) => { - let color_mask = bcolors[rb2.active_set_offset]; + let color_mask = bcolors[rb2.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb2.active_set_offset] |= 1 << *color; + bcolors[rb2.island_offset] |= 1 << *color; } (false, true) => { - let color_mask = bcolors[rb1.active_set_offset]; + let color_mask = bcolors[rb1.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.active_set_offset] |= 1 << *color; + bcolors[rb1.island_offset] |= 1 << *color; } (true, true) => unreachable!(), } @@ -176,6 +175,7 @@ impl InteractionGroups { pub fn group_joints( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], @@ -197,7 +197,7 @@ impl InteractionGroups { // is full, we don't clear the corresponding body mask bit. This may result // in less grouped constraints. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -215,8 +215,8 @@ impl InteractionGroups { } let ijoint = interaction.params.type_id(); - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = body1.island_offset; + let i2 = body2.island_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. @@ -320,6 +320,7 @@ impl InteractionGroups { pub fn group_manifolds( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], @@ -331,7 +332,7 @@ impl InteractionGroups { // in less grouped contacts. // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -365,8 +366,8 @@ impl InteractionGroups { continue; } - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = body1.island_offset; + let i2 = body2.island_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 8b7e587..81fc5d0 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, IslandSet, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { @@ -29,6 +29,7 @@ impl IslandSolver { island_id: usize, counters: &mut Counters, params: &IntegrationParameters, + islands: &IslandSet, bodies: &mut RigidBodySet, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -37,15 +38,22 @@ impl IslandSolver { ) { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.velocity_assembly_time.resume(); - self.contact_constraints - .init(island_id, params, bodies, manifolds, manifold_indices); + self.contact_constraints.init( + island_id, + islands, + params, + bodies, + manifolds, + manifold_indices, + ); self.joint_constraints - .init(island_id, params, bodies, joints, joint_indices); + .init(island_id, islands, params, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve( island_id, + islands, params, bodies, manifolds, @@ -57,7 +65,11 @@ impl IslandSolver { } counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt)); + for handle in islands.active_island(island_id).bodies() { + if let Some(rb) = bodies.get_mut_internal(*handle) { + rb.integrate(params.dt) + } + } counters.solver.velocity_update_time.pause(); if params.position_erp != 0.0 { @@ -66,6 +78,7 @@ impl IslandSolver { self.position_solver.solve( island_id, params, + islands, bodies, &self.contact_constraints.position_constraints, &self.joint_constraints.position_constraints, diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 744c00d..6708d44 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -33,8 +33,8 @@ impl BallPositionConstraint { ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, } } @@ -118,7 +118,7 @@ impl BallPositionGroundConstraint { im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, + position2: rb2.island_offset, local_com2: rb2.mass_properties.local_com, } } else { @@ -127,7 +127,7 @@ impl BallPositionGroundConstraint { im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, local_com2: rb2.mass_properties.local_com, } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index 043eea7..08fb96c 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -43,8 +43,8 @@ impl WBallPositionConstraint { .squared(); let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; Self { local_com1, @@ -151,7 +151,7 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor2 }; SIMD_WIDTH]); - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); Self { diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index e75f978..2791747 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -79,8 +79,8 @@ impl BallVelocityConstraint { BallVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, im2, impulse: cparams.impulse * params.warmstart_coeff, @@ -201,7 +201,7 @@ impl BallVelocityGroundConstraint { BallVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, impulse: cparams.impulse * params.warmstart_coeff, r2: anchor2, diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index 95b0bb5..a18bccb 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -46,7 +46,7 @@ impl WBallVelocityConstraint { let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); @@ -56,7 +56,7 @@ impl WBallVelocityConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); @@ -232,7 +232,7 @@ impl WBallVelocityGroundConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor2 = Point::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 7e8fc97..5e0b485 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -31,8 +31,8 @@ impl FixedPositionConstraint { Self { local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, im1, im2, ii1, @@ -110,7 +110,7 @@ impl FixedPositionGroundConstraint { Self { anchor1, local_anchor2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_com2: rb2.mass_properties.local_com, diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 2868d4b..2f284a7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -112,8 +112,8 @@ impl FixedVelocityConstraint { FixedVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, im2, ii1, @@ -301,7 +301,7 @@ impl FixedVelocityGroundConstraint { FixedVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index c423272..0090a8b 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -65,7 +65,7 @@ impl WFixedVelocityConstraint { let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); @@ -75,7 +75,7 @@ impl WFixedVelocityConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); @@ -319,7 +319,7 @@ impl WFixedVelocityGroundConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Isometry::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ea7e927..f03804a 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -46,8 +46,8 @@ impl PrismaticPositionConstraint { local_frame2: cparams.local_frame2(), local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, limits: cparams.limits, } } @@ -135,7 +135,7 @@ impl PrismaticPositionGroundConstraint { local_frame2, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, limits: cparams.limits, } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index d1943fc..6c8e9b6 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -173,8 +173,8 @@ impl PrismaticVelocityConstraint { PrismaticVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, im2, @@ -463,7 +463,7 @@ impl PrismaticVelocityGroundConstraint { PrismaticVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index f24cfa5..f084d60 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -77,7 +77,7 @@ impl WPrismaticVelocityConstraint { let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); @@ -87,7 +87,7 @@ impl WPrismaticVelocityConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); @@ -435,7 +435,7 @@ impl WPrismaticVelocityGroundConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; #[cfg(feature = "dim2")] let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 19dd451..d9f6cff 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -44,8 +44,8 @@ impl RevolutePositionConstraint { local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, } } @@ -118,7 +118,7 @@ impl RevolutePositionGroundConstraint { local_anchor2, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, } } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 38f56d9..22fd916 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -84,8 +84,8 @@ impl RevoluteVelocityConstraint { RevoluteVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, basis1, @@ -238,7 +238,7 @@ impl RevoluteVelocityGroundConstraint { RevoluteVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 822c2ac..fbfec3b 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -47,7 +47,7 @@ impl WRevoluteVelocityConstraint { let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); @@ -57,7 +57,7 @@ impl WRevoluteVelocityConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); @@ -266,7 +266,7 @@ impl WRevoluteVelocityGroundConstraint { let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); let local_anchor1 = Point::from( diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index af8e9c0..5f7073e 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -248,11 +248,11 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { let rb = &mut bodies[handle.0]; - let dvel = mj_lambdas[rb.active_set_offset]; + let dvel = mj_lambdas[rb.island_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt); - positions[rb.active_set_offset] = rb.position; + positions[rb.island_offset] = rb.position; } } @@ -273,7 +273,7 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.position_writeback_index] { let rb = &mut bodies[handle.0]; - rb.set_position_internal(positions[rb.active_set_offset]); + rb.set_position_internal(positions[rb.island_offset]); } } }) diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index ecf8286..4c68305 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -82,8 +82,8 @@ impl PositionConstraint { } let constraint = PositionConstraint { - rb1: rb1.active_set_offset, - rb2: rb2.active_set_offset, + rb1: rb1.island_offset, + rb2: rb2.island_offset, local_p1, local_p2, local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index b7918c5..ae0fc62 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -54,8 +54,8 @@ impl WPositionConstraint { array![|ii| manifolds[ii].data.normal; SIMD_WIDTH], )); - let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let rb1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let rb2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let num_active_contacts = manifolds[0].data.num_active_contacts(); diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index 1e5a2ff..24a717a 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -58,7 +58,7 @@ impl PositionGroundConstraint { } let constraint = PositionGroundConstraint { - rb2: rb2.active_set_offset, + rb2: rb2.island_offset, p1, local_p2, n1, diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 531eead..9397325 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -55,7 +55,7 @@ impl WPositionGroundConstraint { ); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let rb2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let num_active_contacts = manifolds[0].data.num_active_contacts(); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index df0e3fc..d979233 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,5 +1,7 @@ use super::AnyJointPositionConstraint; -use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; +use crate::dynamics::{ + solver::AnyPositionConstraint, IntegrationParameters, IslandSet, RigidBodySet, +}; use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { @@ -17,15 +19,19 @@ impl PositionSolver { &mut self, island_id: usize, params: &IntegrationParameters, + islands: &IslandSet, bodies: &mut RigidBodySet, contact_constraints: &[AnyPositionConstraint], joint_constraints: &[AnyJointPositionConstraint], ) { self.positions.clear(); self.positions.extend( - bodies - .iter_active_island(island_id) - .map(|(_, b)| b.position), + islands + .active_island(island_id) + .bodies() + .iter() + .filter_map(|h| bodies.get(*h)) + .map(|b| b.position), ); for _ in 0..params.max_position_iterations { @@ -38,8 +44,10 @@ impl PositionSolver { } } - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_position_internal(self.positions[rb.active_set_offset]) - }); + for handle in islands.active_island(island_id).bodies() { + if let Some(rb) = bodies.get_mut(*handle) { + rb.set_position_internal(self.positions[rb.island_offset]) + } + } } } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 1f4cec5..b4ac54d 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -14,7 +14,8 @@ use crate::dynamics::solver::{ VelocityGroundConstraintWithManifoldFriction, }; use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + solver::AnyVelocityConstraint, IntegrationParameters, IslandSet, JointGraphEdge, JointIndex, + RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] @@ -48,6 +49,7 @@ impl SolverConstraints { pub fn init_constraint_groups( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -65,6 +67,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.not_ground_interactions, @@ -73,6 +76,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.ground_interactions, @@ -90,6 +94,7 @@ impl SolverConstraints { pub fn init( &mut self, island_id: usize, + islands: &IslandSet, params: &IntegrationParameters, bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], @@ -98,7 +103,7 @@ impl SolverConstraints { self.velocity_constraints.clear(); self.position_constraints.clear(); - self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); + self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices); #[cfg(feature = "simd-is-enabled")] { @@ -245,6 +250,7 @@ impl SolverConstraints { pub fn init( &mut self, island_id: usize, + islands: &IslandSet, params: &IntegrationParameters, bodies: &RigidBodySet, joints: &[JointGraphEdge], @@ -267,6 +273,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.not_ground_interactions, @@ -275,6 +282,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.ground_interactions, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index aa3ea62..4454a9a 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -177,8 +177,8 @@ impl VelocityConstraint { let inv_dt = params.inv_dt(); let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - let mj_lambda1 = rb1.active_set_offset; - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda1 = rb1.island_offset; + let mj_lambda2 = rb2.island_offset; let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 97aa0eb..03e4ed8 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -93,8 +93,8 @@ impl WVelocityConstraint { let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);