From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/data/arena.rs | 87 ++- src/data/coarena.rs | 30 +- src/data/component_set.rs | 106 +++ src/data/mod.rs | 3 + src/dynamics/ccd/ccd_solver.rs | 444 ++++++++----- src/dynamics/ccd/toi_entry.rs | 112 +++- src/dynamics/coefficient_combine_rule.rs | 14 +- src/dynamics/island_manager.rs | 344 ++++++++++ src/dynamics/joint/joint_set.rs | 116 ++-- src/dynamics/mod.rs | 17 +- src/dynamics/rigid_body.rs | 728 ++++++++------------- src/dynamics/rigid_body_components.rs | 659 +++++++++++++++++++ src/dynamics/rigid_body_set.rs | 658 +++---------------- src/dynamics/solver/categorization.rs | 13 +- src/dynamics/solver/interaction_groups.rs | 96 ++- src/dynamics/solver/island_solver.rs | 92 ++- .../joint_constraint/ball_position_constraint.rs | 56 +- .../ball_position_constraint_wide.rs | 83 ++- .../joint_constraint/ball_velocity_constraint.rs | 114 ++-- .../ball_velocity_constraint_wide.rs | 190 +++--- .../joint_constraint/fixed_position_constraint.rs | 48 +- .../fixed_position_constraint_wide.rs | 34 +- .../joint_constraint/fixed_velocity_constraint.rs | 86 ++- .../fixed_velocity_constraint_wide.rs | 196 +++--- .../generic_position_constraint.rs | 2 +- .../generic_position_constraint_wide.rs | 13 +- .../generic_velocity_constraint.rs | 16 +- .../generic_velocity_constraint_wide.rs | 154 +++-- .../solver/joint_constraint/joint_constraint.rs | 139 ++-- .../joint_constraint/joint_position_constraint.rs | 99 ++- .../prismatic_position_constraint.rs | 42 +- .../prismatic_position_constraint_wide.rs | 34 +- .../prismatic_velocity_constraint.rs | 138 ++-- .../prismatic_velocity_constraint_wide.rs | 296 +++++---- .../revolute_position_constraint.rs | 60 +- .../revolute_position_constraint_wide.rs | 34 +- .../revolute_velocity_constraint.rs | 140 ++-- .../revolute_velocity_constraint_wide.rs | 258 ++++---- src/dynamics/solver/parallel_island_solver.rs | 39 +- src/dynamics/solver/parallel_solver_constraints.rs | 26 +- src/dynamics/solver/position_constraint.rs | 44 +- src/dynamics/solver/position_constraint_wide.rs | 69 +- src/dynamics/solver/position_ground_constraint.rs | 33 +- .../solver/position_ground_constraint_wide.rs | 54 +- src/dynamics/solver/position_solver.rs | 31 +- src/dynamics/solver/solver_constraints.rs | 152 +++-- src/dynamics/solver/velocity_constraint.rs | 60 +- src/dynamics/solver/velocity_constraint_wide.rs | 144 ++-- src/dynamics/solver/velocity_ground_constraint.rs | 56 +- .../solver/velocity_ground_constraint_wide.rs | 123 ++-- src/dynamics/solver/velocity_solver.rs | 51 +- src/geometry/broad_phase_multi_sap/broad_phase.rs | 220 ++++--- src/geometry/broad_phase_multi_sap/sap_layer.rs | 5 +- src/geometry/collider.rs | 305 ++++----- src/geometry/collider_components.rs | 220 +++++++ src/geometry/collider_set.rs | 277 +++----- src/geometry/contact_pair.rs | 22 +- src/geometry/mod.rs | 17 +- src/geometry/narrow_phase.rs | 593 +++++++++++------ src/lib.rs | 8 +- src/pipeline/collision_pipeline.rs | 67 +- src/pipeline/mod.rs | 1 + src/pipeline/physics_hooks.rs | 83 +-- src/pipeline/physics_pipeline.rs | 360 +++++++--- src/pipeline/query_pipeline.rs | 424 ++++++++---- src/pipeline/user_changes.rs | 156 +++++ 66 files changed, 5829 insertions(+), 3562 deletions(-) create mode 100644 src/data/component_set.rs create mode 100644 src/dynamics/island_manager.rs create mode 100644 src/dynamics/rigid_body_components.rs create mode 100644 src/geometry/collider_components.rs create mode 100644 src/pipeline/user_changes.rs (limited to 'src') diff --git a/src/data/arena.rs b/src/data/arena.rs index 9d057b8..bc7176d 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -19,16 +19,16 @@ use std::vec; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Arena { items: Vec>, - generation: u64, - free_list_head: Option, + generation: u32, + free_list_head: Option, len: usize, } #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] enum Entry { - Free { next_free: Option }, - Occupied { generation: u64, value: T }, + Free { next_free: Option }, + Occupied { generation: u32, value: T }, } /// An index (and generation) into an `Arena`. @@ -48,17 +48,17 @@ enum Entry { #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Index { - index: usize, - generation: u64, + index: u32, + generation: u32, } impl IndexedData for Index { fn default() -> Self { - Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + Self::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32) } fn index(&self) -> usize { - self.into_raw_parts().0 + self.into_raw_parts().0 as usize } } @@ -70,7 +70,7 @@ impl Index { /// /// Providing arbitrary values will lead to malformed indices and ultimately /// panics. - pub fn from_raw_parts(a: usize, b: u64) -> Index { + pub fn from_raw_parts(a: u32, b: u32) -> Index { Index { index: a, generation: b, @@ -84,7 +84,7 @@ impl Index { /// `Index` like `pub struct MyIdentifier(Index);`. However, for external /// types whose definition you can't customize, but which you can construct /// instances of, this method can be useful. - pub fn into_raw_parts(self) -> (usize, u64) { + pub fn into_raw_parts(self) -> (u32, u32) { (self.index, self.generation) } } @@ -161,7 +161,7 @@ impl Arena { pub fn clear(&mut self) { self.items.clear(); - let end = self.items.capacity(); + let end = self.items.capacity() as u32; self.items.extend((0..end).map(|i| { if i == end - 1 { Entry::Free { next_free: None } @@ -206,7 +206,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(value), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value, }; @@ -247,7 +247,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(create), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value: create(index), }; @@ -260,13 +260,13 @@ impl Arena { fn try_alloc_next_index(&mut self) -> Option { match self.free_list_head { None => None, - Some(i) => match self.items[i] { + Some(i) => match self.items[i as usize] { Entry::Occupied { .. } => panic!("corrupt free list"), Entry::Free { next_free } => { self.free_list_head = next_free; self.len += 1; Some(Index { - index: i, + index: i as u32, generation: self.generation, }) } @@ -355,14 +355,14 @@ impl Arena { /// assert_eq!(arena.remove(idx), None); /// ``` pub fn remove(&mut self, i: Index) -> Option { - if i.index >= self.items.len() { + if i.index >= self.items.len() as u32 { return None; } - match self.items[i.index] { + match self.items[i.index as usize] { Entry::Occupied { generation, .. } if i.generation == generation => { let entry = mem::replace( - &mut self.items[i.index], + &mut self.items[i.index as usize], Entry::Free { next_free: self.free_list_head, }, @@ -402,8 +402,8 @@ impl Arena { /// assert!(crew_members.next().is_none()); /// ``` pub fn retain(&mut self, mut predicate: impl FnMut(Index, &mut T) -> bool) { - for i in 0..self.capacity() { - let remove = match &mut self.items[i] { + for i in 0..self.capacity() as u32 { + let remove = match &mut self.items[i as usize] { Entry::Occupied { generation, value } => { let index = Index { index: i, @@ -462,7 +462,7 @@ impl Arena { /// assert!(arena.get(idx).is_none()); /// ``` pub fn get(&self, i: Index) -> Option<&T> { - match self.items.get(i.index) { + match self.items.get(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -488,7 +488,7 @@ impl Arena { /// assert!(arena.get_mut(idx).is_none()); /// ``` pub fn get_mut(&mut self, i: Index) -> Option<&mut T> { - match self.items.get_mut(i.index) { + match self.items.get_mut(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -526,7 +526,7 @@ impl Arena { /// assert_eq!(arena[idx2], 4); /// ``` pub fn get2_mut(&mut self, i1: Index, i2: Index) -> (Option<&mut T>, Option<&mut T>) { - let len = self.items.len(); + let len = self.items.len() as u32; if i1.index == i2.index { assert!(i1.generation != i2.generation); @@ -544,11 +544,13 @@ impl Arena { } let (raw_item1, raw_item2) = { - let (xs, ys) = self.items.split_at_mut(cmp::max(i1.index, i2.index)); + let (xs, ys) = self + .items + .split_at_mut(cmp::max(i1.index, i2.index) as usize); if i1.index < i2.index { - (&mut xs[i1.index], &mut ys[0]) + (&mut xs[i1.index as usize], &mut ys[0]) } else { - (&mut ys[0], &mut xs[i2.index]) + (&mut ys[0], &mut xs[i2.index as usize]) } }; @@ -666,11 +668,11 @@ impl Arena { } } else { Entry::Free { - next_free: Some(i + 1), + next_free: Some(i as u32 + 1), } } })); - self.free_list_head = Some(start); + self.free_list_head = Some(start as u32); } /// Iterate over shared references to the elements in this arena. @@ -774,7 +776,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -797,7 +799,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -941,7 +943,10 @@ impl<'a, T> Iterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -970,7 +975,10 @@ impl<'a, T> DoubleEndedIterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1039,7 +1047,10 @@ impl<'a, T> Iterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1068,7 +1079,10 @@ impl<'a, T> DoubleEndedIterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1126,7 +1140,10 @@ impl<'a, T> Iterator for Drain<'a, T> { match self.inner.next() { Some((_, Entry::Free { .. })) => continue, Some((index, Entry::Occupied { generation, value })) => { - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => return None, diff --git a/src/data/coarena.rs b/src/data/coarena.rs index c25cc55..cd64910 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -4,7 +4,7 @@ use crate::data::arena::Index; #[derive(Clone, Debug)] /// A container for data associated to item existing into another Arena. pub struct Coarena { - data: Vec<(u64, T)>, + data: Vec<(u32, T)>, } impl Coarena { @@ -17,7 +17,7 @@ impl Coarena { pub fn get(&self, index: Index) -> Option<&T> { let (i, g) = index.into_raw_parts(); self.data - .get(i) + .get(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -25,7 +25,7 @@ impl Coarena { pub fn get_mut(&mut self, index: Index) -> Option<&mut T> { let (i, g) = index.into_raw_parts(); self.data - .get_mut(i) + .get_mut(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -36,11 +36,11 @@ impl Coarena { { let (i1, g1) = a.into_raw_parts(); - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, T::default())); + if self.data.len() <= i1 as usize { + self.data.resize(i1 as usize + 1, (u32::MAX, T::default())); } - self.data[i1] = (g1, value); + self.data[i1 as usize] = (g1, value); } /// Ensure that elements at the two given indices exist in this coarena, and return their reference. @@ -56,20 +56,22 @@ impl Coarena { assert_ne!(i1, i2, "Cannot index the same object twice."); let (elt1, elt2) = if i1 > i2 { - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i1 as usize { + self.data + .resize(i1 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i1); - (&mut right[0], &mut left[i2]) + let (left, right) = self.data.split_at_mut(i1 as usize); + (&mut right[0], &mut left[i2 as usize]) } else { // i2 > i1 - if self.data.len() <= i2 { - self.data.resize(i2 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i2 as usize { + self.data + .resize(i2 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i2); - (&mut left[i1], &mut right[0]) + let (left, right) = self.data.split_at_mut(i2 as usize); + (&mut left[i1 as usize], &mut right[0]) }; if elt1.0 != g1 { diff --git a/src/data/component_set.rs b/src/data/component_set.rs new file mode 100644 index 0000000..76e076a --- /dev/null +++ b/src/data/component_set.rs @@ -0,0 +1,106 @@ +use crate::data::Index; + +// TODO ECS: use this to handle optional components properly. +// pub trait OptionalComponentSet { +// fn get(&self, handle: Index) -> Option<&T>; +// } + +pub trait ComponentSetOption { + fn get(&self, handle: Index) -> Option<&T>; +} + +pub trait ComponentSet: ComponentSetOption { + fn size_hint(&self) -> usize; + // TODO ECS: remove this, its only needed by the query pipeline update + // which should only take the modified colliders into account. + fn for_each(&self, f: impl FnMut(Index, &T)); + fn index(&self, handle: Index) -> &T { + self.get(handle).unwrap() + } +} + +pub trait ComponentSetMut: ComponentSet { + fn map_mut_internal( + &mut self, + handle: crate::data::Index, + f: impl FnOnce(&mut T) -> Result, + ) -> Option; + fn set_internal(&mut self, handle: crate::data::Index, val: T); +} + +pub trait BundleSet<'a, T> { + fn index_bundle(&'a self, handle: Index) -> T; +} + +impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T +where + T: ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) { + (self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T +where + T: ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) { + (self.index(handle), self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T +where + T: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c49314..f20e433 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -1,8 +1,11 @@ //! Data structures modified with guaranteed deterministic behavior after deserialization. +pub use self::arena::{Arena, Index}; pub use self::coarena::Coarena; +pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; pub mod arena; mod coarena; +mod component_set; pub(crate) mod graph; pub mod pubsub; diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ff463c7..6afd860 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,6 +1,12 @@ use super::TOIEntry; -use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase}; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces}; +use crate::dynamics::{ + RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::{ + ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase, +}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; @@ -44,19 +50,34 @@ impl CCDSolver { /// Apply motion-clamping to the bodies affected by the given `impacts`. /// /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. - pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { + pub fn clamp_motions(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts) + where + Bodies: ComponentSet + + ComponentSetMut + + ComponentSet + + ComponentSet, + { match impacts { PredictedImpacts::Impacts(tois) => { // println!("Num to clamp: {}", tois.len()); for (handle, toi) in tois { - if let Some(body) = bodies.get_mut_internal(*handle) { - let min_toi = (body.ccd_thickness - * 0.15 - * crate::utils::inv(body.max_point_velocity())) - .min(dt); - // println!("Min toi: {}, Toi: {}", min_toi, toi); - body.integrate_next_position(toi.max(min_toi)); - } + let (rb_poss, vels, ccd, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyCcd, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + let local_com = &mprops.mass_properties.local_com; + + let min_toi = (ccd.ccd_thickness + * 0.15 + * crate::utils::inv(ccd.max_point_velocity(vels))) + .min(dt); + // println!("Min toi: {}, Toi: {}", min_toi, toi); + let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com); + bodies.map_mut_internal(handle.0, |rb_poss| { + rb_poss.next_position = new_pos; + }); } } _ => {} @@ -66,34 +87,64 @@ impl CCDSolver { /// Updates the set of bodies that needs CCD to be resolved. /// /// Returns `true` if any rigid-body must have CCD resolved. - pub fn update_ccd_active_flags( + pub fn update_ccd_active_flags( &self, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, dt: Real, include_forces: bool, - ) -> bool { + ) -> bool + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSet, + { let mut ccd_active = false; // println!("Checking CCD activation"); - bodies.foreach_active_dynamic_body_mut_internal(|_, body| { - body.update_ccd_active_flag(dt, include_forces); - // println!("CCD is active: {}, for {:?}", ccd_active, handle); - ccd_active = ccd_active || body.is_ccd_active(); - }); + for handle in islands.active_dynamic_bodies() { + let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) = + bodies.index_bundle(handle.0); + + if ccd.ccd_enabled { + let forces = if include_forces { Some(forces) } else { None }; + let moving_fast = ccd.is_moving_fast(dt, vels, forces); + + bodies.map_mut_internal(handle.0, |ccd| { + ccd.ccd_active = moving_fast; + }); + + ccd_active = ccd_active || moving_fast; + } + } ccd_active } /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider. - pub fn find_first_impact( + pub fn find_first_impact( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, - ) -> Option { + ) -> Option + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -102,19 +153,37 @@ impl CCDSolver { let mut pairs_seen = HashMap::default(); let mut min_toi = dt; - for (_, rb1) in bodies.iter_active_dynamic() { - if rb1.is_ccd_active() { - let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt); - - for ch1 in &rb1.colliders { - let co1 = &colliders[*ch1]; - - if co1.is_sensor() { + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + + if rb_ccd1.ccd_active { + let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + &RigidBodyColliders, + ) = bodies.index_bundle(handle.0); + + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); + + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .expect("Could not find the ColliderParent component."); + let (co_shape1, co_pos1, co_type1): ( + &ColliderShape, + &ColliderPosition, + &ColliderType, + ) = colliders.index_bundle(ch1.0); + + if co_type1.is_sensor() { continue; // Ignore sensors. } - let aabb1 = - co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent())); + let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; + let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -130,12 +199,17 @@ impl CCDSolver { ) .is_none() { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let co_type1: &ColliderType = colliders.index(ch1.0); + let co_type2: &ColliderType = colliders.index(ch1.0); - if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) { // Ignore self-intersection and sensors. return true; } @@ -146,16 +220,15 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + let b2 = bh2.map(|h| bodies.index_bundle(h.0)); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - c1, - c2, - b1, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), b2, None, None, @@ -181,14 +254,27 @@ impl CCDSolver { } /// Outputs the set of bodies as well as their first time-of-impact event. - pub fn predict_impacts_at_next_positions( + pub fn predict_impacts_at_next_positions( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, events: &dyn EventHandler, - ) -> PredictedImpacts { + ) -> PredictedImpacts + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); let mut pairs_seen = HashMap::default(); @@ -196,6 +282,7 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, @@ -207,71 +294,94 @@ impl CCDSolver { * */ // TODO: don't iterate through all the colliders. - for (ch1, co1) in colliders.iter() { - let rb1 = &bodies[co1.parent()]; - if rb1.is_ccd_active() { - let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); - - self.query_pipeline - .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - if ch1 == *ch2 { - // Ignore self-intersection. - return true; - } - - if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) - .is_none() - { - let c1 = colliders.get(ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + + if rb_ccd1.ccd_active { + let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + &RigidBodyColliders, + ) = bodies.index_bundle(handle.0); + + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); + + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .expect("Could not find the ColliderParent component."); + let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = + colliders.index_bundle(ch1.0); + + let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; + let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); - if bh1 == bh2 { + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { + if *ch1 == *ch2 { // Ignore self-intersection. return true; } - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); - - let smallest_dist = narrow_phase - .contact_pair(ch1, *ch2) - .and_then(|p| p.find_deepest_contact()) - .map(|c| c.1.dist) - .unwrap_or(0.0); - - if let Some(toi) = TOIEntry::try_from_colliders( - self.query_pipeline.query_dispatcher(), - ch1, - *ch2, - c1, - c2, - b1, - b2, - None, - None, - 0.0, - // NOTE: we use dt here only once we know that - // there is at least one TOI before dt. - min_overstep, - smallest_dist, - ) { - if toi.toi > dt { - min_overstep = min_overstep.min(toi.toi); - } else { - min_overstep = dt; - all_toi.push(toi); + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + let smallest_dist = narrow_phase + .contact_pair(*ch1, *ch2) + .and_then(|p| p.find_deepest_contact()) + .map(|c| c.1.dist) + .unwrap_or(0.0); + + let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + *ch1, + *ch2, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), + b2, + None, + None, + 0.0, + // NOTE: we use dt here only once we know that + // there is at least one TOI before dt. + min_overstep, + smallest_dist, + ) { + if toi.toi > dt { + min_overstep = min_overstep.min(toi.toi); + } else { + min_overstep = dt; + all_toi.push(toi); + } } } - } - true - }); + true + }); + } } } @@ -293,19 +403,25 @@ impl CCDSolver { while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); - let body1 = bodies.get(toi.b1).unwrap(); - let body2 = bodies.get(toi.b2).unwrap(); + let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> = + toi.b1.map(|b| bodies.index_bundle(b.0)); + let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> = + toi.b2.map(|b| bodies.index_bundle(b.0)); let mut colliders_to_check = Vec::new(); - let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1); - let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2); + let should_freeze1 = rb1.is_some() + && rb1.unwrap().0.ccd_active + && !frozen.contains_key(&toi.b1.unwrap()); + let should_freeze2 = rb2.is_some() + && rb2.unwrap().0.ccd_active + && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { continue; } if toi.is_intersection_test { - // NOTE: this test is rendundant with the previous `if !should_freeze && ...` + // NOTE: this test is redundant with the previous `if !should_freeze && ...` // but let's keep it to avoid tricky regressions if we end up swapping both // `if` for some reasons in the future. if should_freeze1 || should_freeze2 { @@ -318,42 +434,51 @@ impl CCDSolver { } if should_freeze1 { - let _ = frozen.insert(toi.b1, toi.toi); - colliders_to_check.extend_from_slice(&body1.colliders); + let _ = frozen.insert(toi.b1.unwrap(), toi.toi); + colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0); } if should_freeze2 { - let _ = frozen.insert(toi.b2, toi.toi); - colliders_to_check.extend_from_slice(&body2.colliders); + let _ = frozen.insert(toi.b2.unwrap(), toi.toi); + colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0); } let start_time = toi.toi; for ch1 in &colliders_to_check { - let co1 = &colliders[*ch1]; - let rb1 = &bodies[co1.parent]; - let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap(); + let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = + colliders.index_bundle(ch1.0); + + let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0); + let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent; + let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); if bh1 == bh2 { // Ignore self-intersection. return true; } - let frozen1 = frozen.get(&bh1); - let frozen2 = frozen.get(&bh2); + let frozen1 = bh1.and_then(|h| frozen.get(&h)); + let frozen2 = bh2.and_then(|h| frozen.get(&h)); - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + let b1: Option<(_, _, _, &RigidBodyCcd)> = + bh1.map(|h| bodies.index_bundle(h.0)); + let b2: Option<(_, _, _, &RigidBodyCcd)> = + bh1.map(|h| bodies.index_bundle(h.0)); - if (frozen1.is_some() || !b1.is_ccd_active()) - && (frozen2.is_some() || !b2.is_ccd_active()) + if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false)) + && (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false)) { // We already did a resweep. return true; @@ -369,8 +494,8 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - c1, - c2, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), b1, b2, frozen1.copied(), @@ -395,30 +520,57 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it wasn't intersecting // before, then we need to generate one interaction-start and one interaction-stop // events because it will never be detected by the narrow-phase because of tunneling. - let body1 = &bodies[toi.b1]; - let body2 = &bodies[toi.b2]; - let co1 = &colliders[toi.c1]; - let co2 = &colliders[toi.c2]; - let frozen1 = frozen.get(&toi.b1); - let frozen2 = frozen.get(&toi.b2); - let pos1 = frozen1 - .map(|t| body1.integrate_velocity(*t)) - .unwrap_or(body1.next_position); - let pos2 = frozen2 - .map(|t| body2.integrate_velocity(*t)) - .unwrap_or(body2.next_position); - - let prev_coll_pos12 = co1.position.inv_mul(&co2.position); - let next_coll_pos12 = - (pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent()); + let (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(toi.c1.0); + let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(toi.c2.0); + + let co_next_pos1 = if let Some(b1) = toi.b1 { + let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); + let (rb_pos1, rb_vels1, rb_mprops1): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + ) = bodies.index_bundle(b1.0); + + let local_com1 = &rb_mprops1.mass_properties.local_com; + let frozen1 = frozen.get(&b1); + let pos1 = frozen1 + .map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1)) + .unwrap_or(rb_pos1.next_position); + pos1 * co_parent1.pos_wrt_parent + } else { + co_pos1.0 + }; + + let co_next_pos2 = if let Some(b2) = toi.b2 { + let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap(); + let (rb_pos2, rb_vels2, rb_mprops2): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + ) = bodies.index_bundle(b2.0); + + let local_com2 = &rb_mprops2.mass_properties.local_com; + let frozen2 = frozen.get(&b2); + let pos2 = frozen2 + .map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2)) + .unwrap_or(rb_pos2.next_position); + pos2 * co_parent2.pos_wrt_parent + } else { + co_pos2.0 + }; + + let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2); + let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2); let query_dispatcher = self.query_pipeline.query_dispatcher(); let intersect_before = query_dispatcher - .intersection_test(&prev_coll_pos12, co1.shape(), co2.shape()) + .intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) .unwrap_or(false); let intersect_after = query_dispatcher - .intersection_test(&next_coll_pos12, co1.shape(), co2.shape()) + .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) .unwrap_or(false); if !intersect_before && !intersect_after { diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index f1066e0..4637940 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,5 +1,9 @@ -use crate::dynamics::{RigidBody, RigidBodyHandle}; -use crate::geometry::{Collider, ColliderHandle}; +use crate::dynamics::{ + RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::{ + ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, +}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher}; pub struct TOIEntry { pub toi: Real, pub c1: ColliderHandle, - pub b1: RigidBodyHandle, + pub b1: Option, pub c2: ColliderHandle, - pub b2: RigidBodyHandle, + pub b2: Option, pub is_intersection_test: bool, pub timestamp: usize, } @@ -18,9 +22,9 @@ impl TOIEntry { fn new( toi: Real, c1: ColliderHandle, - b1: RigidBodyHandle, + b1: Option, c2: ColliderHandle, - b2: RigidBodyHandle, + b2: Option, is_intersection_test: bool, timestamp: usize, ) -> Self { @@ -39,10 +43,30 @@ impl TOIEntry { query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, - c1: &Collider, - c2: &Collider, - b1: &RigidBody, - b2: &RigidBody, + c1: ( + &ColliderType, + &ColliderShape, + &ColliderPosition, + Option<&ColliderParent>, + ), + c2: ( + &ColliderType, + &ColliderShape, + &ColliderPosition, + Option<&ColliderParent>, + ), + b1: Option<( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + )>, + b2: Option<( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + )>, frozen1: Option, frozen2: Option, start_time: Real, @@ -50,35 +74,46 @@ impl TOIEntry { smallest_contact_dist: Real, ) -> Option { assert!(start_time <= end_time); + if b1.is_none() && b2.is_none() { + return None; + } + + let (co_type1, co_shape1, co_pos1, co_parent1) = c1; + let (co_type2, co_shape2, co_pos2, co_parent2) = c2; - let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel(); - let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel(); - let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel(); - let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel(); + let linvel1 = + frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); + let linvel2 = + frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero()); + let angvel1 = + frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero()); + let angvel2 = + frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.abs() * b1.ccd_max_dist - + angvel2.abs() * b2.ccd_max_dist; + + angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) + + angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); #[cfg(feature = "dim3")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.norm() * b1.ccd_max_dist - + angvel2.norm() * b2.ccd_max_dist; + + angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) + + angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); // We may be slightly over-conservative by taking the `max(0.0)` here. // But removing the `max` doesn't really affect performances so let's // keep it since more conservatism is good at this stage. - let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()) + let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) + smallest_contact_dist.max(0.0); - let is_intersection_test = c1.is_sensor() || c2.is_sensor(); + let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor(); if (end_time - start_time) * vel12 < thickness { return None; } // Compute the TOI. - let mut motion1 = Self::body_motion(b1); - let mut motion2 = Self::body_motion(b2); + let identity = NonlinearRigidMotion::identity(); + let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity); + let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity); if let Some(t) = frozen1 { motion1.freeze(t); @@ -88,8 +123,8 @@ impl TOIEntry { motion2.freeze(t); } - let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); - let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); + let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0)); + let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0)); // println!("start_time: {}", start_time); @@ -105,9 +140,9 @@ impl TOIEntry { let res_toi = query_dispatcher .nonlinear_time_of_impact( &motion_c1, - c1.shape(), + co_shape1.as_ref(), &motion_c2, - c2.shape(), + co_shape2.as_ref(), start_time, end_time, stop_at_penetration, @@ -119,24 +154,31 @@ impl TOIEntry { Some(Self::new( toi.toi, ch1, - c1.parent(), + co_parent1.map(|p| p.handle), ch2, - c2.parent(), + co_parent2.map(|p| p.handle), is_intersection_test, 0, )) } - fn body_motion(body: &RigidBody) -> NonlinearRigidMotion { - if body.is_ccd_active() { + fn body_motion( + (poss, vels, mprops, ccd): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + ), + ) -> NonlinearRigidMotion { + if ccd.ccd_active { NonlinearRigidMotion::new( - body.position, - body.mass_properties.local_com, - body.linvel, - body.angvel, + poss.position, + mprops.mass_properties.local_com, + vels.linvel, + vels.angvel, ) } else { - NonlinearRigidMotion::constant_position(body.next_position) + NonlinearRigidMotion::constant_position(poss.next_position) } } } diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 9b3b9ee..1016d09 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -20,17 +20,13 @@ pub enum CoefficientCombineRule { Max, } -impl CoefficientCombineRule { - pub(crate) fn from_value(val: u8) -> Self { - match val { - 0 => CoefficientCombineRule::Average, - 1 => CoefficientCombineRule::Min, - 2 => CoefficientCombineRule::Multiply, - 3 => CoefficientCombineRule::Max, - _ => panic!("Invalid coefficient combine rule."), - } +impl Default for CoefficientCombineRule { + fn default() -> Self { + CoefficientCombineRule::Average } +} +impl CoefficientCombineRule { pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real { let effective_rule = rule_value1.max(rule_value2); diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs new file mode 100644 index 0000000..c29609f --- /dev/null +++ b/src/dynamics/island_manager.rs @@ -0,0 +1,344 @@ +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{ + Joint, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodyType, + RigidBodyVelocity, +}; +use crate::geometry::{ColliderParent, InteractionGraph, NarrowPhase}; +use crate::math::Real; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct IslandManager { + pub(crate) active_dynamic_set: Vec, + pub(crate) active_kinematic_set: Vec, + pub(crate) active_islands: Vec, + active_set_timestamp: u32, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + can_sleep: Vec, // Workspace. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + stack: Vec, // Workspace. +} + +impl IslandManager { + pub fn new() -> Self { + Self { + active_dynamic_set: vec![], + active_kinematic_set: vec![], + active_islands: vec![], + active_set_timestamp: 0, + can_sleep: vec![], + stack: vec![], + } + } + + pub(crate) fn num_islands(&self) -> usize { + self.active_islands.len() - 1 + } + + pub fn cleanup_removed_rigid_bodies( + &mut self, + bodies: &mut impl ComponentSetMut, + ) { + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + let mut i = 0; + + while i < active_set.len() { + let handle = active_set[i]; + if bodies.get(handle.0).is_none() { + // This rigid-body no longer exists, so we need to remove it from the active set. + active_set.swap_remove(i); + + if i < active_set.len() { + bodies.map_mut_internal(active_set[i].0, |rb_ids| rb_ids.active_set_id = i); + } + } else { + i += 1; + } + } + } + } + + pub fn rigid_body_removed( + &mut self, + removed_handle: RigidBodyHandle, + removed_ids: &RigidBodyIds, + bodies: &mut impl ComponentSetMut, + ) { + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { + active_set.swap_remove(removed_ids.active_set_id); + + if let Some(replacement) = active_set.get(removed_ids.active_set_id) { + bodies.map_mut_internal(replacement.0, |ids| { + ids.active_set_id = removed_ids.active_set_id; + }); + } + } + } + } + + /// Forces the specified rigid-body to wake up if it is dynamic. + /// + /// If `strong` is `true` then it is assured that the rigid-body will + /// remain awake during multiple subsequent timesteps. + pub fn wake_up(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool) + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { + // TODO: what about kinematic bodies? + let status: RigidBodyType = *bodies.index(handle.0); + if status.is_dynamic() { + bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { + activation.wake_up(strong) + }); + bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { + if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) { + ids.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } + }); + } + } + + /// Iter through all the active kinematic rigid-bodies on this set. + pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] { + &self.active_kinematic_set[..] + } + + /// Iter through all the active dynamic rigid-bodies on this set. + pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] { + &self.active_dynamic_set[..] + } + + #[cfg(not(feature = "parallel"))] + pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + &self.active_dynamic_set[island_range] + } + + #[inline(always)] + pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator + 'a { + self.active_dynamic_set + .iter() + .copied() + .chain(self.active_kinematic_set.iter().copied()) + } + + /* + #[cfg(feature = "parallel")] + #[inline(always)] + #[allow(dead_code)] + pub(crate) fn foreach_active_island_body_mut_internal_parallel( + &self, + island_id: usize, + bodies: &mut Set, + f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, + ) where + Set: ComponentSet, + { + 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 bodies as *mut _); + self.active_dynamic_set[island_range] + .par_iter() + .for_each_init( + || bodies.load(Ordering::Relaxed), + |bodies, handle| { + let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) }; + if let Some(rb) = bodies.get_mut_internal(handle.0) { + f(*handle, rb) + } + }, + ); + } + */ + + #[cfg(feature = "parallel")] + 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 update_active_set_with_contacts( + &mut self, + bodies: &mut Bodies, + colliders: &Colliders, + narrow_phase: &NarrowPhase, + joint_graph: &InteractionGraph, + min_island_size: usize, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + +