From ee504f506219f368569fd8523e858375467b3601 Mon Sep 17 00:00:00 2001 From: Ali Baydur <67105447+yagizgil@users.noreply.github.com> Date: Sat, 7 Mar 2026 11:42:51 +0300 Subject: [PATCH] Perf: optimize kinematic body velocity interpolation by tracking them in islands --- src/data/graph.rs | 23 +++++++++++----------- src/dynamics/island_manager/island.rs | 27 ++++++++++++++++++++++++++ src/dynamics/island_manager/manager.rs | 21 ++++++++++++++++++++ src/pipeline/physics_pipeline.rs | 7 +------ 4 files changed, 61 insertions(+), 17 deletions(-) diff --git a/src/data/graph.rs b/src/data/graph.rs index 12ead1d70..77b781246 100644 --- a/src/data/graph.rs +++ b/src/data/graph.rs @@ -140,17 +140,18 @@ enum Pair { /// Get mutable references at index `a` and `b`. fn index_twice(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> { - if max(a, b) >= arr.len() { - Pair::None - } else if a == b { - Pair::One(&mut arr[max(a, b)]) - } else { - // safe because a, b are in bounds and distinct - unsafe { - let ar = &mut *(arr.get_unchecked_mut(a) as *mut _); - let br = &mut *(arr.get_unchecked_mut(b) as *mut _); - Pair::Both(ar, br) - } + let len = arr.len(); + if a >= len || b >= len { + return Pair::None; + } + if a == b { + return Pair::One(&mut arr[a]); + } + // safe because a, b are in bounds and distinct + unsafe { + let ar = &mut *(arr.get_unchecked_mut(a) as *mut _); + let br = &mut *(arr.get_unchecked_mut(b) as *mut _); + Pair::Both(ar, br) } } diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs index efd750725..da9f9301b 100644 --- a/src/dynamics/island_manager/island.rs +++ b/src/dynamics/island_manager/island.rs @@ -13,14 +13,21 @@ pub(crate) struct Island { /// /// If `None`, the island is sleeping. pub(super) id_in_awake_list: Option, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(super) active_kinematic_bodies: Vec, } impl Island { pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self { + let mut active_kinematic_bodies = Vec::new(); + if rb.is_kinematic() { + active_kinematic_bodies.push(handle); + } Self { bodies: vec![handle], additional_solver_iterations: rb.additional_solver_iterations, id_in_awake_list: None, + active_kinematic_bodies, } } @@ -60,6 +67,8 @@ impl IslandManager { let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); let source_island = &mut self.islands[source_id]; + new_island.active_kinematic_bodies.clear(); + for (id, handle) in new_island.bodies.iter().enumerate() { let rb = bodies.index_mut_internal(*handle); @@ -68,6 +77,19 @@ impl IslandManager { rb.sleep(); } + // If the body is kinematic, add it to the active kinematic bodies list. + if rb.is_kinematic() { + new_island.active_kinematic_bodies.push(*handle); + + if let Some(pos) = source_island + .active_kinematic_bodies + .iter() + .position(|h| *h == *handle) + { + source_island.active_kinematic_bodies.swap_remove(pos); + } + } + let id_to_remove = rb.ids.active_set_id; assert_eq!( @@ -145,6 +167,11 @@ impl IslandManager { rb.wake_up(false); rb.ids.active_island_id = to_keep; rb.ids.active_set_id = target_island.bodies.len(); + + if rb.is_kinematic() { + target_island.active_kinematic_bodies.push(*handle); + } + target_island.bodies.push(*handle); target_island.additional_solver_iterations = target_island .additional_solver_iterations diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index 26e546269..6a5c87a71 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -55,6 +55,13 @@ impl IslandManager { &self.awake_islands } + /// Returns an iterator over the handles of kinematic rigid-bodies that are currently active. + pub(crate) fn active_kinematic_bodies(&self) -> impl Iterator + '_ { + self.awake_islands + .iter() + .flat_map(move |i| self.islands[*i].active_kinematic_bodies.iter().copied()) + } + pub(crate) fn rigid_body_removed_or_disabled( &mut self, removed_handle: RigidBodyHandle, @@ -75,6 +82,15 @@ impl IslandManager { let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); island.bodies.swap_remove(removed_ids.active_set_id); + // If the removed body was kinematic, remove it from the active kinematic bodies list. + if let Some(pos) = island + .active_kinematic_bodies + .iter() + .position(|h| *h == removed_handle) + { + island.active_kinematic_bodies.swap_remove(pos); + } + // Remap the active_set_id of the body we moved with the `swap_remove`. if swapped_handle != removed_handle { let swapped_body = bodies @@ -191,6 +207,11 @@ impl IslandManager { rb.ids.active_island_id = id; rb.ids.active_set_id = target_island.bodies.len(); target_island.bodies.push(handle); + + // If the body is kinematic, add it to the active kinematic bodies list. + if rb.is_kinematic() { + target_island.active_kinematic_bodies.push(handle); + } } else { let mut new_island = Island::singleton(handle, rb); let id = self.free_islands.pop().unwrap_or(self.islands.len()); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 2998ec576..a9111f554 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -412,12 +412,7 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, ) { // Update kinematic bodies velocities. - // TODO: what is the best place for this? It should at least be - // located before the island computation because we test the velocity - // there to determine if this kinematic body should wake-up dynamic - // bodies it is touching. - for handle in islands.active_bodies() { - // TODO PERF: only iterate on kinematic position-based bodies + for handle in islands.active_kinematic_bodies() { let rb = bodies.index_mut_internal(handle); match rb.body_type {