1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
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::new(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);
}
}
}
}
}