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
use generational_arena::Arena;
use std::hash::Hash;

use crate::object::{Body, DefaultColliderHandle, Multibody, RigidBody};
use crate::world::GeometricalWorld;
use na::RealField;

/// Trait auto-implemented for types that can be used as a Body handle.
///
/// Body handles must be unique, i.e., they should not suffer from the ABA problem.
pub trait BodyHandle: Copy + Hash + PartialEq + Eq + 'static + Send + Sync {}

impl<T: Copy + Hash + PartialEq + Eq + 'static + Send + Sync> BodyHandle for T {}

/// Trait implemented by sets of bodies.
///
/// A set of bodies maps a body handle to a body instance. In addition, it must maintain a set of
/// body handle of bodies that have been removed (see the `pop_removal_event` method for details).
pub trait BodySet<N: RealField> {
    /// Type of a body handle identifying a body in this set.
    type Handle: BodyHandle;

    /// Gets a reference to the body identified by `handle`.
    fn get(&self, handle: Self::Handle) -> Option<&dyn Body<N>>;
    /// Gets a mutable reference to the body identified by `handle`.
    fn get_mut(&mut self, handle: Self::Handle) -> Option<&mut dyn Body<N>>;

    /// Gets a reference to the two bodies identified by `handle1` and `handle2`.
    ///
    /// Both handles are allowed to be equal.
    fn get_pair(
        &self,
        handle1: Self::Handle,
        handle2: Self::Handle,
    ) -> (Option<&dyn Body<N>>, Option<&dyn Body<N>>) {
        (self.get(handle1), self.get(handle2))
    }

    /// Check if this set contains a body identified by `handle`.
    fn contains(&self, handle: Self::Handle) -> bool;

    /// Iterate through all the bodies on this set, applying the closure `f` on them.
    fn foreach(&self, f: &mut dyn FnMut(Self::Handle, &dyn Body<N>));
    /// Mutable iterate through all the bodies on this set, applying the closure `f` on them.
    fn foreach_mut(&mut self, f: &mut dyn FnMut(Self::Handle, &mut dyn Body<N>));

    /// Gets the handle of one body that has been removed.
    ///
    /// A body set must keep track (using typically a stack or a queue) of every body that has been
    /// removed from it. This is used by  nphysics to perform some internal cleanup actions, or
    /// physical actions like waking bodies touching one that has been removed.
    ///
    /// This method should return a removed body handle only once.
    fn pop_removal_event(&mut self) -> Option<Self::Handle>;
}

/// The default set containing all the bodies added to the world.
///
/// It is based on an arena using generational indices to avoid the ABA problem.
pub struct DefaultBodySet<N: RealField> {
    bodies: Arena<Box<dyn Body<N>>>,
    removed: Vec<DefaultBodyHandle>,
}

impl<N: RealField> DefaultBodySet<N> {
    /// Creates an empty set.
    pub fn new() -> Self {
        DefaultBodySet {
            bodies: Arena::new(),
            removed: Vec::new(),
        }
    }

    /// Adds a body to this set.
    pub fn insert(&mut self, body: impl Body<N>) -> DefaultBodyHandle {
        self.bodies.insert(Box::new(body))
    }

    /// Adds a body (represented as a boxed trait-object) to this set.
    pub fn insert_boxed(&mut self, body: Box<dyn Body<N>>) -> DefaultBodyHandle {
        self.bodies.insert(body)
    }

    /// Removes a body from this set.
    pub fn remove(&mut self, to_remove: DefaultBodyHandle) -> Option<Box<dyn Body<N>>> {
        let res = self.bodies.remove(to_remove)?;
        self.removed.push(to_remove);
        Some(res)
    }

    /// Check if this set contains a body identified by `handle`.
    fn contains(&self, handle: DefaultBodyHandle) -> bool {
        self.bodies.contains(handle)
    }

    /// Gets a reference to the body identified by `handle`.
    pub fn get(&self, handle: DefaultBodyHandle) -> Option<&dyn Body<N>> {
        self.bodies.get(handle).map(|b| &**b)
    }

