Perf: optimize kinematic body velocity interpolation by tracking them…

src/data/graph.rs

@@ -140,17 +140,18 @@ enum Pair<T> {
 
 /// Get mutable references at index `a` and `b`.
 fn index_twice<T>(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)
     }
 }
 

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<usize>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(super) active_kinematic_bodies: Vec<RigidBodyHandle>,
 }
 
 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

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<Item = RigidBodyHandle> + '_ {
+        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());

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 {