rapier3d/pipeline/
user_changes.rs

1use crate::alloc_prelude::*;
2use crate::dynamics::{
3    ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges,
4    RigidBodyHandle, RigidBodySet,
5};
6use crate::geometry::{
7    ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet,
8    ModifiedColliders,
9};
10
11pub(crate) fn handle_user_changes_to_colliders(
12    bodies: &mut RigidBodySet,
13    colliders: &mut ColliderSet,
14    modified_colliders: &[ColliderHandle],
15) {
16    for handle in modified_colliders {
17        // NOTE: we use `get` because the collider may no longer
18        //       exist if it has been removed.
19        if let Some(co) = colliders.get_mut_internal(*handle) {
20            if co.changes.contains(ColliderChanges::PARENT) {
21                if let Some(co_parent) = co.parent {
22                    let parent_rb = &bodies[co_parent.handle];
23
24                    co.pos = ColliderPosition(parent_rb.pos.position * co_parent.pos_wrt_parent);
25                    co.changes |= ColliderChanges::POSITION;
26                }
27            }
28
29            if co.changes.intersects(
30                ColliderChanges::SHAPE
31                    | ColliderChanges::LOCAL_MASS_PROPERTIES
32                    | ColliderChanges::ENABLED_OR_DISABLED
33                    | ColliderChanges::PARENT,
34            ) {
35                if let Some(rb) = co
36                    .parent
37                    .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle))
38                {
39                    rb.changes |= RigidBodyChanges::LOCAL_MASS_PROPERTIES;
40                }
41            }
42        }
43    }
44}
45
46pub(crate) fn handle_user_changes_to_rigid_bodies(
47    mut islands: Option<&mut IslandManager>,
48    bodies: &mut RigidBodySet,
49    colliders: &mut ColliderSet,
50    impulse_joints: &mut ImpulseJointSet,
51    multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies
52    modified_bodies: &[RigidBodyHandle],
53    modified_colliders: &mut ModifiedColliders,
54) {
55    enum FinalAction {
56        RemoveFromIsland,
57    }
58
59    let mut any_jointed_body_modified = false;
60
61    for handle in modified_bodies {
62        let mut final_action = None;
63        let type_changed;
64
65        if !bodies.contains(*handle) {
66            // The body no longer exists.
67            continue;
68        }
69
70        // A modified body invalidates the solver's persistent joint assembly if
71        // any impulse joint is attached to it (its type, pose, mass properties
72        // or solver settings may be baked into the cached joint builders).
73        any_jointed_body_modified =
74            any_jointed_body_modified || impulse_joints.body_may_have_joints(*handle);
75
76        {
77            let rb = bodies.index_mut_internal(*handle);
78            let changes = rb.changes;
79            let activation = rb.activation;
80
81            if rb.is_enabled() {
82                // The body's status changed. We need to make sure
83                // it is on the correct active set.
84                if let Some(islands) = islands.as_deref_mut() {
85                    islands.rigid_body_updated(*handle, bodies);
86                }
87            }
88
89            let rb = bodies.index_mut_internal(*handle);
90
91            // Update the colliders' positions.
92            if changes.contains(RigidBodyChanges::POSITION)
93                || changes.contains(RigidBodyChanges::COLLIDERS)
94            {
95                rb.colliders
96                    .update_positions(colliders, modified_colliders, &rb.pos.position);
97
98                // Update the world-space mass-properties: the only pre-solver update for
99                // user-moved (or newly inserted) bodies; the regular per-step update happens at
100                // the end of the step, right after pose integration.
101                rb.mprops
102                    .update_world_mass_properties(rb.body_type, &rb.pos.position);
103            }
104
105            type_changed = changes.contains(RigidBodyChanges::TYPE);
106
107            if changes.contains(RigidBodyChanges::DOMINANCE)
108                || changes.contains(RigidBodyChanges::TYPE)
109            {
110                for handle in rb.colliders.0.iter() {
111                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
112                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
113                    // instead of the one passed to this method.
114                    let co = colliders.index_mut_internal(*handle);
115                    modified_colliders.push_once(*handle, co);
116                    co.changes |= ColliderChanges::PARENT_EFFECTIVE_DOMINANCE;
117                }
118            }
119
120            if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) {
121                // Propagate the rigid-body’s enabled/disable status to its colliders.
122                for handle in rb.colliders.0.iter() {
123                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
124                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
125                    // instead of the one passed to this method.
126                    let co = colliders.index_mut_internal(*handle);
127                    modified_colliders.push_once(*handle, co);
128
129                    if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent {
130                        co.flags.enabled = ColliderEnabled::Enabled;
131                    } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled {
132                        co.flags.enabled = ColliderEnabled::DisabledByParent;
133                    }
134
135                    co.changes |= ColliderChanges::ENABLED_OR_DISABLED;
136                }
137
138                // Propagate the rigid-body’s enabled/disable status to its attached impulse joints.
139                let mut joint_island_events = Vec::new();
140                impulse_joints.map_attached_joints_mut(*handle, |rb1, rb2, joint_handle, joint| {
141                    if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody {
142                        joint.data.enabled = JointEnabled::Enabled;
143                        joint_island_events.push(crate::dynamics::ImpulseJointIslandEvent::Link {
144                            handle: joint_handle,
145                            body1: rb1,
146                            body2: rb2,
147                        });
148                    } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled {
149                        joint.data.enabled = JointEnabled::DisabledByAttachedBody;
150                        joint_island_events.push(
151                            crate::dynamics::ImpulseJointIslandEvent::Unlink {
152                                handle: joint_handle,
153                            },
154                        );
155                    }
156                });
157                impulse_joints.island_events.extend(joint_island_events);
158
159                // Persistent islands: a body toggling enabled/disabled changes
160                // which bodies its multibody's connectivity chain spans.
161                if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() {
162                    multibody_joints.island_chain_events.push(link.multibody);
163                }
164
165                // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints.
166
167                // Remove the rigid-body from the island manager.
168                if !rb.enabled {
169                    final_action = Some(FinalAction::RemoveFromIsland);
170                }
171            }
172
173            // NOTE: recompute the mass-properties AFTER dealing with the rigid-body changes
174            //       that imply a collider change (in particular, after propagation of the
175            //       enabled/disabled status).
176            if changes
177                .intersects(RigidBodyChanges::LOCAL_MASS_PROPERTIES | RigidBodyChanges::COLLIDERS)
178            {
179                rb.mprops.recompute_mass_properties_from_colliders(
180                    colliders,
181                    &rb.colliders,
182                    rb.body_type,
183                    &rb.pos.position,
184                );
185            }
186
187            rb.activation = activation;
188        }
189
190        // Adjust some ids, if needed.
191        if let Some(islands) = islands.as_deref_mut() {
192            if let Some(action) = final_action {
193                match action {
194                    FinalAction::RemoveFromIsland => {
195                        let rb = bodies.index_mut_internal(*handle);
196                        let ids = rb.ids;
197                        islands.rigid_body_removed_or_disabled(*handle, &ids, bodies);
198                    }
199                };
200            }
201
202            if type_changed {
203                // Persistent islands: a link recorded while an endpoint was
204                // fixed doesn't connect (and vice versa), so a type change
205                // must update every joint link of this body. (Contact links
206                // are updated by the narrow-phase's modified-colliders pass;
207                // the body's own island membership by `rigid_body_updated`.)
208                let mut joint_island_events = Vec::new();
209                impulse_joints.map_attached_joints_mut(*handle, |rb1, rb2, joint_handle, joint| {
210                    joint_island_events.push(crate::dynamics::ImpulseJointIslandEvent::Unlink {
211                        handle: joint_handle,
212                    });
213                    if joint.data.is_enabled() {
214                        joint_island_events.push(crate::dynamics::ImpulseJointIslandEvent::Link {
215                            handle: joint_handle,
216                            body1: rb1,
217                            body2: rb2,
218                        });
219                    }
220                });
221                impulse_joints.island_events.extend(joint_island_events);
222                if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() {
223                    multibody_joints.island_chain_events.push(link.multibody);
224                }
225            }
226
227            // A moved *fixed* body must wake its joint partners: fixed bodies
228            // are not island members, so their own wake is a no-op, and only
229            // *contact* partners get woken through the modified-colliders
230            // path. (A moved dynamic/kinematic body wakes its whole island,
231            // joint partners included.)
232            let rb = &bodies[*handle];
233            if rb.is_fixed() && rb.changes.contains(RigidBodyChanges::POSITION) {
234                let mut to_wake = Vec::new();
235                impulse_joints.map_attached_joints_mut(*handle, |rb1, rb2, _, _| {
236                    to_wake.push(if rb1 == *handle { rb2 } else { rb1 });
237                });
238                for other in multibody_joints.bodies_attached_with_enabled_joint(*handle) {
239                    to_wake.push(other);
240                }
241                for partner in to_wake {
242                    islands.wake_up(bodies, partner, true);
243                }
244            }
245        }
246    }
247
248    if any_jointed_body_modified {
249        impulse_joints.bump_assembly_epoch();
250    }
251}
rapier3d/pipeline/user_changes.rs

rapier3d/pipeline/
user_changes.rs
