ncollide3d/partitioning/
dbvt.rs

1use crate::bounding_volume::BoundingVolume;
2use crate::math::Point;
3use crate::partitioning::BVH;
4use na::{self, RealField};
5use slab::Slab;
6use std::ops::Index;
7
8#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
9/// The unique identifier of a DBVT leaf.
10pub struct DBVTLeafId(usize);
11
12impl DBVTLeafId {
13    /// Creates an invalid identifier.
14    #[inline]
15    pub fn new_invalid() -> Self {
16        DBVTLeafId(usize::max_value())
17    }
18
19    /// Checkis if this identifier is invalid.
20    #[inline]
21    pub fn is_invalid(&self) -> bool {
22        let DBVTLeafId(val) = *self;
23        val == usize::max_value()
24    }
25}
26
27#[derive(Copy, Clone, Debug)]
28enum UpdateStatus {
29    NeedsShrink,
30    UpToDate,
31}
32
33#[derive(Copy, Clone, Debug, Hash)]
34enum DBVTInternalId {
35    RightChildOf(usize),
36    LeftChildOf(usize),
37    Root,
38}
39
40/// The identifier of a node of the DBVT.
41#[derive(Copy, Clone, Debug, Hash)]
42pub enum DBVTNodeId {
43    /// Id of a leaf.
44    Leaf(usize),
45    /// Id of an internal node.
46    Internal(usize),
47}
48
49/// A bounding volume hierarchy on which objects can be added or removed after construction.
50#[derive(Clone)]
51pub struct DBVT<N: RealField + Copy, T, BV> {
52    root: DBVTNodeId,
53    leaves: Slab<DBVTLeaf<N, T, BV>>,
54    internals: Slab<DBVTInternal<N, BV>>,
55}
56
57/// Leaf of a Dynamic Bounding Volume Tree.
58#[derive(Clone)]
59pub struct DBVTLeaf<N: RealField + Copy, T, BV> {
60    /// The bounding volume of this node.
61    pub bounding_volume: BV,
62    /// The center of this node bounding volume.
63    pub center: Point<N>,
64    /// An user-defined data.
65    pub data: T,
66    /// This node parent.
67    parent: DBVTInternalId,
68}
69
70/// Internal node of a DBVT. An internal node always has two children.
71#[derive(Clone)]
72struct DBVTInternal<N: RealField + Copy, BV> {
73    /// The bounding volume of this node. It always encloses both its children bounding volumes.
74    bounding_volume: BV,
75    /// The center of this node bounding volume.
76    center: Point<N>,
77    /// This node left child.
78    left: DBVTNodeId,
79    /// This node right child.
80    right: DBVTNodeId,
81    /// This node parent.
82    parent: DBVTInternalId,
83
84    state: UpdateStatus,
85}
86
87impl<N: RealField + Copy, T, BV: BoundingVolume<N>> DBVTLeaf<N, T, BV> {
88    /// Creates a new DBVT leaf from its bounding volume and contained data.
89    pub fn new(bounding_volume: BV, data: T) -> DBVTLeaf<N, T, BV> {
90        DBVTLeaf {
91            center: bounding_volume.center(),
92            bounding_volume: bounding_volume,
93            data: data,
94            parent: DBVTInternalId::Root,
95        }
96    }
97
98    /// Returns `true` if this leaf is the root of the tree, or if it detached from any tree.
99    pub fn is_root(&self) -> bool {
100        match self.parent {
101            DBVTInternalId::Root => true,
102            _ => false,
103        }
104    }
105}
106
107impl<N: RealField + Copy, BV: BoundingVolume<N>> DBVTInternal<N, BV> {
108    /// Creates a new internal node.
109    fn new(
110        bounding_volume: BV,
111        parent: DBVTInternalId,
112        left: DBVTNodeId,
113        right: DBVTNodeId,
114    ) -> DBVTInternal<N, BV> {
115        DBVTInternal {
116            center: bounding_volume.center(),
117            bounding_volume: bounding_volume,
118            left: left,
119            right: right,
120            parent: parent,
121            state: UpdateStatus::UpToDate,
122        }
123    }
124}
125
126impl<N: RealField + Copy, T, BV: BoundingVolume<N>> DBVT<N, T, BV> {
127    /// Creates a new empty dynamic bonding volume hierarchy.
128    pub fn new() -> DBVT<N, T, BV> {
129        DBVT {
130            root: DBVTNodeId::Leaf(0),
131            leaves: Slab::new(),
132            internals: Slab::new(),
133        }
134    }
135
136    /// The bounding volume of the root of this DBVT.
137    ///
138    /// Returns `None` if the DBVT is empty.
139    #[inline]
140    pub fn root_bounding_volume(&self) -> Option<&BV> {
141        if self.leaves.len() == 0 {
142            return None;
143        }
144
145        match self.root {
146            DBVTNodeId::Leaf(i) => Some(&self.leaves[i].bounding_volume),
147            DBVTNodeId::Internal(i) => Some(&self.internals[i].bounding_volume),
148        }
149    }
150
151    /// Indicates whether this DBVT empty.
152    #[inline]
153    pub fn is_empty(&self) -> bool {
154        self.leaves.is_empty()
155    }
156
157    /// Inserts a leaf into this DBVT.
158    pub fn insert(&mut self, leaf: DBVTLeaf<N, T, BV>) -> DBVTLeafId {
159        if self.is_empty() {
160            let new_id = self.leaves.insert(leaf);
161            self.leaves[new_id].parent = DBVTInternalId::Root;
162            self.root = DBVTNodeId::Leaf(new_id);
163
164            return DBVTLeafId(new_id);
165        }
166
167        match self.root {
168            DBVTNodeId::Internal(_) => {
169                let mut curr = self.root;
170
171                loop {
172                    match curr {
173                        DBVTNodeId::Internal(id) => {
174                            // FIXME: we could avoid the systematic merge
175                            let (left, right) = {
176                                let node = &mut self.internals[id];
177                                node.bounding_volume.merge(&leaf.bounding_volume);
178                                (node.left, node.right)
179                            };
180
181                            let dist1 = match left {
182                                DBVTNodeId::Leaf(l) => {
183                                    na::distance_squared(&self.leaves[l].center, &leaf.center)
184                                }
185                                DBVTNodeId::Internal(i) => {
186                                    na::distance_squared(&self.internals[i].center, &leaf.center)
187                                }
188                            };
189
190                            let dist2 = match right {
191                                DBVTNodeId::Leaf(l) => {
192                                    na::distance_squared(&self.leaves[l].center, &leaf.center)
193                                }
194                                DBVTNodeId::Internal(i) => {
195                                    na::distance_squared(&self.internals[i].center, &leaf.center)
196                                }
197                            };
198
199                            curr = if dist1 < dist2 { left } else { right };
200                        }
201                        DBVTNodeId::Leaf(id) => {
202                            let parent_bv = self.leaves[id]
203                                .bounding_volume
204                                .merged(&leaf.bounding_volume);
205                            let grand_parent = self.leaves[id].parent;
206
207                            let new_id = self.leaves.insert(leaf);
208                            let parent = DBVTInternal::new(
209                                parent_bv,
210                                grand_parent,
211                                curr,
212                                DBVTNodeId::Leaf(new_id),
213                            );
214                            let parent_id = self.internals.insert(parent);
215                            self.leaves[id].parent = DBVTInternalId::LeftChildOf(parent_id);
216                            self.leaves[new_id].parent = DBVTInternalId::RightChildOf(parent_id);
217
218                            match grand_parent {
219                                DBVTInternalId::LeftChildOf(pp) => {
220                                    self.internals[pp].left = DBVTNodeId::Internal(parent_id)
221                                }
222                                DBVTInternalId::RightChildOf(pp) => {
223                                    self.internals[pp].right = DBVTNodeId::Internal(parent_id)
224                                }
225                                _ => unreachable!(),
226                            }
227
228                            break DBVTLeafId(new_id);
229                        }
230                    }
231                }
232            }
233            DBVTNodeId::Leaf(id) => {
234                let new_id = self.leaves.insert(leaf);
235
236                // Create a common parent which is the new root.
237                let root_bv = self.leaves[id]
238                    .bounding_volume
239                    .merged(&self.leaves[new_id].bounding_volume);
240                let root = DBVTInternal::new(
241                    root_bv,
242                    DBVTInternalId::Root,
243                    DBVTNodeId::Leaf(id),
244                    DBVTNodeId::Leaf(new_id),
245                );
246
247                let root_id = self.internals.insert(root);
248                self.leaves[id].parent = DBVTInternalId::LeftChildOf(root_id);
249                self.leaves[new_id].parent = DBVTInternalId::RightChildOf(root_id);
250                self.root = DBVTNodeId::Internal(root_id);
251
252                DBVTLeafId(new_id)
253            }
254        }
255    }
256
257    /// Removes a leaf from this DBVT.
258    ///
259    /// Panics if the provided leaf is not attached to this DBVT.
260    pub fn remove(&mut self, leaf_id: DBVTLeafId) -> DBVTLeaf<N, T, BV> {
261        let DBVTLeafId(leaf_id) = leaf_id;
262        let leaf = self.leaves.remove(leaf_id);
263
264        if !leaf.is_root() {
265            let p;
266            let other;
267
268            match leaf.parent {
269                DBVTInternalId::RightChildOf(parent) => {
270                    other = self.internals[parent].left;
271                    p = parent;
272                }
273                DBVTInternalId::LeftChildOf(parent) => {
274                    other = self.internals[parent].right;
275                    p = parent;
276                }
277                DBVTInternalId::Root => unreachable!(),
278            }
279
280            match self.internals[p].parent {
281                DBVTInternalId::RightChildOf(pp) => {
282                    match other {
283                        DBVTNodeId::Internal(id) => {
284                            self.internals[id].parent = DBVTInternalId::RightChildOf(pp)
285                        }
286                        DBVTNodeId::Leaf(id) => {
287                            self.leaves[id].parent = DBVTInternalId::RightChildOf(pp)
288                        }
289                    }
290
291                    self.internals[pp].right = other;
292                    self.internals[pp].state = UpdateStatus::NeedsShrink;
293                }
294                DBVTInternalId::LeftChildOf(pp) => {
295                    match other {
296                        DBVTNodeId::Internal(id) => {
297                            self.internals[id].parent = DBVTInternalId::LeftChildOf(pp)
298                        }
299                        DBVTNodeId::Leaf(id) => {
300                            self.leaves[id].parent = DBVTInternalId::LeftChildOf(pp)
301                        }
302                    }
303
304                    self.internals[pp].left = other;
305                    self.internals[pp].state = UpdateStatus::NeedsShrink;
306                }
307                DBVTInternalId::Root => {
308                    // The root changes to the other child.
309                    match other {
310                        DBVTNodeId::Leaf(id) => self.leaves[id].parent = DBVTInternalId::Root,
311                        DBVTNodeId::Internal(id) => {
312                            self.internals[id].parent = DBVTInternalId::Root
313                        }
314                    }
315
316                    self.root = other;
317                }
318            }
319
320            let _ = self.internals.remove(p);
321        } else {
322            // The tree is now empty.
323            self.leaves.clear();
324            self.internals.clear();
325        }
326
327        leaf
328    }
329
330    /// Gets the given leaf if it exists.
331    #[inline]
332    pub fn get(&self, DBVTLeafId(id): DBVTLeafId) -> Option<&DBVTLeaf<N, T, BV>> {
333        self.leaves.get(id)
334    }
335}
336
337impl<N: RealField + Copy, T, BV> Index<DBVTLeafId> for DBVT<N, T, BV> {
338    type Output = DBVTLeaf<N, T, BV>;
339
340    #[inline]
341    fn index(&self, DBVTLeafId(id): DBVTLeafId) -> &Self::Output {
342        &self.leaves[id]
343    }
344}
345
346impl<'a, N: RealField + Copy, T, BV> BVH<T, BV> for DBVT<N, T, BV> {
347    type Node = DBVTNodeId;
348
349    fn root(&self) -> Option<Self::Node> {
350        if self.leaves.len() != 0 {
351            Some(self.root)
352        } else {
353            None
354        }
355    }
356
357    fn num_children(&self, node: Self::Node) -> usize {
358        match node {
359            DBVTNodeId::Internal(_) => 2,
360            DBVTNodeId::Leaf(_) => 0,
361        }
362    }
363
364    fn child(&self, i: usize, node: Self::Node) -> Self::Node {
365        match node {
366            DBVTNodeId::Internal(node_id) => {
367                if i == 0 {
368                    self.internals[node_id].left
369                } else {
370                    self.internals[node_id].right
371                }
372            }
373            DBVTNodeId::Leaf(_) => panic!("DBVT child index out of bounds."),
374        }
375    }
376
377    fn content(&self, node: Self::Node) -> (&BV, Option<&T>) {
378        match node {
379            DBVTNodeId::Internal(i) => {
380                let node = &self.internals[i];
381                (&node.bounding_volume, None)
382            }
383            DBVTNodeId::Leaf(i) => {
384                let node = &self.leaves[i];
385                (&node.bounding_volume, Some(&node.data))
386            }
387        }
388    }
389}