    /// Gets a mutable reference to the body identified by `handle`.
    pub fn get_mut(&mut self, handle: DefaultBodyHandle) -> Option<&mut dyn Body<N>> {
        self.bodies.get_mut(handle).map(|b| &mut **b)
    }

    /// Iterate through all the bodies and their handles.
    pub fn iter(&self) -> impl Iterator<Item = (DefaultBodyHandle, &dyn Body<N>)> {
        self.bodies.iter().map(|b| (b.0, &**b.1))
    }

    /// Mutably iterate through all the bodies and their handles.
    pub fn iter_mut(&mut self) -> impl Iterator<Item = (DefaultBodyHandle, &mut dyn Body<N>)> {
        self.bodies.iter_mut().map(|b| (b.0, &mut **b.1))
    }

    /// Gets the rigid body identified by `handle`.
    ///
    /// Returns `None` if the body does not exists, of if it exists but is not a rigid body.
    pub fn rigid_body(&self, handle: DefaultBodyHandle) -> Option<&RigidBody<N>> {
        self.get(handle).and_then(|b| b.downcast_ref())
    }

    /// Gets a mutable reference to the rigid body identified by `handle`.
    ///
    /// Returns `None` if the body does not exists, of if it exists but is not a rigid body.
    pub fn rigid_body_mut(&mut self, handle: DefaultBodyHandle) -> Option<&mut RigidBody<N>> {
        self.get_mut(handle).and_then(|b| b.downcast_mut())
    }

    /// Gets the multibody identified by `handle`.
    ///
    /// Returns `None` if the body does not exists, of if it exists but is not a multibody.
    pub fn multibody(&self, handle: DefaultBodyHandle) -> Option<&Multibody<N>> {
        self.get(handle).and_then(|b| b.downcast_ref())
    }

    /// Gets a mutable reference to the multibody identified by `handle`.
    ///
    /// Returns `None` if the body does not exists, of if it exists but is not a multibody.
    pub fn multibody_mut(&mut self, handle: DefaultBodyHandle) -> Option<&mut Multibody<N>> {
        self.get_mut(handle).and_then(|b| b.downcast_mut())
    }
}

impl<N: RealField> BodySet<N> for DefaultBodySet<N> {
    type Handle = DefaultBodyHandle;

    fn get(&self, handle: Self::Handle) -> Option<&dyn Body<N>> {
        self.get(handle)
    }

    fn get_mut(&mut self, handle: Self::Handle) -> Option<&mut dyn Body<N>> {
        self.get_mut(handle)
    }

    fn contains(&self, handle: Self::Handle) -> bool {
        self.contains(handle)
    }

    fn foreach(&self, f: &mut dyn FnMut(Self::Handle, &dyn Body<N>)) {
        for (h, b) in self.iter() {
            f(h, b)
        }
    }

    fn foreach_mut(&mut self, f: &mut dyn FnMut(Self::Handle, &mut dyn Body<N>)) {
        for (h, b) in self.iter_mut() {
            f(h, b)
        }
    }

    fn pop_removal_event(&mut self) -> Option<Self::Handle> {
        self.removed.pop()
    }
}

/// The body handle used by the `DefaultBodySet`.
pub type DefaultBodyHandle = generational_arena::Index;
/// The body part handle used by the `DefaultBodySet`.
pub type DefaultBodyPartHandle = BodyPartHandle<DefaultBodyHandle>;

/// A unique identifier of a body part added to the world.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
pub struct BodyPartHandle<Handle: BodyHandle>(pub Handle, pub usize);

/// A abstract body descriptor to be passed to the physics `World` to create a body.
pub trait BodyDesc<N: RealField> {
    /// The type of body being generated.
    type Body: Body<N>;

    /// Called by the `World` to create a body with the given allocated handle.
    fn build_with_handle(
        &self,
        gworld: &mut GeometricalWorld<N, DefaultBodyHandle, DefaultColliderHandle>,
        handle: DefaultBodyHandle,
    ) -> Self::Body;
}