rapier3d/dynamics/
rigid_body_set.rs

1use crate::data::{Arena, HasModifiedFlag, ModifiedObjects};
2use crate::dynamics::{
3    ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle,
4};
5use crate::geometry::ColliderSet;
6use std::ops::{Index, IndexMut};
7
8#[derive(Copy, Clone, Debug, PartialEq, Eq)]
9#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
10/// A pair of rigid body handles.
11pub struct BodyPair {
12    /// The first rigid body handle.
13    pub body1: RigidBodyHandle,
14    /// The second rigid body handle.
15    pub body2: RigidBodyHandle,
16}
17
18impl BodyPair {
19    /// Builds a new pair of rigid-body handles.
20    pub fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
21        BodyPair { body1, body2 }
22    }
23}
24
25pub(crate) type ModifiedRigidBodies = ModifiedObjects<RigidBodyHandle, RigidBody>;
26
27impl HasModifiedFlag for RigidBody {
28    #[inline]
29    fn has_modified_flag(&self) -> bool {
30        self.changes.contains(RigidBodyChanges::MODIFIED)
31    }
32
33    #[inline]
34    fn set_modified_flag(&mut self) {
35        self.changes |= RigidBodyChanges::MODIFIED;
36    }
37}
38
39#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
40#[derive(Clone, Default, Debug)]
41/// A set of rigid bodies that can be handled by a physics pipeline.
42pub struct RigidBodySet {
43    // NOTE: the pub(crate) are needed by the broad phase
44    // to avoid borrowing issues. It is also needed for
45    // parallelism because the `Receiver` breaks the Sync impl.
46    // Could we avoid this?
47    pub(crate) bodies: Arena<RigidBody>,
48    pub(crate) modified_bodies: ModifiedRigidBodies,
49}
50
51impl RigidBodySet {
52    /// Create a new empty set of rigid bodies.
53    pub fn new() -> Self {
54        RigidBodySet {
55            bodies: Arena::new(),
56            modified_bodies: ModifiedObjects::default(),
57        }
58    }
59
60    /// Create a new set of rigid bodies, with an initial capacity.
61    pub fn with_capacity(capacity: usize) -> Self {
62        RigidBodySet {
63            bodies: Arena::with_capacity(capacity),
64            modified_bodies: ModifiedRigidBodies::with_capacity(capacity),
65        }
66    }
67
68    pub(crate) fn take_modified(&mut self) -> ModifiedRigidBodies {
69        std::mem::take(&mut self.modified_bodies)
70    }
71
72    /// The number of rigid bodies on this set.
73    pub fn len(&self) -> usize {
74        self.bodies.len()
75    }
76
77    /// `true` if there are no rigid bodies in this set.
78    pub fn is_empty(&self) -> bool {
79        self.bodies.is_empty()
80    }
81
82    /// Is the given body handle valid?
83    pub fn contains(&self, handle: RigidBodyHandle) -> bool {
84        self.bodies.contains(handle.0)
85    }
86
87    /// Insert a rigid body into this set and retrieve its handle.
88    pub fn insert(&mut self, rb: impl Into<RigidBody>) -> RigidBodyHandle {
89        let mut rb = rb.into();
90        // Make sure the internal links are reset, they may not be
91        // if this rigid-body was obtained by cloning another one.
92        rb.reset_internal_references();
93        rb.changes.set(RigidBodyChanges::all(), true);
94
95        let handle = RigidBodyHandle(self.bodies.insert(rb));
96        // Using push_unchecked because this is a brand new rigid-body with the MODIFIED
97        // flags set but isn’t in the modified_bodies yet.
98        self.modified_bodies
99            .push_unchecked(handle, &mut self.bodies[handle.0]);
100        handle
101    }
102
103    /// Removes a rigid-body, and all its attached colliders and impulse_joints, from these sets.
104    #[profiling::function]
105    pub fn remove(
106        &mut self,
107        handle: RigidBodyHandle,
108        islands: &mut IslandManager,
109        colliders: &mut ColliderSet,
110        impulse_joints: &mut ImpulseJointSet,
111        multibody_joints: &mut MultibodyJointSet,
112        remove_attached_colliders: bool,
113    ) -> Option<RigidBody> {
114        let rb = self.bodies.remove(handle.0)?;
115        /*
116         * Update active sets.
117         */
118        islands.rigid_body_removed(handle, &rb.ids, self);
119
120        /*
121         * Remove colliders attached to this rigid-body.
122         */
123        if remove_attached_colliders {
124            for collider in rb.colliders() {
125                colliders.remove(*collider, islands, self, false);
126            }
127        } else {
128            // If we don’t remove the attached colliders, simply detach them.
129            let colliders_to_detach = rb.colliders().to_vec();
130            for co_handle in colliders_to_detach {
131                colliders.set_parent(co_handle, None, self);
132            }
133        }
134
135        /*
136         * Remove impulse_joints attached to this rigid-body.
137         */
138        impulse_joints.remove_joints_attached_to_rigid_body(handle);
139        multibody_joints.remove_joints_attached_to_rigid_body(handle);
140
141        Some(rb)
142    }
143
144    /// Gets the rigid-body with the given handle without a known generation.
145    ///
146    /// This is useful when you know you want the rigid-body at position `i` but
147    /// don't know what is its current generation number. Generation numbers are
148    /// used to protect from the ABA problem because the rigid-body position `i`
149    /// are recycled between two insertion and a removal.
150    ///
151    /// Using this is discouraged in favor of `self.get(handle)` which does not
152    /// suffer form the ABA problem.
153    pub fn get_unknown_gen(&self, i: u32) -> Option<(&RigidBody, RigidBodyHandle)> {
154        self.bodies
155            .get_unknown_gen(i)
156            .map(|(b, h)| (b, RigidBodyHandle(h)))
157    }
158
159    /// Gets a mutable reference to the rigid-body with the given handle without a known generation.
160    ///
161    /// This is useful when you know you want the rigid-body at position `i` but
162    /// don't know what is its current generation number. Generation numbers are
163    /// used to protect from the ABA problem because the rigid-body position `i`
164    /// are recycled between two insertion and a removal.
165    ///
166    /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
167    /// suffer form the ABA problem.
168    #[cfg(not(feature = "dev-remove-slow-accessors"))]
169    pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut RigidBody, RigidBodyHandle)> {
170        let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
171        let handle = RigidBodyHandle(handle);
172        self.modified_bodies.push_once(handle, rb);
173        Some((rb, handle))
174    }
175
176    /// Gets the rigid-body with the given handle.
177    pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
178        self.bodies.get(handle.0)
179    }
180
181    /// Gets a mutable reference to the rigid-body with the given handle.
182    #[cfg(not(feature = "dev-remove-slow-accessors"))]
183    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
184        let result = self.bodies.get_mut(handle.0)?;
185        self.modified_bodies.push_once(handle, result);
186        Some(result)
187    }
188
189    /// Gets a mutable reference to the two rigid-bodies with the given handles.
190    ///
191    /// If `handle1 == handle2`, only the first returned value will be `Some`.
192    #[cfg(not(feature = "dev-remove-slow-accessors"))]
193    pub fn get_pair_mut(
194        &mut self,
195        handle1: RigidBodyHandle,
196        handle2: RigidBodyHandle,
197    ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
198        if handle1 == handle2 {
199            (self.get_mut(handle1), None)
200        } else {
201            let (mut rb1, mut rb2) = self.bodies.get2_mut(handle1.0, handle2.0);
202            if let Some(rb1) = rb1.as_deref_mut() {
203                self.modified_bodies.push_once(handle1, rb1);
204            }
205            if let Some(rb2) = rb2.as_deref_mut() {
206                self.modified_bodies.push_once(handle2, rb2);
207            }
208            (rb1, rb2)
209        }
210    }
211
212    pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
213        self.bodies.get_mut(handle.0)
214    }
215
216    pub(crate) fn index_mut_internal(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
217        &mut self.bodies[handle.0]
218    }
219
220    // Just a very long name instead of `.get_mut` to make sure
221    // this is really the method we wanted to use instead of `get_mut_internal`.
222    pub(crate) fn get_mut_internal_with_modification_tracking(
223        &mut self,
224        handle: RigidBodyHandle,
225    ) -> Option<&mut RigidBody> {
226        let result = self.bodies.get_mut(handle.0)?;
227        self.modified_bodies.push_once(handle, result);
228        Some(result)
229    }
230
231    /// Iterates through all the rigid-bodies on this set.
232    pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
233        self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
234    }
235
236    /// Iterates mutably through all the rigid-bodies on this set.
237    #[cfg(not(feature = "dev-remove-slow-accessors"))]
238    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
239        self.modified_bodies.clear();
240        let modified_bodies = &mut self.modified_bodies;
241        self.bodies.iter_mut().map(move |(h, b)| {
242            // NOTE: using `push_unchecked` because we just cleared `modified_bodies`
243            //       before iterating.
244            modified_bodies.push_unchecked(RigidBodyHandle(h), b);
245            (RigidBodyHandle(h), b)
246        })
247    }
248
249    /// Update colliders positions after rigid-bodies moved.
250    ///
251    /// When a rigid-body moves, the positions of the colliders attached to it need to be updated.
252    /// This update is generally automatically done at the beginning and the end of each simulation
253    /// step with `PhysicsPipeline::step`. If the positions need to be updated without running a
254    /// simulation step (for example when using the `QueryPipeline` alone), this method can be called
255    /// manually.  
256    pub fn propagate_modified_body_positions_to_colliders(&self, colliders: &mut ColliderSet) {
257        for body in self.modified_bodies.iter().filter_map(|h| self.get(*h)) {
258            if body.changes.contains(RigidBodyChanges::POSITION) {
259                for handle in body.colliders() {
260                    if let Some(collider) = colliders.get_mut(*handle) {
261                        let new_pos = body.position() * collider.position_wrt_parent().unwrap();
262                        collider.set_position(new_pos);
263                    }
264                }
265            }
266        }
267    }
268}
269
270impl Index<RigidBodyHandle> for RigidBodySet {
271    type Output = RigidBody;
272
273    fn index(&self, index: RigidBodyHandle) -> &RigidBody {
274        &self.bodies[index.0]
275    }
276}
277
278impl Index<crate::data::Index> for RigidBodySet {
279    type Output = RigidBody;
280
281    fn index(&self, index: crate::data::Index) -> &RigidBody {
282        &self.bodies[index]
283    }
284}
285
286#[cfg(not(feature = "dev-remove-slow-accessors"))]
287impl IndexMut<RigidBodyHandle> for RigidBodySet {
288    fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
289        let rb = &mut self.bodies[handle.0];
290        self.modified_bodies.push_once(handle, rb);
291        rb
292    }
293}
rapier3d/dynamics/rigid_body_set.rs

rapier3d/dynamics/
rigid_body_set.rs
