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/dynamics/island_manager.rs | 344 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 344 insertions(+) create mode 100644 src/dynamics/island_manager.rs (limited to 'src/dynamics/island_manager.rs') 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 + + ComponentSet, + Colliders: ComponentSetOption, + { + assert!( + min_island_size > 0, + "The minimum island size must be at least 1." + ); + + // Update the energy of every rigid body and + // keep only those that may not sleep. + // let t = instant::now(); + self.active_set_timestamp += 1; + self.stack.clear(); + self.can_sleep.clear(); + + // NOTE: the `.rev()` is here so that two successive timesteps preserve + // the order of the bodies in the `active_dynamic_set` vec. This reversal + // does not seem to affect performances nor stability. However it makes + // debugging slightly nicer so we keep this rev. + for h in self.active_dynamic_set.drain(..).rev() { + let can_sleep = &mut self.can_sleep; + let stack = &mut self.stack; + + let vels: &RigidBodyVelocity = bodies.index(h.0); + let pseudo_kinetic_energy = vels.pseudo_kinetic_energy(); + + bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { + update_energy(activation, pseudo_kinetic_energy); + + if activation.energy <= activation.threshold { + // Mark them as sleeping for now. This will + // be set to false during the graph traversal + // if it should not be put to sleep. + activation.sleeping = true; + can_sleep.push(h); + } else { + stack.push(h); + } + }); + } + + // Read all the contacts and push objects touching touching this rigid-body. + #[inline(always)] + fn push_contacting_bodies( + rb_colliders: &RigidBodyColliders, + colliders: &impl ComponentSetOption, + narrow_phase: &NarrowPhase, + stack: &mut Vec, + ) { + for collider_handle in &rb_colliders.0 { + if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { + for inter in contacts { + for manifold in &inter.2.manifolds { + if !manifold.data.solver_contacts.is_empty() { + let other = crate::utils::select_other( + (inter.0, inter.1), + *collider_handle, + ); + if let Some(other_body) = colliders.get(other.0) { + stack.push(other_body.handle); + } + break; + } + } + } + } + } + } + + // Now iterate on all active kinematic bodies and push all the bodies + // touching them to the stack so they can be woken up. + for h in self.active_kinematic_set.iter() { + let (vels, rb_colliders): (&RigidBodyVelocity, _) = bodies.index_bundle(h.0); + + if vels.is_zero() { + // If the kinematic body does not move, it does not have + // to wake up any dynamic body. + continue; + } + + push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + } + + // println!("Selection: {}", instant::now() - t); + + // let t = instant::now(); + // Propagation of awake state and awake island computation through the + // traversal of the interaction graph. + self.active_islands.clear(); + self.active_islands.push(0); + + // The max avoid underflow when the stack is empty. + let mut island_marker = self.stack.len().max(1) - 1; + + while let Some(handle) = self.stack.pop() { + let (rb_status, rb_ids, rb_colliders): ( + &RigidBodyType, + &RigidBodyIds, + &RigidBodyColliders, + ) = bodies.index_bundle(handle.0); + + if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() { + // We already visited this body and its neighbors. + // Also, we don't propagate awake state through static bodies. + continue; + } + + if self.stack.len() < island_marker { + if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size + { + // We are starting a new island. + self.active_islands.push(self.active_dynamic_set.len()); + } + + island_marker = self.stack.len(); + } + + // Transmit the active state to all the rigid-bodies with colliders + // in contact or joined with this collider. + push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + + for inter in joint_graph.interactions_with(rb_ids.joint_graph_index) { + let other = crate::utils::select_other((inter.0, inter.1), handle); + self.stack.push(other); + } + + bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { + activation.wake_up(false); + }); + bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { + ids.active_island_id = self.active_islands.len() - 1; + ids.active_set_id = self.active_dynamic_set.len(); + ids.active_set_offset = + ids.active_set_id - self.active_islands[ids.active_island_id]; + ids.active_set_timestamp = self.active_set_timestamp; + }); + + self.active_dynamic_set.push(handle); + } + + self.active_islands.push(self.active_dynamic_set.len()); + // println!( + // "Extraction: {}, num islands: {}", + // instant::now() - t, + // self.active_islands.len() - 1 + // ); + + // Actually put to sleep bodies which have not been detected as awake. + for h in &self.can_sleep { + let activation: &RigidBodyActivation = bodies.index(h.0); + if activation.sleeping { + bodies.set_internal(h.0, RigidBodyVelocity::zero()); + bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { + activation.sleep() + }); + } + } + } +} + +fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) { + let mix_factor = 0.01; + let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy; + activation.energy = new_energy.min(activation.threshold.abs() * 4.0); +} -- cgit