rapier2d/dynamics/island_manager/
manager.rs

1use super::{Island, IslandsOptimizer};
2use crate::dynamics::{
3    ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyIds,
4    RigidBodySet,
5};
6use crate::geometry::{ColliderSet, NarrowPhase};
7use crate::math::Real;
8use crate::prelude::SleepRootState;
9use crate::utils::DotProduct;
10use std::collections::VecDeque;
11use vec_map::VecMap;
12
13/// An island starting at this rigid-body might be eligible for sleeping.
14#[derive(Copy, Clone, Debug, PartialEq)]
15#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
16pub(super) struct SleepCandidate(RigidBodyHandle);
17
18/// System that manages which bodies are active (awake) vs sleeping to optimize performance.
19///
20/// ## Sleeping Optimization
21///
22/// Bodies at rest automatically "sleep" - they're excluded from simulation until something
23/// disturbs them (collision, joint connection to moving body, manual wake-up). This can
24/// dramatically improve performance in scenes with many static/resting objects.
25///
26/// ## Islands
27///
28/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together.
29/// This allows parallel solving and better organization.
30///
31/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline).
32#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
33#[derive(Clone, Default)]
34pub struct IslandManager {
35    pub(crate) islands: VecMap<Island>,
36    pub(crate) awake_islands: Vec<usize>,
37    // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation?
38    pub(crate) free_islands: Vec<usize>,
39    /// Potential candidate roots for graph traversal to identify a sleeping
40    /// connected component or to split an island in two.
41    pub(super) traversal_candidates: VecDeque<SleepCandidate>,
42    pub(super) traversal_timestamp: u32,
43    pub(super) optimizer: IslandsOptimizer,
44    #[cfg_attr(feature = "serde-serialize", serde(skip))]
45    pub(super) stack: Vec<RigidBodyHandle>, // Workspace.
46}
47
48impl IslandManager {
49    /// Creates a new empty island manager.
50    pub fn new() -> Self {
51        Self::default()
52    }
53
54    pub(crate) fn active_islands(&self) -> &[usize] {
55        &self.awake_islands
56    }
57
58    pub(crate) fn rigid_body_removed_or_disabled(
59        &mut self,
60        removed_handle: RigidBodyHandle,
61        removed_ids: &RigidBodyIds,
62        bodies: &mut RigidBodySet,
63    ) {
64        let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else {
65            // The island already doesn’t exist.
66            return;
67        };
68
69        // If the rigid-body was disabled, it is still in the body set. Invalid its islands ids.
70        if let Some(body) = bodies.get_mut_internal(removed_handle) {
71            body.ids.active_island_id = usize::MAX;
72            body.ids.active_set_id = usize::MAX;
73        }
74
75        let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle);
76        island.bodies.swap_remove(removed_ids.active_set_id);
77
78        // Remap the active_set_id of the body we moved with the `swap_remove`.
79        if swapped_handle != removed_handle {
80            let swapped_body = bodies
81                .get_mut(swapped_handle)
82                .expect("Internal error: bodies must be removed from islands on at a times");
83            swapped_body.ids.active_set_id = removed_ids.active_set_id;
84        }
85
86        // If we deleted the last body from this island, delete the island.
87        if island.bodies.is_empty() {
88            if let Some(awake_id) = island.id_in_awake_list {
89                // Remove it from the free island list.
90                self.awake_islands.swap_remove(awake_id);
91                // Update the awake list index of the awake island id we moved.
92                if let Some(moved_id) = self.awake_islands.get(awake_id) {
93                    self.islands[*moved_id].id_in_awake_list = Some(awake_id);
94                }
95            }
96            self.islands.remove(removed_ids.active_island_id);
97            self.free_islands.push(removed_ids.active_island_id);
98        }
99    }
100
101    pub(crate) fn interaction_started_or_stopped(
102        &mut self,
103        bodies: &mut RigidBodySet,
104        handle1: Option<RigidBodyHandle>,
105        handle2: Option<RigidBodyHandle>,
106        started: bool,
107        wake_up: bool,
108    ) {
109        match (handle1, handle2) {
110            (Some(handle1), Some(handle2)) => {
111                if wake_up {
112                    self.wake_up(bodies, handle1, false);
113                    self.wake_up(bodies, handle2, false);
114                }
115
116                if started {
117                    if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) {
118                        assert!(rb1.is_fixed() || rb1.ids.active_island_id != usize::MAX);
119                        assert!(rb2.is_fixed() || rb2.ids.active_island_id != usize::MAX);
120
121                        // If both bodies are not part of the same island, merge the islands.
122                        if !rb1.is_fixed()
123                            && !rb2.is_fixed()
124                            && rb1.ids.active_island_id != rb2.ids.active_island_id
125                        {
126                            self.merge_islands(
127                                bodies,
128                                rb1.ids.active_island_id,
129                                rb2.ids.active_island_id,
130                            );
131                        }
132                    }
133                }
134            }
135            (Some(handle1), None) => {
136                if wake_up {
137                    // NOTE: see NOTE of the Some(_), Some(_) case.
138                    self.wake_up(bodies, handle1, false);
139                }
140            }
141            (None, Some(handle2)) => {
142                if wake_up {
143                    // NOTE: see NOTE of the Some(_), Some(_) case.
144                    self.wake_up(bodies, handle2, false);
145                }
146            }
147            (None, None) => { /* Nothing to do. */ }
148        }
149    }
150
151    pub(crate) fn island(&self, island_id: usize) -> &Island {
152        &self.islands[island_id]
153    }
154
155    /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping).
156    #[inline]
157    pub fn active_bodies(&self) -> impl Iterator<Item = RigidBodyHandle> + '_ {
158        self.awake_islands
159            .iter()
160            .flat_map(|i| self.islands[*i].bodies.iter().copied())
161    }
162
163    pub(crate) fn rigid_body_updated(
164        &mut self,
165        handle: RigidBodyHandle,
166        bodies: &mut RigidBodySet,
167    ) {
168        let Some(rb) = bodies.get_mut(handle) else {
169            return;
170        };
171
172        if rb.is_fixed() {
173            return;
174        }
175
176        // Check if this is the first time we see this rigid-body.
177        if rb.ids.active_island_id == usize::MAX {
178            // Check if there is room in the last awake island to add this body.
179            // NOTE: only checking the last is suboptimal. Perhaps we should keep vec of
180            //       small islands ids?
181            let insert_in_last_island = self.awake_islands.last().map(|id| {
182                self.islands[*id].bodies.len() < self.optimizer.min_island_size
183                    && self.islands[*id].is_sleeping() == rb.is_sleeping()
184            });
185            // let insert_in_last_island = insert_in_last_island.is_some().then_some(true);
186
187            if !rb.is_sleeping() && insert_in_last_island == Some(true) {
188                let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!());
189                let target_island = &mut self.islands[id];
190
191                rb.ids.active_island_id = id;
192                rb.ids.active_set_id = target_island.bodies.len();
193                target_island.bodies.push(handle);
194            } else {
195                let mut new_island = Island::singleton(handle, rb);
196                let id = self.free_islands.pop().unwrap_or(self.islands.len());
197
198                if !rb.is_sleeping() {
199                    new_island.id_in_awake_list = Some(self.awake_islands.len());
200                    self.awake_islands.push(id);
201                }
202
203                self.islands.insert(id, new_island);
204                rb.ids.active_island_id = id;
205                rb.ids.active_set_id = 0;
206            }
207        }
208
209        // Push the body to the active set if it is not inside the active set yet, and
210        // is not longer sleeping or became dynamic.
211        if (rb.changes.contains(RigidBodyChanges::SLEEP)
212            || rb.changes.contains(RigidBodyChanges::TYPE))
213            && rb.is_enabled()
214            // Don’t wake up if the user put it to sleep manually.
215            && !rb.activation.sleeping
216        {
217            self.wake_up(bodies, handle, false);
218        }
219    }
220
221    pub(crate) fn update_islands(
222        &mut self,
223        dt: Real,
224        length_unit: Real,
225        bodies: &mut RigidBodySet,
226        colliders: &ColliderSet,
227        narrow_phase: &NarrowPhase,
228        impulse_joints: &ImpulseJointSet,
229        multibody_joints: &MultibodyJointSet,
230    ) {
231        // 1. Update active rigid-bodies energy.
232        // TODO PERF: should this done by the velocity solver after solving the constraints?
233        // let t0 = std::time::Instant::now();
234        for handle in self
235            .awake_islands
236            .iter()
237            .flat_map(|i| self.islands[*i].bodies.iter().copied())
238        {
239            let Some(rb) = bodies.get_mut_internal(handle) else {
240                // This branch happens if the rigid-body no longer exists.
241                continue;
242            };
243            let sq_linvel = rb.vels.linvel.length_squared();
244            let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
245            rb.activation
246                .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt);
247
248            let can_sleep_now = rb.activation.is_eligible_for_sleep();
249
250            // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep"
251            //    and push the sleep root candidate if applicable.
252            if can_sleep_now && rb.activation.sleep_root_state == SleepRootState::Unknown {
253                // This is a new candidate for island extraction.
254                self.traversal_candidates.push_back(SleepCandidate(handle));
255                rb.activation.sleep_root_state = SleepRootState::TraversalPending;
256            } else if !can_sleep_now {
257                rb.activation.sleep_root_state = SleepRootState::Unknown;
258            }
259        }
260        // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0);
261
262        let mut cost = 0;
263
264        // 3. Perform one, or multiple, sleeping islands extraction (graph traversal).
265        //    Limit the traversal cost by not traversing all the known sleeping roots if
266        //    there are too many.
267        const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value.
268        while let Some(sleep_root) = self.traversal_candidates.pop_front() {
269            cost += self.extract_sleeping_island(
270                bodies,
271                colliders,
272                impulse_joints,
273                multibody_joints,
274                narrow_phase,
275                sleep_root.0,
276            );
277
278            if cost > MAX_PER_FRAME_COST {
279                // Early-break if we consider we have done enough island extraction work.
280                break;
281            }
282        }
283
284        self.update_optimizer(
285            bodies,
286            colliders,
287            impulse_joints,
288            multibody_joints,
289            narrow_phase,
290        );
291        // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0);
292
293        // NOTE: uncomment for debugging.
294        // self.assert_state_is_valid(bodies, colliders, narrow_phase);
295    }
296}
rapier2d/dynamics/island_manager/manager.rs

rapier2d/dynamics/island_manager/
manager.rs
