diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index ce94a1381..4539e8d5b 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -43,6 +43,7 @@ serde-serialize = [ "dep:serde", "bit-vec/serde", "arrayvec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index a1bd0320f..6ecb5d3e0 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -44,6 +44,7 @@ serde-serialize = [ "dep:serde", "bit-vec/serde", "arrayvec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 0600a8c51..7128fe510 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -46,6 +46,7 @@ serde-serialize = [ "parry3d-f64/serde-serialize", "dep:serde", "bit-vec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 22bfb46e6..db4ad303e 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -48,6 +48,7 @@ serde-serialize = [ "parry3d/serde-serialize", "dep:serde", "bit-vec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"] debug-render = [] diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 0d005c488..158d1262a 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) { } for handle in physics.islands.active_bodies() { - let body = &mut physics.bodies[*handle]; + let body = &mut physics.bodies[handle]; if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index f9f7cb05b..5f7dc6898 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) { } for handle in physics.islands.active_bodies() { - let body = physics.bodies.get_mut(*handle).unwrap(); + let body = physics.bodies.get_mut(handle).unwrap(); if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs index 1d2d28fa8..43730c570 100644 --- a/src/counters/collision_detection_counters.rs +++ b/src/counters/collision_detection_counters.rs @@ -8,6 +8,9 @@ pub struct CollisionDetectionCounters { pub ncontact_pairs: usize, /// Time spent for the broad-phase of the collision detection. pub broad_phase_time: Timer, + /// Time spent by the final broad-phase AABB update after body movement to keep + /// user scene queries valid. + pub final_broad_phase_time: Timer, /// Time spent for the narrow-phase of the collision detection. pub narrow_phase_time: Timer, } @@ -18,6 +21,7 @@ impl CollisionDetectionCounters { CollisionDetectionCounters { ncontact_pairs: 0, broad_phase_time: Timer::new(), + final_broad_phase_time: Timer::new(), narrow_phase_time: Timer::new(), } } @@ -26,6 +30,7 @@ impl CollisionDetectionCounters { pub fn reset(&mut self) { self.ncontact_pairs = 0; self.broad_phase_time.reset(); + self.final_broad_phase_time.reset(); self.narrow_phase_time.reset(); } } @@ -34,6 +39,7 @@ impl Display for CollisionDetectionCounters { fn fmt(&self, f: &mut Formatter) -> Result { writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?; writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?; + writeln!(f, "Final broad-phase time: {}", self.final_broad_phase_time)?; writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time) } } diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs index aced5f396..19b721f78 100644 --- a/src/counters/solver_counters.rs +++ b/src/counters/solver_counters.rs @@ -12,6 +12,10 @@ pub struct SolverCounters { pub velocity_resolution_time: Timer, /// Time spent for the assembly of all the velocity constraints. pub velocity_assembly_time: Timer, + /// Time spent by the velocity assembly for initializing solver bodies. + pub velocity_assembly_time_solver_bodies: Timer, + /// Time spent by the velocity assemble for initializing the constraints. + pub velocity_assembly_time_constraints_init: Timer, /// Time spent for the update of the velocity of the bodies. pub velocity_update_time: Timer, /// Time spent to write force back to user-accessible data. @@ -25,6 +29,8 @@ impl SolverCounters { nconstraints: 0, ncontacts: 0, velocity_assembly_time: Timer::new(), + velocity_assembly_time_solver_bodies: Timer::new(), + velocity_assembly_time_constraints_init: Timer::new(), velocity_resolution_time: Timer::new(), velocity_update_time: Timer::new(), velocity_writeback_time: Timer::new(), @@ -37,6 +43,8 @@ impl SolverCounters { self.ncontacts = 0; self.velocity_resolution_time.reset(); self.velocity_assembly_time.reset(); + self.velocity_assembly_time_solver_bodies.reset(); + self.velocity_assembly_time_constraints_init.reset(); self.velocity_update_time.reset(); self.velocity_writeback_time.reset(); } diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs index a3a1841fd..eaaf2516a 100644 --- a/src/counters/stages_counters.rs +++ b/src/counters/stages_counters.rs @@ -10,6 +10,8 @@ pub struct StagesCounters { pub collision_detection_time: Timer, /// Time spent for the computation of collision island and body activation/deactivation (sleeping). pub island_construction_time: Timer, + /// Time spent for collecting awake constraints from islands. + pub island_constraints_collection_time: Timer, /// Total time spent for the constraints resolution and position update.t pub solver_time: Timer, /// Total time spent for CCD and CCD resolution. @@ -25,6 +27,7 @@ impl StagesCounters { update_time: Timer::new(), collision_detection_time: Timer::new(), island_construction_time: Timer::new(), + island_constraints_collection_time: Timer::new(), solver_time: Timer::new(), ccd_time: Timer::new(), user_changes: Timer::new(), @@ -36,6 +39,7 @@ impl StagesCounters { self.update_time.reset(); self.collision_detection_time.reset(); self.island_construction_time.reset(); + self.island_constraints_collection_time.reset(); self.solver_time.reset(); self.ccd_time.reset(); self.user_changes.reset(); @@ -55,6 +59,11 @@ impl Display for StagesCounters { "Island construction time: {}", self.island_construction_time )?; + writeln!( + f, + "Island construction time: {}", + self.island_constraints_collection_time + )?; writeln!(f, "Solver time: {}", self.solver_time)?; writeln!(f, "CCD time: {}", self.ccd_time)?; writeln!(f, "User changes time: {}", self.user_changes) diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 376373023..52b8e9376 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -88,7 +88,7 @@ impl CCDSolver { // println!("Checking CCD activation"); for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); if rb.ccd.ccd_enabled { let forces = if include_forces { @@ -143,7 +143,7 @@ impl CCDSolver { let mut min_toi = dt; for handle in islands.active_bodies() { - let rb1 = &bodies[*handle]; + let rb1 = &bodies[handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( @@ -276,7 +276,7 @@ impl CCDSolver { */ // TODO: don't iterate through all the colliders. for handle in islands.active_bodies() { - let rb1 = &bodies[*handle]; + let rb1 = &bodies[handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs deleted file mode 100644 index d580ef589..000000000 --- a/src/dynamics/island_manager.rs +++ /dev/null @@ -1,357 +0,0 @@ -use crate::dynamics::{ - ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, - RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, -}; -use crate::geometry::{ColliderSet, NarrowPhase}; -use crate::math::Real; -use crate::utils::SimdDot; - -/// System that manages which bodies are active (awake) vs sleeping to optimize performance. -/// -/// ## Sleeping Optimization -/// -/// Bodies at rest automatically "sleep" - they're excluded from simulation until something -/// disturbs them (collision, joint connection to moving body, manual wake-up). This can -/// dramatically improve performance in scenes with many static/resting objects. -/// -/// ## Islands -/// -/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. -/// This allows parallel solving and better organization. -/// -/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Default)] -pub struct IslandManager { - pub(crate) active_set: Vec, - pub(crate) active_islands: Vec, - pub(crate) active_islands_additional_solver_iterations: 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 { - /// Creates a new empty island manager. - pub fn new() -> Self { - Self { - active_set: vec![], - active_islands: vec![], - active_islands_additional_solver_iterations: vec![], - active_set_timestamp: 0, - can_sleep: vec![], - stack: vec![], - } - } - - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len().saturating_sub(1) - } - - /// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`. - pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) { - let mut i = 0; - - while i < self.active_set.len() { - let handle = self.active_set[i]; - if bodies.get(handle).is_none() { - // This rigid-body no longer exists, so we need to remove it from the active set. - self.active_set.swap_remove(i); - - if i < self.active_set.len() { - // Update the self.active_set_id for the body that has been swapped. - if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) { - swapped_rb.ids.active_set_id = i; - } - } - } else { - i += 1; - } - } - } - - pub(crate) fn rigid_body_removed( - &mut self, - removed_handle: RigidBodyHandle, - removed_ids: &RigidBodyIds, - bodies: &mut RigidBodySet, - ) { - if self.active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { - self.active_set.swap_remove(removed_ids.active_set_id); - - if let Some(replacement) = self - .active_set - .get(removed_ids.active_set_id) - .and_then(|h| bodies.get_mut_internal(*h)) - { - replacement.ids.active_set_id = removed_ids.active_set_id; - } - } - } - - /// Wakes up a sleeping body, forcing it back into the active simulation. - /// - /// Use this when you want to ensure a body is active (useful after manually moving - /// a sleeping body, or to prevent it from sleeping in the next few frames). - /// - /// # Parameters - /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. - /// If `false`, it might sleep again immediately if conditions are met. - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// # let mut bodies = RigidBodySet::new(); - /// # let mut islands = IslandManager::new(); - /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); - /// islands.wake_up(&mut bodies, body_handle, true); - /// let body = bodies.get_mut(body_handle).unwrap(); - /// // Wake up a body before applying force to it - /// body.add_force(vector![100.0, 0.0, 0.0], false); - /// ``` - /// - /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). - pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { - // NOTE: the use an Option here because there are many legitimate cases (like when - // deleting a joint attached to an already-removed body) where we could be - // attempting to wake-up a rigid-body that has already been deleted. - if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { - let rb = bodies.index_mut_internal(handle); - - // Check that the user didn’t change the sleeping state explicitly, in which - // case we don’t overwrite it. - if !rb.changes.contains(RigidBodyChanges::SLEEP) { - rb.activation.wake_up(strong); - - if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) { - rb.ids.active_set_id = self.active_set.len(); - self.active_set.push(handle); - } - } - } - } - - 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_set[island_range] - } - - pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { - self.active_islands_additional_solver_iterations[island_id] - } - - /// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). - #[inline] - pub fn active_bodies(&self) -> &[RigidBodyHandle] { - &self.active_set - } - - #[cfg(feature = "parallel")] - #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. - 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, - dt: Real, - length_unit: Real, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - min_island_size: usize, - ) { - 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_set` vec. This reversal - // does not seem to affect performances nor stability. However it makes - // debugging slightly nicer. - for h in self.active_set.drain(..).rev() { - let can_sleep = &mut self.can_sleep; - let stack = &mut self.stack; - - let rb = bodies.index_mut_internal(h); - let sq_linvel = rb.vels.linvel.norm_squared(); - let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - - update_energy( - &mut rb.activation, - rb.body_type, - length_unit, - sq_linvel, - sq_angvel, - dt, - ); - - if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep { - // Mark them as sleeping for now. This will - // be set to false during the graph traversal - // if it should not be put to sleep. - rb.activation.sleeping = true; - can_sleep.push(h); - } else { - stack.push(h); - } - } - - // Read all the contacts and push objects touching touching this rigid-body. - #[inline] - fn push_contacting_bodies( - rb_colliders: &RigidBodyColliders, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - stack: &mut Vec, - ) { - for collider_handle in &rb_colliders.0 { - for inter in narrow_phase.contact_pairs_with(*collider_handle) { - for manifold in &inter.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.collider1, inter.collider2), - *collider_handle, - ); - if let Some(other_body) = colliders[other].parent { - stack.push(other_body.handle); - } - break; - } - } - } - } - } - - // 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_additional_solver_iterations.clear(); - self.active_islands.clear(); - self.active_islands.push(0); - - // saturating_sub(1) prevents underflow when the stack is empty. - let mut island_marker = self.stack.len().saturating_sub(1); - - // NOTE: islands containing a body with non-standard number of iterations won’t - // be merged with another island, unless another island with standard - // iterations number already started before and got continued due to the - // `min_island_size`. That could be avoided by pushing bodies with non-standard - // iterations on top of the stack (and other bodies on the back). Not sure it’s - // worth it though. - let mut additional_solver_iterations = 0; - - while let Some(handle) = self.stack.pop() { - let rb = bodies.index_mut_internal(handle); - - if rb.ids.active_set_timestamp == self.active_set_timestamp - || !rb.is_dynamic_or_kinematic() - { - // We already visited this body and its neighbors. - // Also, we don't propagate awake state through fixed bodies. - continue; - } - - if self.stack.len() < island_marker { - if additional_solver_iterations != rb.additional_solver_iterations - || self.active_set.len() - *self.active_islands.last().unwrap() - >= min_island_size - { - // We are starting a new island. - self.active_islands_additional_solver_iterations - .push(additional_solver_iterations); - self.active_islands.push(self.active_set.len()); - additional_solver_iterations = 0; - } - - island_marker = self.stack.len(); - } - - additional_solver_iterations = - additional_solver_iterations.max(rb.additional_solver_iterations); - - // 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 impulse_joints.attached_enabled_joints(handle) { - let other = crate::utils::select_other((inter.0, inter.1), handle); - self.stack.push(other); - } - - for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { - self.stack.push(other); - } - - rb.activation.wake_up(false); - rb.ids.active_island_id = self.active_islands.len() - 1; - rb.ids.active_set_id = self.active_set.len(); - rb.ids.active_set_offset = - (rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]) as u32; - rb.ids.active_set_timestamp = self.active_set_timestamp; - - self.active_set.push(handle); - } - - self.active_islands_additional_solver_iterations - .push(additional_solver_iterations); - self.active_islands.push(self.active_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 handle in &self.can_sleep { - let rb = bodies.index_mut_internal(*handle); - if rb.activation.sleeping { - rb.vels = RigidBodyVelocity::zero(); - rb.activation.sleep(); - } - } - } -} - -fn update_energy( - activation: &mut RigidBodyActivation, - body_type: RigidBodyType, - length_unit: Real, - sq_linvel: Real, - sq_angvel: Real, - dt: Real, -) { - let can_sleep = match body_type { - RigidBodyType::Dynamic => { - let linear_threshold = activation.normalized_linear_threshold * length_unit; - sq_linvel < linear_threshold * linear_threshold.abs() - && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() - } - RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { - // Platforms only sleep if both velocities are exactly zero. If it’s not exactly - // zero, then the user really wants them to move. - sq_linvel == 0.0 && sq_angvel == 0.0 - } - RigidBodyType::Fixed => true, - }; - - if can_sleep { - activation.time_since_can_sleep += dt; - } else { - activation.time_since_can_sleep = 0.0; - } -} diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs new file mode 100644 index 000000000..f9cd27b53 --- /dev/null +++ b/src/dynamics/island_manager/island.rs @@ -0,0 +1,163 @@ +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; + +use super::IslandManager; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +pub(crate) struct Island { + /// The rigid-bodies part of this island. + pub(super) bodies: Vec, + /// The additional solver iterations needed by this island. + pub(super) additional_solver_iterations: usize, + /// Index of this island in `IslandManager::awake_islands`. + /// + /// If `None`, the island is sleeping. + pub(super) id_in_awake_list: Option, +} + +impl Island { + pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self { + Self { + bodies: vec![handle], + additional_solver_iterations: rb.additional_solver_iterations, + id_in_awake_list: None, + } + } + + pub fn bodies(&self) -> &[RigidBodyHandle] { + &self.bodies + } + + pub fn additional_solver_iterations(&self) -> usize { + self.additional_solver_iterations + } + + pub fn is_sleeping(&self) -> bool { + self.id_in_awake_list.is_none() + } + + pub fn len(&self) -> usize { + self.bodies.len() + } + + pub(crate) fn id_in_awake_list(&self) -> Option { + self.id_in_awake_list + } +} + +impl IslandManager { + /// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and + /// insert `new_island` into the islands set. + /// + /// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`. + pub(super) fn extract_sub_island( + &mut self, + bodies: &mut RigidBodySet, + source_id: usize, + mut new_island: Island, + sleep: bool, + ) { + let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); + let source_island = &mut self.islands[source_id]; + + for (id, handle) in new_island.bodies.iter().enumerate() { + let rb = bodies.index_mut_internal(*handle); + + // If the new island is sleeping, ensure all its bodies are sleeping. + if sleep { + rb.sleep(); + } + + let id_to_remove = rb.ids.active_set_id; + + assert_eq!( + rb.ids.active_island_id, source_id, + "note, new id: {}", + new_island_id + ); + rb.ids.active_island_id = new_island_id; + rb.ids.active_set_id = id; + + new_island.additional_solver_iterations = new_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); + source_island.bodies.swap_remove(id_to_remove); + + if let Some(moved_handle) = source_island.bodies.get(id_to_remove).copied() { + let moved_rb = bodies.index_mut_internal(moved_handle); + moved_rb.ids.active_set_id = id_to_remove; + } + } + + // If the new island is awake, add it to the awake list. + if !sleep { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(new_island_id); + } else { + new_island.id_in_awake_list = None; + } + + self.islands.insert(new_island_id, new_island); + } + + pub(super) fn merge_islands( + &mut self, + bodies: &mut RigidBodySet, + island_id1: usize, + island_id2: usize, + ) { + if island_id1 == island_id2 { + return; + } + + let island1 = &self.islands[island_id1]; + let island2 = &self.islands[island_id2]; + + assert_eq!( + island1.id_in_awake_list.is_some(), + island2.id_in_awake_list.is_some(), + "Internal error: cannot merge two island with different sleeping statuses." + ); + + // Prefer removing the smallest island to reduce the amount of memory to move. + let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() { + (island_id2, island_id1) + } else { + (island_id1, island_id2) + }; + + // println!("Merging: {} <- {}", to_keep, to_remove); + + let Some(removed_island) = self.islands.remove(to_remove) else { + // TODO: the island doesn’t exist is that an internal error? + return; + }; + + self.free_islands.push(to_remove); + + // TODO: if we switched to linked list, we could avoid moving around all this memory. + let target_island = &mut self.islands[to_keep]; + for handle in &removed_island.bodies { + let Some(rb) = bodies.get_mut_internal(*handle) else { + // This body no longer exists. + continue; + }; + rb.wake_up(false); + rb.ids.active_island_id = to_keep; + rb.ids.active_set_id = target_island.bodies.len(); + target_island.bodies.push(*handle); + target_island.additional_solver_iterations = target_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); + } + + // Update the awake_islands list. + if let Some(awake_id_to_remove) = removed_island.id_in_awake_list { + self.awake_islands.swap_remove(awake_id_to_remove); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); + } + } + } +} diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs new file mode 100644 index 000000000..196d5f31b --- /dev/null +++ b/src/dynamics/island_manager/manager.rs @@ -0,0 +1,290 @@ +use super::{Island, IslandsOptimizer}; +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, + RigidBodySet, +}; +use crate::geometry::{ColliderSet, NarrowPhase}; +use crate::math::Real; +use crate::prelude::SleepRootState; +use crate::utils::SimdDot; +use std::collections::VecDeque; +use vec_map::VecMap; + +/// An island starting at this rigid-body might be eligible for sleeping. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct SleepCandidate(RigidBodyHandle); + +/// System that manages which bodies are active (awake) vs sleeping to optimize performance. +/// +/// ## Sleeping Optimization +/// +/// Bodies at rest automatically "sleep" - they're excluded from simulation until something +/// disturbs them (collision, joint connection to moving body, manual wake-up). This can +/// dramatically improve performance in scenes with many static/resting objects. +/// +/// ## Islands +/// +/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. +/// This allows parallel solving and better organization. +/// +/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +pub struct IslandManager { + pub(crate) islands: VecMap, + pub(crate) awake_islands: Vec, + // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation? + pub(crate) free_islands: Vec, + /// Potential candidate roots for graph traversal to identify a sleeping + /// connected component or to split an island in two. + pub(super) traversal_candidates: VecDeque, + pub(super) traversal_timestamp: u32, + pub(super) optimizer: IslandsOptimizer, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(super) stack: Vec, // Workspace. +} + +impl IslandManager { + /// Creates a new empty island manager. + pub fn new() -> Self { + Self::default() + } + + pub(crate) fn active_islands(&self) -> &[usize] { + &self.awake_islands + } + + pub(crate) fn rigid_body_removed( + &mut self, + removed_handle: RigidBodyHandle, + removed_ids: &RigidBodyIds, + bodies: &mut RigidBodySet, + ) { + let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else { + // The island already doesn’t exist. + return; + }; + + let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); + island.bodies.swap_remove(removed_ids.active_set_id); + + // Remap the active_set_id of the body we moved with the `swap_remove`. + if swapped_handle != removed_handle { + let swapped_body = bodies + .get_mut(swapped_handle) + .expect("Internal error: bodies must be removed from islands on at a times"); + swapped_body.ids.active_set_id = removed_ids.active_set_id; + } + + // If we deleted the last body from this island, delete the island. + if island.bodies.is_empty() { + if let Some(awake_id) = island.id_in_awake_list { + // Remove it from the free island list. + self.awake_islands.swap_remove(awake_id); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id); + } + } + self.islands.remove(removed_ids.active_island_id); + self.free_islands.push(removed_ids.active_island_id); + } + } + + pub(crate) fn interaction_started_or_stopped( + &mut self, + bodies: &mut RigidBodySet, + handle1: Option, + handle2: Option, + started: bool, + wake_up: bool, + ) { + match (handle1, handle2) { + (Some(handle1), Some(handle2)) => { + if wake_up { + self.wake_up(bodies, handle1, false); + self.wake_up(bodies, handle2, false); + } + + if started { + if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { + assert!(rb1.is_fixed() || rb1.ids.active_island_id != usize::MAX); + assert!(rb2.is_fixed() || rb2.ids.active_island_id != usize::MAX); + + // If both bodies are not part of the same island, merge the islands. + if !rb1.is_fixed() + && !rb2.is_fixed() + && rb1.ids.active_island_id != rb2.ids.active_island_id + { + self.merge_islands( + bodies, + rb1.ids.active_island_id, + rb2.ids.active_island_id, + ); + } + } + } + } + (Some(handle1), None) => { + if wake_up { + // NOTE: see NOTE of the Some(_), Some(_) case. + self.wake_up(bodies, handle1, false); + } + } + (None, Some(handle2)) => { + if wake_up { + // NOTE: see NOTE of the Some(_), Some(_) case. + self.wake_up(bodies, handle2, false); + } + } + (None, None) => { /* Nothing to do. */ } + } + } + + pub(crate) fn island(&self, island_id: usize) -> &Island { + &self.islands[island_id] + } + + /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). + #[inline] + pub fn active_bodies(&self) -> impl Iterator + '_ { + self.awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) + } + + pub(crate) fn rigid_body_updated( + &mut self, + handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + ) { + let Some(rb) = bodies.get_mut(handle) else { + return; + }; + + if rb.is_fixed() { + return; + } + + // Check if this is the first time we see this rigid-body. + if rb.ids.active_island_id == usize::MAX { + // Check if there is room in the last awake island to add this body. + // NOTE: only checking the last is suboptimal. Perhaps we should keep vec of + // small islands ids? + let insert_in_last_island = self.awake_islands.last().map(|id| { + self.islands[*id].bodies.len() < self.optimizer.min_island_size + && self.islands[*id].is_sleeping() == rb.is_sleeping() + }); + // let insert_in_last_island = insert_in_last_island.is_some().then_some(true); + + if !rb.is_sleeping() && insert_in_last_island == Some(true) { + let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!()); + let target_island = &mut self.islands[id]; + + rb.ids.active_island_id = id; + rb.ids.active_set_id = target_island.bodies.len(); + target_island.bodies.push(handle); + } else { + let mut new_island = Island::singleton(handle, rb); + let id = self.free_islands.pop().unwrap_or(self.islands.len()); + + if !rb.is_sleeping() { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(id); + } + + self.islands.insert(id, new_island); + rb.ids.active_island_id = id; + rb.ids.active_set_id = 0; + } + } + + // Push the body to the active set if it is not inside the active set yet, and + // is not longer sleeping or became dynamic. + if (rb.changes.contains(RigidBodyChanges::SLEEP) + || rb.changes.contains(RigidBodyChanges::TYPE)) + && rb.is_enabled() + // Don’t wake up if the user put it to sleep manually. + && !rb.activation.sleeping + { + self.wake_up(bodies, handle, false); + } + } + + pub(crate) fn update_islands( + &mut self, + dt: Real, + length_unit: Real, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + ) { + // 1. Update active rigid-bodies energy. + // TODO PERF: should this done by the velocity solver after solving the constraints? + // let t0 = std::time::Instant::now(); + for handle in self + .awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) + { + let Some(rb) = bodies.get_mut_internal(handle) else { + // This branch happens if the rigid-body no longer exists. + continue; + }; + let sq_linvel = rb.vels.linvel.norm_squared(); + let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); + rb.activation + .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt); + + let can_sleep_now = rb.activation.is_eligible_for_sleep(); + + // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep" + // and push the sleep root candidate if applicable. + if can_sleep_now && rb.activation.sleep_root_state == SleepRootState::Unknown { + // This is a new candidate for island extraction. + self.traversal_candidates.push_back(SleepCandidate(handle)); + rb.activation.sleep_root_state = SleepRootState::TraversalPending; + } else if !can_sleep_now { + rb.activation.sleep_root_state = SleepRootState::Unknown; + } + } + // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); + + let mut cost = 0; + + // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). + // Limit the traversal cost by not traversing all the known sleeping roots if + // there are too many. + const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value. + while let Some(sleep_root) = self.traversal_candidates.pop_front() { + cost += self.extract_sleeping_island( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + sleep_root.0, + ); + + if cost > MAX_PER_FRAME_COST { + // Early-break if we consider we have done enough island extraction work. + break; + } + } + + self.update_optimizer( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); + // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); + + // NOTE: uncomment for debugging. + // self.assert_state_is_valid(bodies, colliders, narrow_phase); + } +} diff --git a/src/dynamics/island_manager/mod.rs b/src/dynamics/island_manager/mod.rs new file mode 100644 index 000000000..93f9c8f54 --- /dev/null +++ b/src/dynamics/island_manager/mod.rs @@ -0,0 +1,11 @@ +pub use manager::IslandManager; + +pub(crate) use island::Island; +use optimizer::IslandsOptimizer; + +mod island; +mod manager; +mod optimizer; +mod sleep; +mod utils; +mod validation; diff --git a/src/dynamics/island_manager/optimizer.rs b/src/dynamics/island_manager/optimizer.rs new file mode 100644 index 000000000..fab096984 --- /dev/null +++ b/src/dynamics/island_manager/optimizer.rs @@ -0,0 +1,225 @@ +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ColliderSet, NarrowPhase}; +use std::ops::IndexMut; + +use super::{Island, IslandManager}; + +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizerMergeState { + curr_awake_id: usize, +} + +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizerSplitState { + curr_awake_id: usize, + curr_body_id: usize, +} + +/// Configuration of the awake islands optimization strategy. +/// +/// Note that this currently only affects active islands. Sleeping islands are always kept minimal. +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizer { + /// The optimizer will try to merge small islands so their size exceed this minimum. + /// + /// Note that it will never merge incompatible islands (currently define as islands with + /// different additional solver iteration counts). + pub(super) min_island_size: usize, + /// The optimizer will try to split large islands so their size do not exceed this maximum. + /// + /// IMPORTANT: Must be greater than `2 * min_island_size` to avoid conflict between the splits + /// and merges. + pub(super) max_island_size: usize, + /// Indicates if the optimizer is in split or merge mode. Swaps between modes every step. + pub(super) mode: usize, + pub(super) merge_state: IslandsOptimizerMergeState, + pub(super) split_state: IslandsOptimizerSplitState, +} + +impl Default for IslandsOptimizer { + fn default() -> Self { + // TODO: figure out the best values. + Self { + min_island_size: 1024, + max_island_size: 4096, + mode: 0, + merge_state: Default::default(), + split_state: Default::default(), + } + } +} + +impl IslandManager { + pub(super) fn update_optimizer( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.mode % 2 == 0 { + self.incremental_merge(bodies); + } else { + self.incremental_split( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); + } + + self.optimizer.mode = self.optimizer.mode.wrapping_add(1); + } + + /// Attempts to merge awake islands that are too small. + fn incremental_merge(&mut self, bodies: &mut RigidBodySet) { + struct IslandData { + id: usize, + awake_id: usize, + solver_iters: usize, + } + + // Ensure the awake id is still in bounds. + if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.merge_state.curr_awake_id = 0; + } + + // Find a first candidate for a merge. + let mut island1 = None; + for awake_id in self.optimizer.merge_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + let island = &self.islands[id]; + if island.len() < self.optimizer.min_island_size { + island1 = Some(IslandData { + awake_id, + id, + solver_iters: island.additional_solver_iterations, + }); + break; + } + } + + if let Some(island1) = island1 { + // Indicates if we found a merge candidate for the next incremental update. + let mut found_next = false; + self.optimizer.merge_state.curr_awake_id = island1.awake_id + 1; + + // Find a second candidate for a merge. + for awake_id2 in island1.awake_id + 1..self.awake_islands.len() { + let id2 = self.awake_islands[awake_id2]; + let island2 = &self.islands[id2]; + + if island1.solver_iters == island2.additional_solver_iterations + && island2.len() < self.optimizer.min_island_size + { + // Found a second candidate! Merge them. + self.merge_islands(bodies, island1.id, id2); + + // TODO: support doing more than a single merge per frame. + return; + } else if island2.len() < self.optimizer.min_island_size && !found_next { + // We found a good candidate for the next incremental merge (we can’t just + // merge it now because it’s not compatible with the current island). + self.optimizer.merge_state.curr_awake_id = awake_id2; + found_next = true; + } + } + } else { + self.optimizer.merge_state.curr_awake_id = 0; + } + } + + /// Attempts to split awake islands that are too big. + fn incremental_split( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.split_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.split_state.curr_awake_id = 0; + } + + for awake_id in self.optimizer.split_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + if self.islands[id].len() > self.optimizer.max_island_size { + // Try to split this island. + // Note that the traversal logic is similar to the sleeping island + // extraction, except that we have different stopping criteria. + self.stack.clear(); + + // TODO: implement islands recycling to avoid reallocating every time. + let mut new_island = Island::default(); + self.traversal_timestamp += 1; + + for root in &self.islands[id].bodies { + self.stack.push(*root); + + let len_before_traversal = new_island.len(); + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut(handle); + if rb.is_fixed() { + // Don’t propagate islands through rigid-bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.traversal_timestamp { + // We already visited this body. + continue; + } + + rb.ids.active_set_timestamp = self.traversal_timestamp; + assert!(!rb.activation.sleeping); + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + super::utils::push_contacting_bodies( + &rb.colliders, + colliders, + narrow_phase, + &mut self.stack, + ); + super::utils::push_linked_bodies( + impulse_joints, + multibody_joints, + handle, + &mut self.stack, + ); + new_island.bodies.push(handle); + + // Our new island cannot grow any further. + if new_island.bodies.len() > self.optimizer.max_island_size { + new_island.bodies.truncate(len_before_traversal); + self.stack.clear(); + break; + } + } + + // Extract this island. + if new_island.len() == 0 { + // println!("Failed to split island."); + return; + } else if new_island.bodies.len() >= self.optimizer.min_island_size { + // println!( + // "Split an island: {}/{} ({} islands)", + // new_island.len(), + // self.islands[id].len(), + // self.awake_islands.len(), + // ); + self.extract_sub_island(bodies, id, new_island, false); + return; // TODO: support extracting more than one island per frame. + } + } + } else { + self.optimizer.split_state.curr_awake_id = awake_id + 1; + } + } + } +} diff --git a/src/dynamics/island_manager/sleep.rs b/src/dynamics/island_manager/sleep.rs new file mode 100644 index 000000000..a15a9846d --- /dev/null +++ b/src/dynamics/island_manager/sleep.rs @@ -0,0 +1,203 @@ +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SleepRootState, +}; +use crate::geometry::{ColliderSet, NarrowPhase}; + +use super::{Island, IslandManager}; + +impl IslandManager { + /// Wakes up a sleeping body, forcing it back into the active simulation. + /// + /// Use this when you want to ensure a body is active (useful after manually moving + /// a sleeping body, or to prevent it from sleeping in the next few frames). + /// + /// # Parameters + /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. + /// If `false`, it might sleep again immediately if conditions are met. + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// # let mut bodies = RigidBodySet::new(); + /// # let mut islands = IslandManager::new(); + /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); + /// islands.wake_up(&mut bodies, body_handle, true); + /// let body = bodies.get_mut(body_handle).unwrap(); + /// // Wake up a body before applying force to it + /// body.add_force(vector![100.0, 0.0, 0.0], false); + /// ``` + /// + /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). + pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { + // NOTE: the use an Option here because there are many legitimate cases (like when + // deleting a joint attached to an already-removed body) where we could be + // attempting to wake-up a rigid-body that has already been deleted. + if bodies.get(handle).map(|rb| !rb.is_fixed()) == Some(true) { + let rb = bodies.index_mut_internal(handle); + + // TODO: not sure if this is still relevant: + // // Check that the user didn’t change the sleeping state explicitly, in which + // // case we don’t overwrite it. + // if rb.changes.contains(RigidBodyChanges::SLEEP) { + // return; + // } + + rb.activation.wake_up(strong); + let island_to_wake_up = rb.ids.active_island_id; + self.wake_up_island(bodies, island_to_wake_up); + } + } + + /// Returns the number of iterations run by the graph traversal so we can balance load across + /// frames. + pub(super) fn extract_sleeping_island( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + sleep_root: RigidBodyHandle, + ) -> usize { + let Some(rb) = bodies.get_mut_internal(sleep_root) else { + // This branch happens if the rigid-body no longer exists. + return 0; + }; + + if rb.activation.sleep_root_state != SleepRootState::TraversalPending { + // We already traversed this sleep root. + return 0; + } + + rb.activation.sleep_root_state = SleepRootState::Traversed; + + let active_island_id = rb.ids.active_island_id; + let active_island = &mut self.islands[active_island_id]; + if active_island.is_sleeping() { + // This rigid-body is already part of a sleeping island. + return 0; + } + + // TODO: implement recycling islands to avoid repeated allocations? + let mut new_island = Island::default(); + self.stack.clear(); + self.stack.push(sleep_root); + + let mut niter = 0; + self.traversal_timestamp += 1; + + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut_internal(handle); + + if rb.is_fixed() { + // Don’t propagate islands through fixed bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.traversal_timestamp { + // We already visited this body and its neighbors. + continue; + } + + // if rb.ids.active_set_timestamp >= frame_base_timestamp { + // // We already visited this body and its neighbors during this frame. + // // So we already know this islands cannot sleep (otherwise the bodies + // // currently being traversed would already have been marked as sleeping). + // return niter; + // } + + niter += 1; + rb.ids.active_set_timestamp = self.traversal_timestamp; + + if rb.activation.is_eligible_for_sleep() { + rb.activation.sleep_root_state = SleepRootState::Traversed; + } + + assert_eq!( + rb.ids.active_island_id, + active_island_id, + "handle: {:?}, note niter: {}, isl size: {}", + handle, + niter, + active_island.len() + ); + assert!( + !rb.activation.sleeping, + "is sleeping: {:?} note niter: {}, isl size: {}", + handle, + niter, + active_island.len() + ); + + if !rb.activation.is_eligible_for_sleep() { + // If this body cannot sleep, abort the traversal, we are not traversing + // yet an island that can sleep. + self.stack.clear(); + return niter; + } + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + super::utils::push_contacting_bodies( + &rb.colliders, + colliders, + narrow_phase, + &mut self.stack, + ); + super::utils::push_linked_bodies( + impulse_joints, + multibody_joints, + handle, + &mut self.stack, + ); + new_island.bodies.push(handle); + } + + // If we reached this line, we completed a sleeping island traversal. + // - Put its bodies to sleep. + // - Remove them from the active set. + // - Push the sleeping island. + if active_island.len() == new_island.len() { + // The whole island is asleep. No need to insert a new one. + // Put all its bodies to sleep. + for handle in &active_island.bodies { + let rb = bodies.index_mut_internal(*handle); + rb.sleep(); + } + + // Mark the existing island as sleeping (by clearing its `id_in_awake_list`) + // and remove it from the awake list. + let island_awake_id = active_island + .id_in_awake_list + .take() + .unwrap_or_else(|| unreachable!()); + self.awake_islands.swap_remove(island_awake_id); + + if let Some(moved_id) = self.awake_islands.get(island_awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(island_awake_id); + } + } else { + niter += new_island.len(); // Include this part into the cost estimate for this function. + self.extract_sub_island(bodies, active_island_id, new_island, true); + } + niter + } + + fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) { + let Some(island) = self.islands.get_mut(island_id) else { + return; + }; + + if island.is_sleeping() { + island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(island_id); + + // Wake up all the bodies from this island. + for handle in &island.bodies { + if let Some(rb) = bodies.get_mut(*handle) { + rb.wake_up(false); + } + } + } + } +} diff --git a/src/dynamics/island_manager/utils.rs b/src/dynamics/island_manager/utils.rs new file mode 100644 index 000000000..d9db20870 --- /dev/null +++ b/src/dynamics/island_manager/utils.rs @@ -0,0 +1,41 @@ +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyColliders, RigidBodyHandle}; +use crate::geometry::{ColliderSet, NarrowPhase}; + +// Read all the contacts and push objects touching this rigid-body. +#[inline] +pub(super) fn push_contacting_bodies( + rb_colliders: &RigidBodyColliders, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + stack: &mut Vec, +) { + for collider_handle in &rb_colliders.0 { + for inter in narrow_phase.contact_pairs_with(*collider_handle) { + if inter.has_any_active_contact() { + let other = crate::utils::select_other( + (inter.collider1, inter.collider2), + *collider_handle, + ); + if let Some(other_body) = colliders[other].parent { + stack.push(other_body.handle); + } + } + } + } +} + +pub(super) fn push_linked_bodies( + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + handle: RigidBodyHandle, + stack: &mut Vec, +) { + for inter in impulse_joints.attached_enabled_joints(handle) { + let other = crate::utils::select_other((inter.0, inter.1), handle); + stack.push(other); + } + + for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { + stack.push(other); + } +} diff --git a/src/dynamics/island_manager/validation.rs b/src/dynamics/island_manager/validation.rs new file mode 100644 index 000000000..e6e565ba1 --- /dev/null +++ b/src/dynamics/island_manager/validation.rs @@ -0,0 +1,71 @@ +use crate::dynamics::{IslandManager, RigidBodySet}; +use crate::geometry::NarrowPhase; +use crate::prelude::ColliderSet; + +impl IslandManager { + #[allow(dead_code)] + pub(super) fn assert_state_is_valid( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + nf: &NarrowPhase, + ) { + for (island_id, island) in self.islands.iter() { + // Sleeping island must not be in the awake list. + if island.is_sleeping() { + assert!(!self.awake_islands.contains(&island_id)); + } else { + // If the island is awake, the awake id must match. + let awake_id = island.id_in_awake_list.unwrap(); + assert_eq!(self.awake_islands[awake_id], island_id); + } + + for (body_id, handle) in island.bodies.iter().enumerate() { + if let Some(rb) = bodies.get(*handle) { + // The body’s sleeping status must match the island’s status. + assert_eq!(rb.is_sleeping(), island.is_sleeping()); + // The body’s island id must match the island id. + assert_eq!(rb.ids.active_island_id, island_id); + // The body’s active set id must match its handle’s position in island.bodies. + assert_eq!(body_id, rb.ids.active_set_id); + } + } + } + + // Free island ids must actually be free. + for id in self.free_islands.iter() { + assert!(self.islands.get(*id).is_none()); + } + + // The awake islands list must not have duplicates. + let mut awake_islands_dedup = self.awake_islands.clone(); + awake_islands_dedup.sort(); + awake_islands_dedup.dedup(); + assert_eq!(self.awake_islands.len(), awake_islands_dedup.len()); + + // If two bodies have solver contacts, they must be in the same island. + for pair in nf.contact_pairs() { + let Some(body_handle1) = colliders[pair.collider1].parent.map(|p| p.handle) else { + continue; + }; + let Some(body_handle2) = colliders[pair.collider2].parent.map(|p| p.handle) else { + continue; + }; + + let body1 = &bodies[body_handle1]; + let body2 = &bodies[body_handle2]; + + if body1.is_fixed() || body2.is_fixed() { + continue; + } + + if pair.has_any_active_contact() { + assert_eq!(body1.ids.active_island_id, body2.ids.active_island_id); + } + } + + println!( + "`IslandManager::assert_state_is_valid` validation checks passed. This is slow. Only enable for debugging." + ); + } +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 08c668bb0..07104b4bd 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -73,6 +73,8 @@ pub struct ImpulseJointSet { joint_graph: InteractionGraph, /// A set of rigid-body handles to wake-up during the next timestep. pub(crate) to_wake_up: HashSet, + /// A set of rigid-body pairs to join in the island manager during the next timestep. + pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>, } impl ImpulseJointSet { @@ -83,6 +85,7 @@ impl ImpulseJointSet { joint_ids: Arena::new(), joint_graph: InteractionGraph::new(), to_wake_up: HashSet::default(), + to_join: HashSet::default(), } } @@ -366,6 +369,8 @@ impl ImpulseJointSet { self.to_wake_up.insert(body2); } + self.to_join.insert((body1, body2)); + ImpulseJointHandle(handle) } @@ -377,7 +382,7 @@ impl ImpulseJointSet { bodies: &RigidBodySet, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_islands()] { + for out_island in &mut out[..islands.active_islands().len()] { out_island.clear(); } @@ -392,13 +397,17 @@ impl ImpulseJointSet { && (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping()) && (!rb2.is_dynamic_or_kinematic() || !rb2.is_sleeping()) { - let island_index = if !rb1.is_dynamic_or_kinematic() { - rb2.ids.active_island_id + let island_awake_index = if !rb1.is_dynamic_or_kinematic() { + islands.islands[rb2.ids.active_island_id] + .id_in_awake_list() + .expect("Internal error: island should be awake.") } else { - rb1.ids.active_island_id + islands.islands[rb1.ids.active_island_id] + .id_in_awake_list() + .expect("Internal error: island should be awake.") }; - out[island_index].push(i); + out[island_awake_index].push(i); } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index b3e184e57..7295fd7bd 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -95,6 +95,8 @@ pub struct MultibodyJointSet { // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, pub(crate) to_wake_up: HashSet, + /// A set of rigid-body pairs to join in the island manager during the next timestep. + pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>, } impl MultibodyJointSet { @@ -105,6 +107,7 @@ impl MultibodyJointSet { rb2mb: Coarena::new(), connectivity_graph: InteractionGraph::new(), to_wake_up: HashSet::default(), + to_join: HashSet::default(), } } @@ -205,6 +208,8 @@ impl MultibodyJointSet { self.to_wake_up.insert(body2); } + self.to_join.insert((body1, body2)); + // Because each rigid-body can only have one parent link, // we can use the second rigid-body’s handle as the multibody_joint’s // handle. diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 63e394a4f..31f4065e4 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -665,7 +665,7 @@ impl RigidBody { // to all the fixed bodies active set offsets? pub fn effective_active_set_offset(&self) -> u32 { if self.is_dynamic_or_kinematic() { - self.ids.active_set_offset + self.ids.active_set_id as u32 } else { u32::MAX } diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index d8cce7a5e..899efa13a 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1029,7 +1029,6 @@ impl RigidBodyCcd { pub struct RigidBodyIds { pub(crate) active_island_id: usize, pub(crate) active_set_id: usize, - pub(crate) active_set_offset: u32, pub(crate) active_set_timestamp: u32, } @@ -1038,7 +1037,6 @@ impl Default for RigidBodyIds { Self { active_island_id: usize::MAX, active_set_id: usize::MAX, - active_set_offset: u32::MAX, active_set_timestamp: 0, } } @@ -1138,6 +1136,19 @@ impl RigidBodyDominance { } } +#[derive(Copy, Clone, PartialEq, Eq, Debug, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) enum SleepRootState { + /// This sleep root has already been traversed. No need to traverse + /// again until the rigid-body either gets awaken by an event. + Traversed, + /// This sleep root has not been traversed yet. + TraversalPending, + /// This body can become a sleep root once it falls asleep. + #[default] + Unknown, +} + /// Controls when a body goes to sleep (becomes inactive to save CPU). /// /// ## Sleeping System @@ -1183,6 +1194,8 @@ pub struct RigidBodyActivation { /// Is this body currently sleeping? pub sleeping: bool, + + pub(crate) sleep_root_state: SleepRootState, } impl Default for RigidBodyActivation { @@ -1216,6 +1229,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: 0.0, sleeping: false, + sleep_root_state: SleepRootState::Unknown, } } @@ -1227,6 +1241,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: Self::default_time_until_sleep(), sleeping: true, + sleep_root_state: SleepRootState::Unknown, } } @@ -1249,6 +1264,12 @@ impl RigidBodyActivation { #[inline] pub fn wake_up(&mut self, strong: bool) { self.sleeping = false; + + // Make this body eligible as a sleep root again. + if self.sleep_root_state != SleepRootState::TraversalPending { + self.sleep_root_state = SleepRootState::Unknown; + } + if strong { self.time_since_can_sleep = 0.0; } @@ -1260,6 +1281,41 @@ impl RigidBodyActivation { self.sleeping = true; self.time_since_can_sleep = self.time_until_sleep; } + + /// Does this body have a sufficiently low kinetic energy for a long enough + /// duration to be eligible for sleeping? + pub fn is_eligible_for_sleep(&self) -> bool { + self.time_since_can_sleep >= self.time_until_sleep + } + + pub(crate) fn update_energy( + &mut self, + body_type: RigidBodyType, + length_unit: Real, + sq_linvel: Real, + sq_angvel: Real, + dt: Real, + ) { + let can_sleep = match body_type { + RigidBodyType::Dynamic => { + let linear_threshold = self.normalized_linear_threshold * length_unit; + sq_linvel < linear_threshold * linear_threshold.abs() + && sq_angvel < self.angular_threshold * self.angular_threshold.abs() + } + RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { + // Platforms only sleep if both velocities are exactly zero. If it’s not exactly + // zero, then the user really wants them to move. + sq_linvel == 0.0 && sq_angvel == 0.0 + } + RigidBodyType::Fixed => true, + }; + + if can_sleep { + self.time_since_can_sleep += dt; + } else { + self.time_since_can_sleep = 0.0; + } + } } #[cfg(test)] diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 57523e2c0..4d46d7aae 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -11,10 +11,9 @@ use crate::dynamics::{ RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::Real; use crate::math::SIMD_WIDTH; -use crate::math::{MAX_MANIFOLD_POINTS, Real}; use na::DVector; -use parry::math::DIM; use crate::dynamics::solver::contact_constraint::any_contact_constraint::AnyContactConstraintMut; #[cfg(feature = "dim3")] @@ -31,14 +30,8 @@ pub struct ConstraintsCounts { } impl ConstraintsCounts { - pub fn from_contacts(manifold: &ContactManifold) -> Self { - let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; - Self { - num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS - + rest as usize, - num_jacobian_lines: manifold.data.solver_contacts.len() * DIM, - } - } + // NOTE: constraints count from contacts is always 1 since the max number of solver contacts + // matches the max number of contact per constraint. pub fn from_joint(joint: &ImpulseJoint) -> Self { let joint = &joint.data; @@ -334,18 +327,8 @@ impl ContactConstraintsSet { solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { - let total_num_constraints = self - .interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum::() - + self - .interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = (self.interaction_groups.simd_interactions.len() / SIMD_WIDTH) + + self.interaction_groups.nongrouped_interactions.len(); unsafe { reset_buffer( @@ -358,15 +341,14 @@ impl ContactConstraintsSet { ); } - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifolds_i in self .interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; let manifold_id = array![|ii| manifolds_i[ii]]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; @@ -375,16 +357,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_twist_constraints_builder[curr_start..], - &mut self.simd_velocity_twist_constraints[curr_start..], + &mut self.simd_velocity_twist_constraints_builder[curr_id], + &mut self.simd_velocity_twist_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; let mut manifold_id = [usize::MAX; SIMD_WIDTH]; manifold_id[0] = *manifolds_i; let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; @@ -394,14 +374,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_twist_constraints_builder[curr_start..], - &mut self.simd_velocity_twist_constraints[curr_start..], + &mut self.simd_velocity_twist_constraints_builder[curr_id], + &mut self.simd_velocity_twist_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } fn simd_compute_coulomb_constraints( @@ -410,18 +390,8 @@ impl ContactConstraintsSet { solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { - let total_num_constraints = self - .interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum::() - + self - .interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH + + self.interaction_groups.nongrouped_interactions.len(); unsafe { reset_buffer( @@ -434,15 +404,14 @@ impl ContactConstraintsSet { ); } - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifolds_i in self .interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; let manifold_id = array![|ii| manifolds_i[ii]]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; @@ -451,16 +420,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], - &mut self.simd_velocity_coulomb_constraints[curr_start..], + &mut self.simd_velocity_coulomb_constraints_builder[curr_id], + &mut self.simd_velocity_coulomb_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; let mut manifold_id = [usize::MAX; SIMD_WIDTH]; manifold_id[0] = *manifolds_i; let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; @@ -470,14 +437,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], - &mut self.simd_velocity_coulomb_constraints[curr_start..], + &mut self.simd_velocity_coulomb_constraints_builder[curr_id], + &mut self.simd_velocity_coulomb_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } fn compute_generic_constraints( @@ -487,11 +454,7 @@ impl ContactConstraintsSet { manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, ) { - let total_num_constraints = self - .generic_two_body_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = self.generic_two_body_interactions.len(); self.generic_velocity_constraints_builder.resize( total_num_constraints, @@ -500,27 +463,27 @@ impl ContactConstraintsSet { self.generic_velocity_constraints .resize(total_num_constraints, GenericContactConstraint::invalid()); - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifold_i in &self.generic_two_body_interactions { let manifold = &manifolds_all[*manifold_i]; - let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; GenericContactConstraintBuilder::generate( *manifold_i, manifold, bodies, multibody_joints, - &mut self.generic_velocity_constraints_builder[curr_start..], - &mut self.generic_velocity_constraints[curr_start..], + &mut self.generic_velocity_constraints_builder[curr_id], + &mut self.generic_velocity_constraints[curr_id], &mut self.generic_jacobians, jacobian_id, ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } pub fn warmstart( diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs index 50c0bc057..1a9f7387c 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -43,8 +43,8 @@ impl ContactWithCoulombFrictionBuilder { manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, solver_bodies: &SolverBodies, - out_builders: &mut [ContactWithCoulombFrictionBuilder], - out_constraints: &mut [ContactWithCoulombFriction], + out_builder: &mut ContactWithCoulombFrictionBuilder, + out_constraint: &mut ContactWithCoulombFriction, ) { // TODO: could we avoid having to fetch the ids here? It’s the only thing we // read from the original rigid-bodies. @@ -52,7 +52,7 @@ impl ContactWithCoulombFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -60,7 +60,7 @@ impl ContactWithCoulombFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -83,165 +83,159 @@ impl ContactWithCoulombFrictionBuilder { let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = - array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - - constraint.dir1 = force_dir1; - constraint.im1 = poses1.im; - constraint.im2 = poses2.im; - constraint.solver_vel1 = ids1; - constraint.solver_vel2 = ids2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..num_points { - // SAFETY: we already know that the `manifold_points` has `num_points` elements - // so `k` isn’t out of bounds. - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - - let is_bouncy = solver_contact.is_bouncy(); - - let dp1 = solver_contact.point - world_com1; - let dp2 = solver_contact.point - world_com2; - - let vel1 = vels1.linear + vels1.angular.gcross(dp1); - let vel2 = vels2.linear + vels2.angular.gcross(dp2); - - constraint.limit = solver_contact.friction; - constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + out_constraint.dir1 = force_dir1; + out_constraint.im1 = poses1.im; + out_constraint.im2 = poses2.im; + out_constraint.solver_vel1 = ids1; + out_constraint.solver_vel2 = ids2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + out_constraint.tangent1 = tangents1[0]; + } - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - let imsum = poses1.im + poses2.im; - let projected_mass = utils::simd_inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2), - ); + let is_bouncy = solver_contact.is_bouncy(); - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - normal_rhs_wo_bias = - is_bouncy * solver_contact.restitution * projected_velocity; + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; - constraint.normal_part[k].torque_dir1 = torque_dir1; - constraint.normal_part[k].torque_dir2 = torque_dir2; - constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; - constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; - constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; - constraint.normal_part[k].r = projected_mass; - } + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); - // tangent parts. - constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse; + out_constraint.limit = solver_contact.friction; + out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); - for j in 0..DIM - 1 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); - let imsum = poses1.im + poses2.im; + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2); - let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]); - - constraint.tangent_part[k].torque_dir1[j] = torque_dir1; - constraint.tangent_part[k].torque_dir2[j] = torque_dir2; - constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; - constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; - constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; - } + out_constraint.normal_part[k].torque_dir1 = torque_dir1; + out_constraint.normal_part[k].torque_dir2 = torque_dir2; + out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + out_constraint.normal_part[k].r = projected_mass; + } - #[cfg(feature = "dim3")] - { - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - constraint.tangent_part[k].r[2] = SimdReal::splat(2.0) - * (constraint.tangent_part[k].ii_torque_dir1[0] - .gdot(constraint.tangent_part[k].torque_dir1[1]) - + constraint.tangent_part[k].ii_torque_dir2[0] - .gdot(constraint.tangent_part[k].torque_dir2[1])); - } + // tangent parts. + out_constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse; + + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]); + + out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; + out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; + out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias; + out_constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } - // Builder. - builder.infos[k].local_p1 = - poses1.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].local_p2 = - poses2.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].tangent_vel = solver_contact.tangent_velocity; - builder.infos[k].dist = solver_contact.dist; - builder.infos[k].normal_vel = normal_rhs_wo_bias; + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + out_constraint.tangent_part[k].r[2] = SimdReal::splat(2.0) + * (out_constraint.tangent_part[k].ii_torque_dir1[0] + .gdot(out_constraint.tangent_part[k].torque_dir1[1]) + + out_constraint.tangent_part[k].ii_torque_dir2[0] + .gdot(out_constraint.tangent_part[k].torque_dir2[1])); } - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..num_points / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let imsum = poses1.im + poses2.im; - let r0 = constraint.normal_part[k0].r; - let r1 = constraint.normal_part[k1].r; - - let mut r_mat = SdpMatrix2::zero(); - - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) - + constraint.normal_part[k0] - .ii_torque_dir1 - .gdot(constraint.normal_part[k1].torque_dir1) - + constraint.normal_part[k0] - .ii_torque_dir2 - .gdot(constraint.normal_part[k1].torque_dir2); - r_mat.m11 = utils::simd_inv(r0); - r_mat.m22 = utils::simd_inv(r1); - - let (inv, det) = { - let _disable_fe_except = + // Builder. + out_builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].tangent_vel = solver_contact.tangent_velocity; + out_builder.infos[k].dist = solver_contact.dist; + out_builder.infos[k].normal_vel = normal_rhs_wo_bias; + } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..num_points / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let imsum = poses1.im + poses2.im; + let r0 = out_constraint.normal_part[k0].r; + let r1 = out_constraint.normal_part[k1].r; + + let mut r_mat = SdpMatrix2::zero(); + + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) + + out_constraint.normal_part[k0] + .ii_torque_dir1 + .gdot(out_constraint.normal_part[k1].torque_dir1) + + out_constraint.normal_part[k0] + .ii_torque_dir2 + .gdot(out_constraint.normal_part[k1].torque_dir2); + r_mat.m11 = utils::simd_inv(r0); + r_mat.m22 = utils::simd_inv(r1); + + let (inv, det) = { + let _disable_fe_except = crate::utils::DisableFloatingPointExceptionsFlags:: disable_floating_point_exceptions(); - r_mat.inverse_and_get_determinant_unchecked() - }; - let is_invertible = det.simd_gt(SimdReal::zero()); - - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.normal_part[k0].r_mat_elts = [ - inv.m11.select(is_invertible, r0), - inv.m22.select(is_invertible, SimdReal::zero()), - ]; - constraint.normal_part[k1].r_mat_elts = [ - inv.m12.select(is_invertible, SimdReal::zero()), - r_mat.m12.select(is_invertible, SimdReal::zero()), - ]; - } + r_mat.inverse_and_get_determinant_unchecked() + }; + let is_invertible = det.simd_gt(SimdReal::zero()); + + // If inversion failed, the contacts are redundant. + // Ignore the one with the smallest depth (it is too late to + // have the constraint removed from the constraint set, so just + // set the mass (r) matrix elements to 0. + out_constraint.normal_part[k0].r_mat_elts = [ + inv.m11.select(is_invertible, r0), + inv.m22.select(is_invertible, SimdReal::zero()), + ]; + out_constraint.normal_part[k1].r_mat_elts = [ + inv.m12.select(is_invertible, SimdReal::zero()), + r_mat.m12.select(is_invertible, SimdReal::zero()), + ]; } } } diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs index fc50997f2..399f57eb3 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -50,8 +50,8 @@ impl ContactWithTwistFrictionBuilder { manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, solver_bodies: &SolverBodies, - out_builders: &mut [ContactWithTwistFrictionBuilder], - out_constraints: &mut [ContactWithTwistFriction], + out_builder: &mut ContactWithTwistFrictionBuilder, + out_constraint: &mut ContactWithTwistFriction, ) { // TODO: could we avoid having to fetch the ids here? It’s the only thing we // read from the original rigid-bodies. @@ -59,7 +59,7 @@ impl ContactWithTwistFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -67,7 +67,7 @@ impl ContactWithTwistFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -90,166 +90,159 @@ impl ContactWithTwistFrictionBuilder { let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = - array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - let inv_num_points = SimdReal::splat(1.0 / num_points as Real); + let inv_num_points = SimdReal::splat(1.0 / num_points as Real); - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - - constraint.dir1 = force_dir1; - constraint.im1 = poses1.im; - constraint.im2 = poses2.im; - constraint.solver_vel1 = ids1; - constraint.solver_vel2 = ids2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - let mut friction_center = Point::origin(); - let mut twist_warmstart = na::zero(); - let mut tangent_warmstart = na::zero(); - let mut tangent_vel: Vector<_> = na::zero(); + out_constraint.dir1 = force_dir1; + out_constraint.im1 = poses1.im; + out_constraint.im2 = poses2.im; + out_constraint.solver_vel1 = ids1; + out_constraint.solver_vel2 = ids2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + out_constraint.tangent1 = tangents1[0]; + } - for k in 0..num_points { - // SAFETY: we already know that the `manifold_points` has `num_points` elements - // so `k` isn’t out of bounds. - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + let mut friction_center = Point::origin(); + let mut twist_warmstart = na::zero(); + let mut tangent_warmstart = na::zero(); + let mut tangent_vel: Vector<_> = na::zero(); - let is_bouncy = solver_contact.is_bouncy(); + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - friction_center += solver_contact.point.coords * inv_num_points; + let is_bouncy = solver_contact.is_bouncy(); - let dp1 = solver_contact.point - world_com1; - let dp2 = solver_contact.point - world_com2; + friction_center += solver_contact.point.coords * inv_num_points; - let vel1 = vels1.linear + vels1.angular.gcross(dp1); - let vel2 = vels2.linear + vels2.angular.gcross(dp2); + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; - twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points; - tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points; - tangent_vel += solver_contact.tangent_velocity * inv_num_points; + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); - constraint.limit = solver_contact.friction; - constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points; + tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points; + tangent_vel += solver_contact.tangent_velocity * inv_num_points; - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + out_constraint.limit = solver_contact.friction; + out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); - let imsum = poses1.im + poses2.im; - let projected_mass = utils::simd_inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2), - ); + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - normal_rhs_wo_bias = - is_bouncy * solver_contact.restitution * projected_velocity; + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); - constraint.normal_part[k].torque_dir1 = torque_dir1; - constraint.normal_part[k].torque_dir2 = torque_dir2; - constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; - constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; - constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; - constraint.normal_part[k].r = projected_mass; - } + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity; - // Builder. - builder.infos[k].local_p1 = - poses1.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].local_p2 = - poses2.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].dist = solver_contact.dist; - builder.infos[k].normal_vel = normal_rhs_wo_bias; + out_constraint.normal_part[k].torque_dir1 = torque_dir1; + out_constraint.normal_part[k].torque_dir2 = torque_dir2; + out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + out_constraint.normal_part[k].r = projected_mass; } - /* - * Tangent/twist part - */ - constraint.tangent_part.impulse = tangent_warmstart; - constraint.twist_part.impulse = twist_warmstart; - - builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center); - builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center); - - let dp1 = friction_center - world_com1; - let dp2 = friction_center - world_com2; - - // Twist part. It has no effect when there is only one point. - if num_points > 1 { - let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS]; - for k in 0..num_points { - // FIXME PERF: we don’t want to re-fetch here just to get the solver contact point! - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point); - } + // Builder. + out_builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].dist = solver_contact.dist; + out_builder.infos[k].normal_vel = normal_rhs_wo_bias; + } - let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1); - let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1); - constraint.twist_part.rhs = SimdReal::zero(); - constraint.twist_part.ii_twist_dir1 = ii_twist_dir1; - constraint.twist_part.ii_twist_dir2 = ii_twist_dir2; - constraint.twist_part.r = utils::simd_inv( - ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1), - ); - constraint.twist_dists = twist_dists; - } + /* + * Tangent/twist part + */ + out_constraint.tangent_part.impulse = tangent_warmstart; + out_constraint.twist_part.impulse = twist_warmstart; - // Tangent part. - for j in 0..2 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + out_builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center); + out_builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center); - let imsum = poses1.im + poses2.im; + let dp1 = friction_center - world_com1; + let dp2 = friction_center - world_com2; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2); - - // TODO: add something similar to tangent velocity to the twist - // constraint for the case where the different points don’t - // have the same tangent vel? - let rhs_wo_bias = tangent_vel.dot(&tangents1[j]); - - constraint.tangent_part.torque_dir1[j] = torque_dir1; - constraint.tangent_part.torque_dir2[j] = torque_dir2; - constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1; - constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2; - constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part.rhs[j] = rhs_wo_bias; - constraint.tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; + // Twist part. It has no effect when there is only one point. + if num_points > 1 { + let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS]; + for k in 0..num_points { + // FIXME PERF: we don’t want to re-fetch here just to get the solver contact point! + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point); } - #[cfg(feature = "dim3")] - { - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - constraint.tangent_part.r[2] = SimdReal::splat(2.0) - * (constraint.tangent_part.ii_torque_dir1[0] - .gdot(constraint.tangent_part.torque_dir1[1]) - + constraint.tangent_part.ii_torque_dir2[0] - .gdot(constraint.tangent_part.torque_dir2[1])); - } + let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1); + let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1); + out_constraint.twist_part.rhs = SimdReal::zero(); + out_constraint.twist_part.ii_twist_dir1 = ii_twist_dir1; + out_constraint.twist_part.ii_twist_dir2 = ii_twist_dir2; + out_constraint.twist_part.r = + utils::simd_inv(ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1)); + out_constraint.twist_dists = twist_dists; + } + + // Tangent part. + for j in 0..2 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + + // TODO: add something similar to tangent velocity to the twist + // constraint for the case where the different points don’t + // have the same tangent vel? + let rhs_wo_bias = tangent_vel.dot(&tangents1[j]); + + out_constraint.tangent_part.torque_dir1[j] = torque_dir1; + out_constraint.tangent_part.torque_dir2[j] = torque_dir2; + out_constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1; + out_constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2; + out_constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part.rhs[j] = rhs_wo_bias; + out_constraint.tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + out_constraint.tangent_part.r[2] = SimdReal::splat(2.0) + * (out_constraint.tangent_part.ii_torque_dir1[0] + .gdot(out_constraint.tangent_part.torque_dir1[1]) + + out_constraint.tangent_part.ii_torque_dir2[0] + .gdot(out_constraint.tangent_part.torque_dir2[1])); } } diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index c8294bc82..15f348a1d 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -36,8 +36,8 @@ impl GenericContactConstraintBuilder { manifold: &ContactManifold, bodies: &RigidBodySet, multibodies: &MultibodyJointSet, - out_builders: &mut [GenericContactConstraintBuilder], - out_constraints: &mut [GenericContactConstraint], + out_builder: &mut GenericContactConstraintBuilder, + out_constraint: &mut GenericContactConstraint, jacobians: &mut DVector, jacobian_id: &mut usize, ) { @@ -68,7 +68,7 @@ impl GenericContactConstraintBuilder { multibody1 .map(|mb| mb.0.solver_id) .unwrap_or(if type1.is_dynamic_or_kinematic() { - rb1.ids.active_set_offset + rb1.ids.active_set_id as u32 } else { u32::MAX }); @@ -76,7 +76,7 @@ impl GenericContactConstraintBuilder { multibody2 .map(|mb| mb.0.solver_id) .unwrap_or(if type2.is_dynamic_or_kinematic() { - rb2.ids.active_set_offset + rb2.ids.active_set_id as u32 } else { u32::MAX }); @@ -99,54 +99,127 @@ impl GenericContactConstraintBuilder { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } - for (l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() + let chunk_j_id = *jacobian_id; + + let manifold_points = &manifold.data.solver_contacts; + out_constraint.dir1 = force_dir1; + out_constraint.im1 = if type1.is_dynamic_or_kinematic() { + mprops1.effective_inv_mass + } else { + na::zero() + }; + out_constraint.im2 = if type2.is_dynamic_or_kinematic() { + mprops2.effective_inv_mass + } else { + na::zero() + }; + out_constraint.solver_vel1 = solver_vel1; + out_constraint.solver_vel2 = solver_vel2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] { - let chunk_j_id = *jacobian_id; - - let builder = &mut out_builders[l]; - let constraint = &mut out_constraints[l]; - constraint.dir1 = force_dir1; - constraint.im1 = if type1.is_dynamic_or_kinematic() { - mprops1.effective_inv_mass - } else { - na::zero() - }; - constraint.im2 = if type2.is_dynamic_or_kinematic() { - mprops2.effective_inv_mass - } else { - na::zero() - }; - constraint.solver_vel1 = solver_vel1; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = manifold_points.len() as u8; - #[cfg(feature = "dim3")] + out_constraint.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + let dp1 = point - mprops1.world_com; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + out_constraint.limit = manifold_point.friction; + out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + + // Normal part. + let normal_rhs_wo_bias; { - constraint.tangent1 = tangents1[0]; - } + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + + let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { + mprops1 + .effective_world_inv_inertia + .transform_vector(torque_dir1) + } else { + na::zero() + }; + let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { + mprops2 + .effective_world_inv_inertia + .transform_vector(torque_dir2) + } else { + na::zero() + }; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir1), + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if type1.is_dynamic_or_kinematic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if type2.is_dynamic_or_kinematic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + ii_torque_dir2.gdot(torque_dir2) + } else { + 0.0 + }; - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let point = manifold_point.point; - let dp1 = point - mprops1.world_com; - let dp2 = point - mprops2.world_com; + let r = crate::utils::inv(inv_r1 + inv_r2); - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + normal_rhs_wo_bias = + (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); + out_constraint.normal_part[k] = ContactConstraintNormalPart { + torque_dir1, + torque_dir2, + ii_torque_dir1, + ii_torque_dir2, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse_accumulator: na::zero(), + impulse: manifold_point.warmstart_impulse, + r, + r_mat_elts: [0.0; 2], + }; + } + + // Tangent parts. + { + out_constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse; + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { mprops1 .effective_world_inv_inertia @@ -154,6 +227,10 @@ impl GenericContactConstraintBuilder { } else { na::zero() }; + out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; + + let torque_dir2 = dp2.gcross(-tangents1[j]); let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { mprops2 .effective_world_inv_inertia @@ -161,13 +238,15 @@ impl GenericContactConstraintBuilder { } else { na::zero() }; + out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { mb1.fill_jacobians( *link_id1, - force_dir1, + tangents1[j], #[cfg(feature = "dim2")] - na::vector!(torque_dir1), + na::vector![torque_dir1], #[cfg(feature = "dim3")] torque_dir1, jacobian_id, @@ -184,9 +263,9 @@ impl GenericContactConstraintBuilder { let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { mb2.fill_jacobians( *link_id2, - -force_dir1, + -tangents1[j], #[cfg(feature = "dim2")] - na::vector!(torque_dir2), + na::vector![torque_dir2], #[cfg(feature = "dim3")] torque_dir2, jacobian_id, @@ -201,141 +280,54 @@ impl GenericContactConstraintBuilder { }; let r = crate::utils::inv(inv_r1 + inv_r2); + let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - normal_rhs_wo_bias = - (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - - constraint.normal_part[k] = ContactConstraintNormalPart { - torque_dir1, - torque_dir2, - ii_torque_dir1, - ii_torque_dir2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse_accumulator: na::zero(), - impulse: manifold_point.warmstart_impulse, - r, - r_mat_elts: [0.0; 2], - }; - } - - // Tangent parts. - { - constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { - mprops1 - .effective_world_inv_inertia - .transform_vector(torque_dir1) - } else { - na::zero() - }; - constraint.tangent_part[k].torque_dir1[j] = torque_dir1; - constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; - - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { - mprops2 - .effective_world_inv_inertia - .transform_vector(torque_dir2) - } else { - na::zero() - }; - constraint.tangent_part[k].torque_dir2[j] = torque_dir2; - constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; - - let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { - mb1.fill_jacobians( - *link_id1, - tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir1], - #[cfg(feature = "dim3")] - torque_dir1, - jacobian_id, - jacobians, - ) - .0 - } else if type1.is_dynamic_or_kinematic() { - force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - } else { - 0.0 - }; - - let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { - mb2.fill_jacobians( - *link_id2, - -tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir2], - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0 - } else if type2.is_dynamic_or_kinematic() { - force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + ii_torque_dir2.gdot(torque_dir2) - } else { - 0.0 - }; - - let r = crate::utils::inv(inv_r1 + inv_r2); - let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - - constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - - // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) - // in lhs. See the corresponding code on the `velocity_constraint.rs` - // file. - constraint.tangent_part[k].r[j] = r; - } + // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + out_constraint.tangent_part[k].r[j] = r; } + } - // Builder. - let infos = CoulombContactPointInfos { - local_p1: rb1 - .pos - .position - .inverse_transform_point(&manifold_point.point), - local_p2: rb2 - .pos - .position - .inverse_transform_point(&manifold_point.point), - tangent_vel: manifold_point.tangent_velocity, - dist: manifold_point.dist, - normal_vel: normal_rhs_wo_bias, - }; + // Builder. + let infos = CoulombContactPointInfos { + local_p1: rb1 + .pos + .position + .inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_vel: normal_rhs_wo_bias, + }; - builder.handle1 = handle1; - builder.handle2 = handle2; - builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; - builder.infos[k] = infos; - constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; - } + out_builder.handle1 = handle1; + out_builder.handle2 = handle2; + out_builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; + out_builder.infos[k] = infos; + out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + } - let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); - let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); + let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); + let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); - // NOTE: we use the generic constraint for non-dynamic bodies because this will - // reduce all ops to nothing because its ndofs will be zero. - let generic_constraint_mask = (multibody1.is_some() as u8) - | ((multibody2.is_some() as u8) << 1) - | (!type1.is_dynamic_or_kinematic() as u8) - | ((!type2.is_dynamic_or_kinematic() as u8) << 1); + // NOTE: we use the generic constraint for non-dynamic bodies because this will + // reduce all ops to nothing because its ndofs will be zero. + let generic_constraint_mask = (multibody1.is_some() as u8) + | ((multibody2.is_some() as u8) << 1) + | (!type1.is_dynamic_or_kinematic() as u8) + | ((!type2.is_dynamic_or_kinematic() as u8) << 1); - constraint.j_id = chunk_j_id; - constraint.ndofs1 = ndofs1; - constraint.ndofs2 = ndofs2; - constraint.generic_constraint_mask = generic_constraint_mask; - } + out_constraint.j_id = chunk_j_id; + out_constraint.ndofs1 = ndofs1; + out_constraint.ndofs2 = ndofs2; + out_constraint.generic_constraint_mask = generic_constraint_mask; } pub fn update( diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index dcc20b451..80d1457da 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -72,7 +72,7 @@ impl ParallelInteractionGroups { interactions: &[Interaction], interaction_indices: &[usize], ) { - let num_island_bodies = islands.active_island(island_id).len(); + let num_island_bodies = islands.island(island_id).len(); self.bodies_color.clear(); self.interaction_indices.clear(); self.groups.clear(); @@ -117,26 +117,26 @@ impl ParallelInteractionGroups { (false, false) => { let rb1 = &bodies[body_pair.0.unwrap()]; let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = bcolors[rb1.ids.active_set_offset as usize] - | bcolors[rb2.ids.active_set_offset as usize]; + let color_mask = + bcolors[rb1.ids.active_set_id] | bcolors[rb2.ids.active_set_id]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; - bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb1.ids.active_set_id] |= 1 << *color; + bcolors[rb2.ids.active_set_id] |= 1 << *color; } (true, false) => { let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = bcolors[rb2.ids.active_set_offset as usize]; + let color_mask = bcolors[rb2.ids.active_set_id]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb2.ids.active_set_id] |= 1 << *color; } (false, true) => { let rb1 = &bodies[body_pair.0.unwrap()]; - let color_mask = bcolors[rb1.ids.active_set_offset as usize]; + let color_mask = bcolors[rb1.ids.active_set_id]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb1.ids.active_set_id] |= 1 << *color; } (true, true) => unreachable!(), } @@ -244,7 +244,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(islands.active_island(island_id).len(), 0u128); + .resize(islands.island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -270,18 +270,10 @@ impl InteractionGroups { } let ijoint = interaction.data.locked_axes.bits() as usize; - let i1 = rb1.ids.active_set_offset; - let i2 = rb2.ids.active_set_offset; - let conflicts = self - .body_masks - .get(i1 as usize) - .copied() - .unwrap_or_default() - | self - .body_masks - .get(i2 as usize) - .copied() - .unwrap_or_default() + let i1 = rb1.ids.active_set_id; + let i2 = rb2.ids.active_set_id; + let conflicts = self.body_masks.get(i1).copied().unwrap_or_default() + | self.body_masks.get(i2).copied().unwrap_or_default() | joint_type_conflicts[ijoint]; 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; @@ -396,7 +388,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(islands.active_island(island_id).len(), 0u128); + .resize(islands.island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -420,17 +412,15 @@ impl InteractionGroups { continue; } - let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 - { + let (status1, active_set_id1) = if let Some(rb1) = interaction.data.rigid_body1 { let rb1 = &bodies[rb1]; - (rb1.body_type, rb1.ids.active_set_offset) + (rb1.body_type, rb1.ids.active_set_id as u32) } else { (RigidBodyType::Fixed, u32::MAX) }; - let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 - { + let (status2, active_set_id2) = if let Some(rb2) = interaction.data.rigid_body2 { let rb2 = &bodies[rb2]; - (rb2.body_type, rb2.ids.active_set_offset) + (rb2.body_type, rb2.ids.active_set_id as u32) } else { (RigidBodyType::Fixed, u32::MAX) }; @@ -443,8 +433,8 @@ impl InteractionGroups { continue; } - let i1 = active_set_offset1; - let i2 = active_set_offset2; + let i1 = active_set_id1; + let i2 = active_set_id2; let mask1 = if !is_fixed1 { self.body_masks[i1 as usize] } else { diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 80f2d4b6d..96aa56e27 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -43,8 +43,12 @@ impl IslandSolver { multibodies: &mut MultibodyJointSet, ) { counters.solver.velocity_assembly_time.resume(); + counters + .solver + .velocity_assembly_time_solver_bodies + .resume(); let num_solver_iterations = base_params.num_solver_iterations - + islands.active_island_additional_solver_iterations(island_id); + + islands.island(island_id).additional_solver_iterations(); let mut params = *base_params; params.dt /= num_solver_iterations as Real; @@ -55,7 +59,6 @@ impl IslandSolver { * */ // INIT - // let t0 = std::time::Instant::now(); self.velocity_solver .init_solver_velocities_and_solver_bodies( base_params.dt, @@ -65,8 +68,11 @@ impl IslandSolver { bodies, multibodies, ); - // let t_solver_body_init = t0.elapsed().as_secs_f32(); - // let t0 = std::time::Instant::now(); + counters.solver.velocity_assembly_time_solver_bodies.pause(); + counters + .solver + .velocity_assembly_time_constraints_init + .resume(); self.velocity_solver.init_constraints( island_id, islands, @@ -81,13 +87,11 @@ impl IslandSolver { #[cfg(feature = "dim3")] params.friction_model, ); - // let t_init_constraints = t0.elapsed().as_secs_f32(); + counters + .solver + .velocity_assembly_time_constraints_init + .pause(); counters.solver.velocity_assembly_time.pause(); - // println!( - // "Solver body init: {}, init constraints: {}", - // t_solver_body_init * 1000.0, - // t_init_constraints * 1000.0 - // ); // SOLVE counters.solver.velocity_resolution_time.resume(); diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 2dc702d10..52f8de44e 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -123,12 +123,12 @@ impl JointConstraintBuilderSimd { let rb2 = array![|ii| &bodies[joint[ii].body2]]; let body1 = array![|ii| if rb1[ii].is_dynamic_or_kinematic() { - rb1[ii].ids.active_set_offset + rb1[ii].ids.active_set_id as u32 } else { u32::MAX }]; let body2 = array![|ii| if rb2[ii].is_dynamic_or_kinematic() { - rb2[ii].ids.active_set_offset + rb2[ii].ids.active_set_id as u32 } else { u32::MAX }]; diff --git a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs index c7abe9af2..10f28b934 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs @@ -183,6 +183,7 @@ impl JointConstraintsSet { fn compute_generic_joint_constraints( &mut self, + // TODO: pass around the &Island directly. island_id: usize, islands: &IslandManager, bodies: &RigidBodySet, @@ -193,7 +194,7 @@ impl JointConstraintsSet { // Count the internal and external constraints builder. let num_external_constraint_builders = self.generic_two_body_interactions.len(); let mut num_internal_constraint_builders = 0; - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { if let Some(link_id) = multibodies.rigid_body_link(*handle) { if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0 { @@ -230,7 +231,7 @@ impl JointConstraintsSet { // Generate internal constraints builder. They are indexed after the let mut curr_builder = self.generic_two_body_interactions.len(); - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { if curr_builder >= self.generic_velocity_constraints_builder.len() { break; // No more builder need to be generated. } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index ef2d186e7..21845cbea 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -82,7 +82,7 @@ impl VelocitySolver { self.solver_bodies.clear(); let aligned_solver_bodies_len = - islands.active_island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; + islands.island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; self.solver_bodies.resize(aligned_solver_bodies_len); self.solver_vels_increment.clear(); @@ -97,7 +97,8 @@ impl VelocitySolver { // Assign solver ids to multibodies, and collect the relevant roots. // And init solver_vels for rigid-bodies. let mut multibody_solver_id = 0; - for handle in islands.active_island(island_id) { + + for (offset, handle) in islands.island(island_id).bodies().iter().enumerate() { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { let multibody = multibodies .get_multibody_mut_internal(link.multibody) @@ -110,10 +111,10 @@ impl VelocitySolver { } } else { let rb = &bodies[*handle]; - let solver_vel_incr = - &mut self.solver_vels_increment[rb.ids.active_set_offset as usize]; + assert_eq!(offset, rb.ids.active_set_id); + let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_id]; self.solver_bodies - .copy_from(total_step_dt, rb.ids.active_set_offset as usize, rb); + .copy_from(total_step_dt, rb.ids.active_set_id, rb); solver_vel_incr.angular = rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt; @@ -181,7 +182,7 @@ impl VelocitySolver { if params.warmstart_coefficient != 0.0 { // TODO PERF: we could probably figure out a way to avoid this warmstart when // step_id > 0? Maybe for that to happen `solver_vels` needs to - // represent velocity changes instead of total rigid-boody velocities. + // represent velocity changes instead of total rigid-body velocities. // Need to be careful wrt. multibody and joints too. contact_constraints .warmstart(&mut self.solver_bodies, &mut self.generic_solver_vels); @@ -290,7 +291,7 @@ impl VelocitySolver { bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, ) { - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { let link = if self.multibody_roots.is_empty() { None } else { @@ -310,8 +311,8 @@ impl VelocitySolver { } } else { let rb = bodies.index_mut_internal(*handle); - let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_offset as usize]; - let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_offset as usize]; + let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_id]; + let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_id]; let dangvel = solver_vels.angular; diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 243f5a766..dd530c1bd 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -155,8 +155,6 @@ pub struct ContactPair { /// [`Collider::contact_skin`] which only affects the constraint solver and the /// [`SolverContact`]. pub manifolds: Vec, - /// Is there any active contact in this contact pair? - pub has_any_active_contact: bool, /// Was a `CollisionEvent::Started` emitted for this collider? pub(crate) start_event_emitted: bool, pub(crate) workspace: Option, @@ -173,17 +171,22 @@ impl ContactPair { Self { collider1, collider2, - has_any_active_contact: false, manifolds: Vec::new(), start_event_emitted: false, workspace: None, } } + /// Is there any active contact in this contact pair? + pub fn has_any_active_contact(&self) -> bool { + self.manifolds + .iter() + .any(|m| !m.data.solver_contacts.is_empty()) + } + /// Clears all the contacts of this contact pair. pub fn clear(&mut self) { self.manifolds.clear(); - self.has_any_active_contact = false; self.workspace = None; } diff --git a/src/geometry/manifold_reduction.rs b/src/geometry/manifold_reduction.rs new file mode 100644 index 000000000..027037b27 --- /dev/null +++ b/src/geometry/manifold_reduction.rs @@ -0,0 +1,218 @@ +use crate::geometry::ContactManifold; +use crate::math::Real; +use crate::utils::SimdBasis; + +pub(crate) fn reduce_manifold_naive( + manifold: &ContactManifold, + selected: &mut [usize; 4], + num_selected: &mut usize, + prediction_distance: Real, +) { + if manifold.points.len() <= 4 { + return; + } + + // 1. Find the deepest contact. + *selected = [usize::MAX; 4]; + + let mut deepest_dist = Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + if pt.dist < deepest_dist { + deepest_dist = pt.dist; + selected[0] = i; + } + } + + if selected[0] == usize::MAX { + *num_selected = 0; + return; + } + + // 2. Find the point that is the furthest from the deepest point. + let selected_a = &manifold.points[selected[0]].local_p1; + let mut furthest_dist = -Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + let dist = na::distance_squared(&pt.local_p1, selected_a); + if i != selected[0] && pt.dist <= prediction_distance && dist > furthest_dist { + furthest_dist = dist; + selected[1] = i; + } + } + + if selected[1] == usize::MAX { + *num_selected = 1; + return; + } + + // 3. Now find the two points furthest from the segment we built so far. + let selected_b = &manifold.points[selected[1]].local_p1; + + if selected_a == selected_b { + *num_selected = 1; + return; + } + + let selected_ab = selected_b - selected_a; + let tangent = selected_ab.cross(&manifold.local_n1); + + // Find the points that minimize and maximize the dot product with the tangent. + let mut min_dot = Real::MAX; + let mut max_dot = -Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + if i == selected[0] || i == selected[1] || pt.dist > prediction_distance { + continue; + } + + let dot = (pt.local_p1 - selected_a).dot(&tangent); + if dot < min_dot { + min_dot = dot; + selected[2] = i; + } + + if dot > max_dot { + max_dot = dot; + selected[3] = i; + } + } + + if selected[2] == usize::MAX { + *num_selected = 2; + } else if selected[2] == selected[3] { + *num_selected = 3; + } else { + *num_selected = 4; + } +} + +// Run contact reduction using Bepu's InternalReduce algorithm. +// The general idea is quite similar to our naive approach except that they add some +// additional heuristics. This is implemented mainly for comparison purpose to see +// if there is a strong advantage to having the extra checks. +#[allow(dead_code)] +pub(crate) fn reduce_manifold_bepu_like( + manifold: &ContactManifold, + selected: &mut [usize; 4], + num_selected: &mut usize, +) { + if manifold.points.len() <= 4 { + return; + } + + // Step 1: Find the deepest contact, biased by extremity for frame stability. + // The extremity heuristic helps maintain consistent contact selection across frames + // when multiple contacts have similar depths. + let mut best_score = -Real::MAX; + const EXTREMITY_SCALE: Real = 1e-2; + // Use an arbitrary direction (roughly 38 degrees from X axis) to break ties + const EXTREMITY_DIR_X: Real = 0.7946898; + const EXTREMITY_DIR_Y: Real = 0.6070158; + + let tangents = manifold.local_n1.orthonormal_basis(); + + for (i, pt) in manifold.points.iter().enumerate() { + // Extremity measures how far the contact is from the origin in the tangent plane + let tx1 = pt.local_p1.coords.dot(&tangents[0]); + let ty1 = pt.local_p1.coords.dot(&tangents[1]); + + let extremity = (tx1 * EXTREMITY_DIR_X + ty1 * EXTREMITY_DIR_Y).abs(); + + // Score = depth + small extremity bias (only for non-speculative contacts) + // Negative dist = deeper penetration = higher score + let score = if pt.dist >= 0.0 { + -pt.dist // Speculative contact, no extremity bias + } else { + -pt.dist + extremity * EXTREMITY_SCALE + }; + + if score > best_score { + best_score = score; + selected[0] = i; + } + } + + // Step 2: Find the point most distant from the first contact. + // This establishes a baseline "edge" for the manifold. + let contact0_pos = manifold.points[selected[0]].local_p1; + let mut max_distance_squared = 0.0; + + for (i, pt) in manifold.points.iter().enumerate() { + let offset = pt.local_p1 - contact0_pos; + let offset_x = offset.dot(&tangents[0]); + let offset_y = offset.dot(&tangents[1]); + let distance_squared = offset_x * offset_x + offset_y * offset_y; + + if distance_squared > max_distance_squared { + max_distance_squared = distance_squared; + selected[1] = i; + } + } + + // Early out if the contacts are too close together + let epsilon = 1e-6; + if max_distance_squared <= epsilon { + // Only one meaningful contact + *num_selected = 1; + } else { + // Step 3: Find two more contacts that maximize positive and negative signed area. + // Using the first two contacts as an edge, we look for contacts that form triangles + // with the largest magnitude negative and positive areas. This maximizes the + // spatial extent of the contact manifold. + + *num_selected = 2; + selected[2] = usize::MAX; + selected[3] = usize::MAX; + + let contact1_pos = manifold.points[selected[1]].local_p1; + let edge_offset = contact1_pos - contact0_pos; + let edge_offset_x = edge_offset.dot(&tangents[0]); + let edge_offset_y = edge_offset.dot(&tangents[1]); + + let mut min_signed_area = 0.0; + let mut max_signed_area = 0.0; + + for (i, pt) in manifold.points.iter().enumerate() { + let candidate_offset = pt.local_p1 - contact0_pos; + let candidate_offset_x = candidate_offset.dot(&tangents[0]); + let candidate_offset_y = candidate_offset.dot(&tangents[1]); + + // Signed area of the triangle formed by (contact0, contact1, candidate) + // This is a 2D cross product: (candidate - contact0) × (contact1 - contact0) + let mut signed_area = + candidate_offset_x * edge_offset_y - candidate_offset_y * edge_offset_x; + + // Penalize speculative contacts (they're less important) + if pt.dist >= 0.0 { + signed_area *= 0.25; + } + + if signed_area < min_signed_area { + min_signed_area = signed_area; + selected[2] = i; + } + if signed_area > max_signed_area { + max_signed_area = signed_area; + selected[3] = i; + } + } + + // Check if the signed areas are significant enough + // Epsilon based on the edge length squared + let area_epsilon = max_distance_squared * max_distance_squared * 1e-6; + + // If the areas are too small, don't add those contacts + if min_signed_area * min_signed_area <= area_epsilon { + selected[2] = usize::MAX; + } + if max_signed_area * max_signed_area <= area_epsilon { + selected[3] = usize::MAX; + } + + let keep2 = selected[2] != usize::MAX; + let keep3 = selected[3] != usize::MAX; + *num_selected += keep2 as usize + keep3 as usize; + + if !keep2 { + selected[2] = selected[3]; + } + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index e7c97bb5b..ca0e12e1d 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -231,3 +231,6 @@ mod broad_phase_pair_event; mod collider; mod collider_set; mod mesh_converter; + +#[cfg(feature = "dim3")] +mod manifold_reduction; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 64805a7a7..fef653f3c 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -13,7 +13,7 @@ use crate::geometry::{ ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex, }; -use crate::math::{Real, Vector}; +use crate::math::{MAX_MANIFOLD_POINTS, Real, Vector}; use crate::pipeline::{ ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, @@ -582,7 +582,7 @@ impl NarrowPhase { // Emit a contact stopped event if we had a contact before removing the edge. // Also wake up the dynamic bodies that were in contact. if let Some(mut ctct) = contact_pair { - if ctct.has_any_active_contact { + if ctct.has_any_active_contact() { if let Some(islands) = islands { if let Some(co_parent1) = &co1.parent { islands.wake_up(bodies, co_parent1.handle, true); @@ -808,7 +808,8 @@ impl NarrowPhase { &mut self, prediction_distance: Real, dt: Real, - bodies: &RigidBodySet, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, colliders: &ColliderSet, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, @@ -817,10 +818,10 @@ impl NarrowPhase { ) { let query_dispatcher = &*self.query_dispatcher; - // TODO: don't iterate on all the edges. + // TODO PERF: don't iterate on all the edges. par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; - let had_any_active_contact = pair.has_any_active_contact; + let had_any_active_contact = pair.has_any_active_contact(); let co1 = &colliders[pair.collider1]; let co2 = &colliders[pair.collider2]; @@ -831,7 +832,9 @@ impl NarrowPhase { // No update needed for these colliders. return; } - if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some() + + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) + && co1.parent.is_some() { // Same parents. Ignore collisions. pair.clear(); @@ -858,7 +861,7 @@ impl NarrowPhase { let link1 = multibody_joints.rigid_body_link(co_parent1.handle); let link2 = multibody_joints.rigid_body_link(co_parent2.handle); - if let (Some(link1),Some(link2)) = (link1, link2) { + if let (Some(link1), Some(link2)) = (link1, link2) { // If both bodies belong to the same multibody, apply some additional built-in // contact filtering rules. if link1.multibody == link2.multibody { @@ -936,27 +939,30 @@ impl NarrowPhase { let contact_skin_sum = co1.contact_skin() + co2.contact_skin(); let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); - let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 { + let effective_prediction_distance = + if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 { let aabb1 = co1.compute_collision_aabb(0.0); let aabb2 = co2.compute_collision_aabb(0.0); let inv_dt = crate::utils::inv(dt); - let linvel1 = rb1.map(|rb| rb.linvel() - .cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default(); - let linvel2 = rb2.map(|rb| rb.linvel() - .cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default(); + let linvel1 = rb1 + .map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction1 * inv_dt)) + .unwrap_or_default(); + let linvel2 = rb2 + .map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction2 * inv_dt)) + .unwrap_or_default(); - if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) { + if !aabb1.intersects(&aabb2) + && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) + { pair.clear(); break 'emit_events; } - - prediction_distance.max( - dt * (linvel1 - linvel2).norm()) + contact_skin_sum - } else { - prediction_distance + contact_skin_sum - }; + prediction_distance.max(dt * (linvel1 - linvel2).norm()) + contact_skin_sum + } else { + prediction_distance + contact_skin_sum + }; let _ = query_dispatcher.contact_manifolds( &pos12, @@ -984,8 +990,6 @@ impl NarrowPhase { let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero); let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero); - pair.has_any_active_contact = false; - for manifold in &mut pair.manifolds { let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos); let world_pos2 = manifold.subshape_pos2.prepend_to(&co2.pos); @@ -998,20 +1002,42 @@ impl NarrowPhase { manifold.data.normal = world_pos1 * manifold.local_n1; // Generate solver contacts. - for (contact_id, contact) in manifold.points.iter().enumerate() { - if contact_id > u8::MAX as usize { - log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess."); - break; - } + #[allow(unused_mut)] // Mut not needed in 2D. + let mut selected = [0, 1, 2, 3]; + #[allow(unused_mut)] // Mut not needed in 2D. + let mut num_selected = MAX_MANIFOLD_POINTS.min(manifold.points.len()); + + #[cfg(feature = "dim3")] + // super::manifold_reduction::reduce_manifold_bepu_like( + // manifold, + // &mut selected, + // &mut num_selected, + // ); + #[cfg(feature = "dim3")] + super::manifold_reduction::reduce_manifold_naive( + manifold, + &mut selected, + &mut num_selected, + prediction_distance, + ); - let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin(); + for contact_id in &selected[..num_selected] { + // // manifold.points.iter().enumerate() { + let contact = &manifold.points[*contact_id]; + let effective_contact_dist = + contact.dist - co1.contact_skin() - co2.contact_skin(); let keep_solver_contact = effective_contact_dist < prediction_distance || { let world_pt1 = world_pos1 * contact.local_p1; let world_pt2 = world_pos2 * contact.local_p2; - let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default(); - let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default(); - effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance + let vel1 = rb1 + .map(|rb| rb.velocity_at_point(&world_pt1)) + .unwrap_or_default(); + let vel2 = rb2 + .map(|rb| rb.velocity_at_point(&world_pt2)) + .unwrap_or_default(); + effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt + < prediction_distance }; if keep_solver_contact { @@ -1021,7 +1047,7 @@ impl NarrowPhase { let effective_point = na::center(&world_pt1, &world_pt2); let solver_contact = SolverContact { - contact_id: [contact_id as u32], + contact_id: [*contact_id as u32], point: effective_point, dist: effective_contact_dist, friction, @@ -1039,7 +1065,6 @@ impl NarrowPhase { }; manifold.data.solver_contacts.push(solver_contact); - pair.has_any_active_contact = true; } } @@ -1069,48 +1094,33 @@ impl NarrowPhase { manifold.data.normal = modifiable_normal; manifold.data.user_data = modifiable_user_data; } - - /* - * TODO: When using the block solver in 3D, I’d expect this sort to help, but - * it makes the domino demo worse. Needs more investigation. - fn sort_solver_contacts(mut contacts: &mut [SolverContact]) { - while contacts.len() > 2 { - let first = contacts[0]; - let mut furthest_id = 1; - let mut furthest_dist = na::distance(&first.point, &contacts[1].point); - - for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) { - let candidate_dist = na::distance(&first.point, &candidate.point); - - if candidate_dist > furthest_dist { - furthest_dist = candidate_dist; - furthest_id = candidate_id; - } - } - - if furthest_id > 1 { - contacts.swap(1, furthest_id); - } - - contacts = &mut contacts[2..]; - } - } - - sort_solver_contacts(&mut manifold.data.solver_contacts); - */ } } - let active_events = co1.flags.active_events | co2.flags.active_events; - - if pair.has_any_active_contact != had_any_active_contact - && active_events.contains(ActiveEvents::COLLISION_EVENTS) - { - if pair.has_any_active_contact { - pair.emit_start_event(bodies, colliders, events); - } else { - pair.emit_stop_event(bodies, colliders, events); + /* + * Handle actions on contact start/stop: + * - Emit event (if applicable). + * - Notify the island manager to potentially wake up the bodies. + */ + let has_any_active_contact = pair.has_any_active_contact(); + if has_any_active_contact != had_any_active_contact { + let active_events = co1.flags.active_events | co2.flags.active_events; + if active_events.contains(ActiveEvents::COLLISION_EVENTS) { + if has_any_active_contact { + pair.emit_start_event(bodies, colliders, events); + } else { + pair.emit_stop_event(bodies, colliders, events); + } } + + // FIXME: this won’t work with the parallel feature enabled. + islands.interaction_started_or_stopped( + bodies, + co1.parent.map(|p| p.handle), + co2.parent.map(|p| p.handle), + has_any_active_contact, + true, + ); } }); } @@ -1125,7 +1135,7 @@ impl NarrowPhase { out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_islands()] { + for out_island in &mut out[..islands.active_islands().len()] { out_island.clear(); } @@ -1168,13 +1178,17 @@ impl NarrowPhase { && (!rb_type1.is_dynamic() || !sleeping1) && (!rb_type2.is_dynamic() || !sleeping2) { - let island_index = if !rb_type1.is_dynamic() { - active_island_id2 + let island_awake_index = if !rb_type1.is_dynamic() { + islands.islands[active_island_id2] + .id_in_awake_list() + .expect("Internal error: island should be awake.") } else { - active_island_id1 + islands.islands[active_island_id1] + .id_in_awake_list() + .expect("Internal error: island should be awake.") }; - out[island_index].push(out_manifolds.len()); + out[island_awake_index].push(out_manifolds.len()); out_manifolds.push(manifold); push_pair = true; } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index a93f5554f..5b4457d6b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,6 +1,6 @@ //! Physics pipeline structures. -use crate::dynamics::{ImpulseJointSet, IntegrationParameters, MultibodyJointSet}; +use crate::dynamics::{ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet}; use crate::geometry::{ BroadPhaseBvh, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, ModifiedColliders, NarrowPhase, @@ -53,6 +53,7 @@ impl CollisionPipeline { fn detect_collisions( &mut self, prediction_distance: Real, + islands: &mut IslandManager, broad_phase: &mut BroadPhaseBvh, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, @@ -98,6 +99,7 @@ impl CollisionPipeline { narrow_phase.compute_contacts( prediction_distance, 0.0, + islands, bodies, colliders, &ImpulseJointSet::new(), @@ -126,6 +128,7 @@ impl CollisionPipeline { pub fn step( &mut self, prediction_distance: Real, + islands: &mut IslandManager, broad_phase: &mut BroadPhaseBvh, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, @@ -162,6 +165,7 @@ impl CollisionPipeline { self.detect_collisions( prediction_distance, + islands, broad_phase, narrow_phase, bodies, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 1838893aa..cbd086bfb 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -169,6 +169,7 @@ impl PhysicsPipeline { narrow_phase.compute_contacts( integration_parameters.prediction_distance(), integration_parameters.dt, + islands, bodies, colliders, impulse_joints, @@ -195,7 +196,8 @@ impl PhysicsPipeline { events: &dyn EventHandler, ) { self.counters.stages.island_construction_time.resume(); - islands.update_active_set_with_contacts( + // NOTE: islands update must be done after the narrow-phase. + islands.update_islands( integration_parameters.dt, integration_parameters.length_unit, bodies, @@ -203,19 +205,23 @@ impl PhysicsPipeline { narrow_phase, impulse_joints, multibody_joints, - integration_parameters.min_island_size, ); - if self.manifold_indices.len() < islands.num_islands() { - self.manifold_indices - .resize(islands.num_islands(), Vec::new()); + let num_active_islands = islands.active_islands().len(); + if self.manifold_indices.len() < num_active_islands { + self.manifold_indices.resize(num_active_islands, Vec::new()); } - if self.joint_constraint_indices.len() < islands.num_islands() { + if self.joint_constraint_indices.len() < num_active_islands { self.joint_constraint_indices - .resize(islands.num_islands(), Vec::new()); + .resize(num_active_islands, Vec::new()); } + self.counters.stages.island_construction_time.pause(); + self.counters + .stages + .island_constraints_collection_time + .resume(); let mut manifolds = Vec::new(); narrow_phase.select_active_contacts( islands, @@ -229,13 +235,16 @@ impl PhysicsPipeline { bodies, &mut self.joint_constraint_indices, ); - self.counters.stages.island_construction_time.pause(); + self.counters + .stages + .island_constraints_collection_time + .pause(); self.counters.stages.update_time.resume(); for handle in islands.active_bodies() { // TODO: should that be moved to the solver (just like we moved // the multibody dynamics update) since it depends on dt? - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.mprops .update_world_mass_properties(rb.body_type, &rb.pos.position); let effective_mass = rb.mprops.effective_mass(); @@ -245,26 +254,26 @@ impl PhysicsPipeline { self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); - if self.solvers.len() < islands.num_islands() { + if self.solvers.len() < num_active_islands { self.solvers - .resize_with(islands.num_islands(), IslandSolver::new); + .resize_with(num_active_islands, IslandSolver::new); } #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..islands.num_islands() { - self.solvers[island_id].init_and_solve( - island_id, + for (island_awake_id, island_id) in islands.active_islands().iter().enumerate() { + self.solvers[island_awake_id].init_and_solve( + *island_id, &mut self.counters, integration_parameters, islands, bodies, &mut manifolds[..], - &self.manifold_indices[island_id], + &self.manifold_indices[island_awake_id], impulse_joints.joints_mut(), - &self.joint_constraint_indices[island_id], + &self.joint_constraint_indices[island_awake_id], multibody_joints, ) } @@ -276,8 +285,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = islands.num_islands(); - let solvers = &mut self.solvers[..num_islands]; + let solvers = &mut self.solvers[..num_active_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _); let impulse_joints = @@ -296,7 +304,8 @@ impl PhysicsPipeline { solvers .par_iter_mut() .enumerate() - .for_each(|(island_id, solver)| { + .for_each(|(island_awake_id, solver)| { + let island_id = islands.active_islands()[island_awake_id]; let bodies: &mut RigidBodySet = unsafe { &mut *bodies.load(Ordering::Relaxed) }; let manifolds: &mut Vec<&mut ContactManifold> = @@ -314,9 +323,9 @@ impl PhysicsPipeline { islands, bodies, &mut manifolds[..], - &manifold_indices[island_id], + &manifold_indices[island_awake_id], impulse_joints, - &joint_constraint_indices[island_id], + &joint_constraint_indices[island_awake_id], multibody_joints, ) }); @@ -389,7 +398,7 @@ impl PhysicsPipeline { ) { // Set the rigid-bodies and kinematic bodies to their final position. for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.pos.position = rb.pos.next_position; rb.colliders .update_positions(colliders, modified_colliders, &rb.pos.position); @@ -409,7 +418,7 @@ impl PhysicsPipeline { // bodies it is touching. for handle in islands.active_bodies() { // TODO PERF: only iterate on kinematic position-based bodies - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); match rb.body_type { RigidBodyType::KinematicPositionBased => { @@ -495,16 +504,16 @@ impl PhysicsPipeline { // Apply some of delayed wake-ups. self.counters.stages.user_changes.start(); #[cfg(feature = "enhanced-determinism")] - let impulse_joints_iterator = impulse_joints + let to_wake_up_iterator = impulse_joints .to_wake_up .drain(..) .chain(multibody_joints.to_wake_up.drain(..)); #[cfg(not(feature = "enhanced-determinism"))] - let impulse_joints_iterator = impulse_joints + let to_wake_up_iterator = impulse_joints .to_wake_up .drain() .chain(multibody_joints.to_wake_up.drain()); - for handle in impulse_joints_iterator { + for handle in to_wake_up_iterator { islands.wake_up(bodies, handle, true); } @@ -538,6 +547,27 @@ impl PhysicsPipeline { .copied() .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), ); + + // Join islands based on new joints. + #[cfg(feature = "enhanced-determinism")] + let to_join_iterator = impulse_joints + .to_join + .drain(..) + .chain(multibody_joints.to_join.drain(..)); + #[cfg(not(feature = "enhanced-determinism"))] + let to_join_iterator = impulse_joints + .to_join + .drain() + .chain(multibody_joints.to_join.drain()); + for (handle1, handle2) in to_join_iterator { + islands.interaction_started_or_stopped( + bodies, + Some(handle1), + Some(handle2), + true, + false, + ); + } self.counters.stages.user_changes.pause(); // TODO: do this only on user-change. @@ -643,7 +673,9 @@ impl PhysicsPipeline { self.counters.ccd.num_substeps += 1; + self.counters.custom.resume(); self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies); + self.counters.custom.pause(); self.build_islands_and_solve_velocity_constraints( gravity, &integration_parameters, @@ -705,6 +737,8 @@ impl PhysicsPipeline { } else { // If we ran the last substep, just update the broad-phase bvh instead // of a full collision-detection step. + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.final_broad_phase_time.resume(); for handle in modified_colliders.iter() { let co = colliders.index_mut_internal(*handle); // NOTE: `advance_to_final_positions` might have added disabled colliders to @@ -732,6 +766,8 @@ impl PhysicsPipeline { // Empty the modified colliders set. See comment for `co.change.remove(..)` above. modified_colliders.clear(); + self.counters.cd.final_broad_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); } } @@ -743,7 +779,7 @@ impl PhysicsPipeline { // not modified by the user in the mean time. self.counters.stages.update_time.resume(); for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.mprops .update_world_mass_properties(rb.body_type, &rb.pos.position); } diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e39c06676..1eadfb220 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -63,30 +63,21 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( continue; } - let rb = bodies.index_mut_internal(*handle); - let mut ids = rb.ids; - let changes = rb.changes; - let activation = rb.activation; - { + let rb = bodies.index_mut_internal(*handle); + let changes = rb.changes; + let activation = rb.activation; + if rb.is_enabled() { // The body's status changed. We need to make sure // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { - // Push the body to the active set if it is not inside the active set yet, and - // is not longer sleeping or became dynamic. - if (changes.contains(RigidBodyChanges::SLEEP) || changes.contains(RigidBodyChanges::TYPE)) - && rb.is_enabled() - && !rb.activation.sleeping // May happen if the body was put to sleep manually. - && rb.is_dynamic_or_kinematic() // Only dynamic bodies are in the active dynamic set. - && islands.active_set.get(ids.active_set_id) != Some(handle) - { - ids.active_set_id = islands.active_set.len(); // This will handle the case where the activation_channel contains duplicates. - islands.active_set.push(*handle); - } + islands.rigid_body_updated(*handle, bodies); } } + let rb = bodies.index_mut_internal(*handle); + // Update the colliders' positions. if changes.contains(RigidBodyChanges::POSITION) || changes.contains(RigidBodyChanges::COLLIDERS) @@ -157,7 +148,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ); } - rb.ids = ids; rb.activation = activation; } @@ -166,6 +156,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( if let Some(action) = final_action { match action { FinalAction::RemoveFromIsland => { + let rb = bodies.index_mut_internal(*handle); let ids = rb.ids; islands.rigid_body_removed(*handle, &ids, bodies); } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 91e0ae2c8..1f486c0c3 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -1,5 +1,6 @@ #![allow(dead_code)] +use crate::ui::egui::emath::OrderedFloat; use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3}; use physx::cooking::{ ConvexMeshCookingResult, PxConvexMeshDesc, PxCookingParams, PxHeightFieldDesc, @@ -133,7 +134,7 @@ pub static FOUNDATION: std::cell::RefCell = std::cell::RefC pub struct PhysxWorld { // physics: Physics, - materials: Vec>, + // materials: Vec>, shapes: Vec>, scene: Option>, } @@ -141,7 +142,7 @@ pub struct PhysxWorld { impl Drop for PhysxWorld { fn drop(&mut self) { let scene = self.scene.take(); - // FIXME: we get a segfault if we don't forget the scene. + // FIXME: we get a segfault if we don't leak the scene. std::mem::forget(scene); } } @@ -161,7 +162,6 @@ impl PhysxWorld { FOUNDATION.with(|physics| { let mut physics = physics.borrow_mut(); let mut shapes = Vec::new(); - let mut materials = Vec::new(); let friction_type = if use_two_friction_directions { FrictionType::TwoDirectional @@ -300,10 +300,13 @@ impl PhysxWorld { * Colliders * */ + let mut materials_cache = HashMap::new(); for (_, collider) in colliders.iter() { - if let Some((mut px_shape, px_material, collider_pos)) = - physx_collider_from_rapier_collider(&mut *physics, &collider) - { + if let Some((mut px_shape, collider_pos)) = physx_collider_from_rapier_collider( + &mut *physics, + &collider, + &mut materials_cache, + ) { if let Some(parent_handle) = collider.parent() { let parent_body = &bodies[parent_handle]; @@ -331,7 +334,6 @@ impl PhysxWorld { } shapes.push(px_shape); - materials.push(px_material); } } } @@ -396,7 +398,6 @@ impl PhysxWorld { Self { scene: Some(scene), shapes, - materials, } }) } @@ -557,7 +558,8 @@ impl PhysxWorld { fn physx_collider_from_rapier_collider( physics: &mut PxPhysicsFoundation, collider: &Collider, -) -> Option<(Owner, Owner, Isometry3)> { + materials_cache: &mut HashMap<[OrderedFloat; 2], Owner>, +) -> Option<(Owner, Isometry3)> { let mut local_pose = collider.position_wrt_parent().copied().unwrap_or(na::one()); let cooking_params = PxCookingParams::new(physics).unwrap(); let shape = collider.shape(); @@ -566,14 +568,22 @@ fn physx_collider_from_rapier_collider( } else { ShapeFlags::SimulationShape }; - let mut material = physics - .create_material( - collider.material().friction, - collider.material().friction, - collider.material().restitution, - (), - ) - .unwrap(); + + let material = materials_cache + .entry([ + OrderedFloat(collider.material().friction), + OrderedFloat(collider.material().restitution), + ]) + .or_insert_with(|| { + physics + .create_material( + collider.material().friction, + collider.material().friction, + collider.material().restitution, + (), + ) + .unwrap() + }); let materials = &mut [material.as_mut()][..]; let shape = if let Some(cuboid) = shape.as_cuboid() { @@ -705,7 +715,7 @@ fn physx_collider_from_rapier_collider( return None; }; - shape.map(|s| (s, material, local_pose)) + shape.map(|s| (s, local_pose)) } type PxPhysicsFoundation = PhysicsFoundation; diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 844ae39c0..ff0f1dc02 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -320,6 +320,10 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Broad-phase: {:.2}ms", counters.broad_phase_time_ms() )); + ui.label(format!( + "Final broad-phase: {:.2}ms", + counters.cd.final_broad_phase_time.time_ms() + )); ui.label(format!( "Narrow-phase: {:.2}ms", counters.narrow_phase_time_ms() @@ -332,6 +336,20 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Velocity assembly: {:.2}ms", counters.solver.velocity_assembly_time.time_ms() )); + ui.label(format!( + "> Velocity assembly - solver bodies: {:.2}ms", + counters + .solver + .velocity_assembly_time_solver_bodies + .time_ms() + )); + ui.label(format!( + "> Velocity assembly - constraints init: {:.2}ms", + counters + .solver + .velocity_assembly_time_constraints_init + .time_ms() + )); ui.label(format!( "Velocity resolution: {:.2}ms", counters.velocity_resolution_time_ms() @@ -370,10 +388,16 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Island computation: {:.2}ms", counters.island_construction_time_ms() )); + ui.label(format!( + "Active constraints collection: {:.2}ms", + counters.stages.island_constraints_collection_time.time_ms() + )); + ui.label(format!("Mprops update: {:.2}ms", counters.update_time_ms())); ui.label(format!( "User changes: {:.2}ms", counters.stages.user_changes.time_ms() )); + ui.label(format!("Debug timer: {:.2}ms", counters.custom.time_ms())); }); }