ncollide3d/partitioning/
bvt.rs

1//! A read-only Bounding Volume Tree.
2
3use crate::bounding_volume::BoundingVolume;
4use crate::math::{Point, DIM};
5use crate::partitioning::BVH;
6use crate::utils;
7use simba::scalar::RealField;
8use std::collections::VecDeque;
9use std::iter;
10use std::usize;
11
12/// A Bounding Volume Tree.
13#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
14#[derive(Clone)]
15pub struct BVT<T, BV> {
16    root: BVTNodeId,
17    internals: Vec<BVTInternal<BV>>,
18    leaves: Vec<BVTLeaf<T, BV>>,
19    // This will be filled only when at least one
20    // deformation occurred to avoid memory usage
21    // that are not needed in the general case.
22    deformation_timestamp: usize,
23    deformation_infos: Vec<BVTDeformationInfo>,
24    // Infos for leaves are stored starting at the index self.internals.len().
25    parents_to_update: VecDeque<usize>,
26}
27
28/// The identifier of a BVT node.
29#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
30#[derive(Copy, Clone, Hash, PartialEq, Eq, Debug)]
31pub enum BVTNodeId {
32    /// Identifier of an internal node.
33    Internal(usize),
34    /// Identifier of a leaf node.
35    Leaf(usize),
36}
37
38#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
39#[derive(Clone)]
40struct BVTInternal<BV> {
41    bounding_volume: BV,
42    right: BVTNodeId,
43    left: BVTNodeId,
44}
45
46/// A leaf of the BVT.
47#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
48#[derive(Clone)]
49pub struct BVTLeaf<T, BV> {
50    bounding_volume: BV,
51    data: T,
52}
53
54impl<T, BV> BVTLeaf<T, BV> {
55    /// The bounding volume stored on this leaf.
56    #[inline]
57    pub fn bounding_volume(&self) -> &BV {
58        &self.bounding_volume
59    }
60
61    /// The user-data stored on this leaf.
62    #[inline]
63    pub fn data(&self) -> &T {
64        &self.data
65    }
66}
67
68#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
69#[derive(Clone)]
70struct BVTDeformationInfo {
71    parent: usize,
72    timestamp: usize,
73}
74
75/// Result of a binary partition.
76pub enum BinaryPartition<T, BV> {
77    /// Result of the partitioning of one element.
78    Part(T),
79    /// Result of the partitioning of several elements.
80    Parts(Vec<(T, BV)>, Vec<(T, BV)>),
81}
82
83impl<T, BV> BVT<T, BV> {
84    /// Builds a bounding volume tree using the specified partitioning function.
85    #[deprecated(note = "please use `from_partitioning` instead")]
86    pub fn new_with_partitioning<F: FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>)>(
87        elements: Vec<(T, BV)>,
88        partitioning: &mut F,
89    ) -> BVT<T, BV> {
90        Self::from_partitioning(elements, partitioning)
91    }
92
93    // FIXME: add higher level constructors ?
94    /// Builds a bounding volume tree using the specified partitioning function.
95    pub fn from_partitioning(
96        elements: Vec<(T, BV)>,
97        partitioning: &mut impl FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>),
98    ) -> BVT<T, BV> {
99        if elements.len() == 0 {
100            BVT {
101                root: BVTNodeId::Leaf(0),
102                internals: Vec::new(),
103                leaves: Vec::new(),
104                deformation_timestamp: 1,
105                deformation_infos: Vec::new(),
106                parents_to_update: VecDeque::new(),
107            }
108        } else {
109            let mut internals = Vec::new();
110            let mut leaves = Vec::new();
111            let root =
112                Self::_from_partitioning(0, elements, &mut internals, &mut leaves, partitioning);
113            internals.shrink_to_fit();
114            leaves.shrink_to_fit();
115
116            BVT {
117                root,
118                internals,
119                leaves,
120                deformation_timestamp: 1,
121                deformation_infos: Vec::new(),
122                parents_to_update: VecDeque::new(),
123            }
124        }
125    }
126
127    /// The set of leaves on this BVT.
128    #[inline]
129    pub fn leaves(&self) -> &[BVTLeaf<T, BV>] {
130        &self.leaves
131    }
132
133    /// Referenceto the i-th leaf of this BVT.
134    #[inline]
135    pub fn leaf(&self, i: usize) -> &BVTLeaf<T, BV> {
136        &self.leaves[i]
137    }
138
139    /// Reference to the bounding volume of the tree root.
140    pub fn root_bounding_volume(&self) -> Option<&BV> {
141        if self.leaves.is_empty() {
142            return None;
143        }
144
145        match self.root {
146            BVTNodeId::Leaf(i) => Some(&self.leaves[i].bounding_volume),
147            BVTNodeId::Internal(i) => Some(&self.internals[i].bounding_volume),
148        }
149    }
150
151    /// Set the bounding volume of the i-th leaf.
152    ///
153    /// If `refit_now` is `true`, the bounding volumes of all the ancestors of the
154    /// modifiad leaf will be updated as well to enclose the new leaf bounding volume.
155    /// If `refit_now` is `false`, no ancestor update will be performed until the
156    /// `.refit()` method is called. This is useful to refit the tree only once after
157    /// several leaf bounding volume modifications.
158    pub fn set_leaf_bounding_volume<N: RealField + Copy>(
159        &mut self,
160        i: usize,
161        bv: BV,
162        refit_now: bool,
163    ) where
164        BV: BoundingVolume<N>,
165    {
166        self.init_deformation_infos();
167        self.leaves[i].bounding_volume = bv;
168
169        if refit_now {
170            let mut curr = self.deformation_infos[self.internals.len() + i].parent;
171
172            while curr != usize::max_value() {
173                let new_bv = match (self.internals[curr].left, self.internals[curr].right) {
174                    (BVTNodeId::Internal(i), BVTNodeId::Internal(j)) => self.internals[i]
175                        .bounding_volume
176                        .merged(&self.internals[j].bounding_volume),
177                    (BVTNodeId::Internal(i), BVTNodeId::Leaf(j)) => self.internals[i]
178                        .bounding_volume
179                        .merged(&self.leaves[j].bounding_volume),
180                    (BVTNodeId::Leaf(i), BVTNodeId::Internal(j)) => self.leaves[i]
181                        .bounding_volume
182                        .merged(&self.internals[j].bounding_volume),
183                    (BVTNodeId::Leaf(i), BVTNodeId::Leaf(j)) => self.leaves[i]
184                        .bounding_volume
185                        .merged(&self.leaves[j].bounding_volume),
186                };
187                self.internals[curr].bounding_volume = new_bv;
188                curr = self.deformation_infos[curr].parent;
189            }
190        } else {
191            if self.leaves.len() != 1 {
192                self.parents_to_update
193                    .push_back(self.deformation_infos[self.internals.len() + i].parent)
194            }
195        }
196    }
197
198    /// Refits the bounding volumes so that all node of the BVT have boundin volumes that enclose their children.
199    ///
200    /// This must be called to ensure the BVT is in a valid state after several calls to
201    /// `.set_leaf_bounding_volume(_, _, false)`.
202    /// Every bounding volume created during this update will be enlarged by a margin of `margin`.
203    /// The larger this margin here, the looser will the resulting AABB will be, but the less frequent
204    /// future updates will be necessary.
205    /// Setting a margin equal to 0.0 is allowed.
206    pub fn refit<N: RealField + Copy>(&mut self, margin: N)
207    where
208        BV: BoundingVolume<N>,
209    {
210        assert!(margin >= N::zero(), "Cannot set a negative margin.");
211
212        self.deformation_timestamp += 1;
213
214        while let Some(curr) = self.parents_to_update.pop_front() {
215            let infos = &mut self.deformation_infos[curr];
216            if infos.timestamp < self.deformation_timestamp {
217                // This node has not been updated yet.
218                infos.timestamp = self.deformation_timestamp;
219
220                let mut new_bv = match (self.internals[curr].left, self.internals[curr].right) {
221                    (BVTNodeId::Internal(i), BVTNodeId::Internal(j)) => self.internals[i]
222                        .bounding_volume
223                        .merged(&self.internals[j].bounding_volume),
224                    (BVTNodeId::Internal(i), BVTNodeId::Leaf(j)) => self.internals[i]
225                        .bounding_volume
226                        .merged(&self.leaves[j].bounding_volume),
227                    (BVTNodeId::Leaf(i), BVTNodeId::Internal(j)) => self.leaves[i]
228                        .bounding_volume
229                        .merged(&self.internals[j].bounding_volume),
230                    (BVTNodeId::Leaf(i), BVTNodeId::Leaf(j)) => self.leaves[i]
231                        .bounding_volume
232                        .merged(&self.leaves[j].bounding_volume),
233                };
234
235                if !self.internals[curr].bounding_volume.contains(&new_bv) {
236                    if !margin.is_zero() {
237                        new_bv.loosen(margin)
238                    }
239
240                    self.internals[curr].bounding_volume = new_bv;
241
242                    if infos.parent != usize::max_value() {
243                        // Push the parent if it is not the root.
244                        self.parents_to_update.push_back(infos.parent);
245                    }
246                }
247            }
248        }
249    }
250
251    fn init_deformation_infos(&mut self) {
252        if self.deformation_infos.is_empty() {
253            self.deformation_infos = iter::repeat(BVTDeformationInfo {
254                parent: usize::max_value(),
255                timestamp: 0,
256            })
257            .take(self.internals.len() + self.leaves.len())
258            .collect();
259
260            for (i, internal) in self.internals.iter().enumerate() {
261                match internal.left {
262                    BVTNodeId::Internal(j) => self.deformation_infos[j].parent = i,
263                    BVTNodeId::Leaf(j) => {
264                        self.deformation_infos[self.internals.len() + j].parent = i
265                    }
266                }
267
268                match internal.right {
269                    BVTNodeId::Internal(j) => self.deformation_infos[j].parent = i,
270                    BVTNodeId::Leaf(j) => {
271                        self.deformation_infos[self.internals.len() + j].parent = i
272                    }
273                }
274            }
275        }
276    }
277}
278
279impl<T, BV> BVT<T, BV> {
280    /// Creates a balanced `BVT`.
281    pub fn new_balanced<N>(leaves: Vec<(T, BV)>) -> BVT<T, BV>
282    where
283        N: RealField + Copy,
284        BV: BoundingVolume<N> + Clone,
285    {
286        BVT::from_partitioning(leaves, &mut Self::median_partitioning)
287    }
288
289    /// Construction function for a kdree to be used with `BVT::from_partitioning`.
290    pub fn median_partitioning_with_centers<N, F: FnMut(&T, &BV) -> Point<N>>(
291        depth: usize,
292        leaves: Vec<(T, BV)>,
293        center: &mut F,
294    ) -> (BV, BinaryPartition<T, BV>)
295    where
296        N: RealField + Copy,
297        BV: BoundingVolume<N> + Clone,
298    {
299        if leaves.len() == 0 {
300            panic!("Cannot build a tree without leaves.");
301        } else if leaves.len() == 1 {
302            let (b, bv) = leaves.into_iter().next().unwrap();
303            (bv, BinaryPartition::Part(b))
304        } else {
305            let sep_axis = depth % DIM;
306
307            // compute the median along sep_axis
308            let mut median = Vec::new();
309
310            for l in leaves.iter() {
311                let c = (*center)(&l.0, &l.1);
312                median.push(c[sep_axis]);
313            }
314
315            let median = utils::median(&mut median[..]);
316
317            // build the partitions
318            let mut right = Vec::new();
319            let mut left = Vec::new();
320            let mut bounding_bounding_volume = leaves[0].1.clone();
321
322            let mut insert_left = false;
323
324            for (b, bv) in leaves.into_iter() {
325                bounding_bounding_volume.merge(&bv);
326
327                let pos = (*center)(&b, &bv)[sep_axis];
328
329                if pos < median || (pos == median && insert_left) {
330                    left.push((b, bv));
331                    insert_left = false;
332                } else {
333                    right.push((b, bv));
334                    insert_left = true;
335                }
336            }
337
338            // XXX: hack to avoid degeneracies.
339            if left.len() == 0 {
340                left.push(right.pop().unwrap());
341            } else if right.len() == 0 {
342                right.push(left.pop().unwrap());
343            }
344
345            (
346                bounding_bounding_volume,
347                BinaryPartition::Parts(left, right),
348            )
349        }
350    }
351
352    /// Construction function for a kdree to be used with `BVT::from_partitioning`.
353    pub fn median_partitioning<N>(
354        depth: usize,
355        leaves: Vec<(T, BV)>,
356    ) -> (BV, BinaryPartition<T, BV>)
357    where
358        N: RealField + Copy,
359        BV: BoundingVolume<N> + Clone,
360    {
361        Self::median_partitioning_with_centers(depth, leaves, &mut |_, bv| bv.center())
362    }
363
364    fn _from_partitioning<F: FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>)>(
365        depth: usize,
366        leaves: Vec<(T, BV)>,
367        out_internals: &mut Vec<BVTInternal<BV>>,
368        out_leaves: &mut Vec<BVTLeaf<T, BV>>,
369        partitioning: &mut F,
370    ) -> BVTNodeId {
371        let (bv, partitions) = partitioning(depth, leaves);
372
373        match partitions {
374            BinaryPartition::Part(b) => {
375                out_leaves.push(BVTLeaf {
376                    bounding_volume: bv,
377                    data: b,
378                });
379                BVTNodeId::Leaf(out_leaves.len() - 1)
380            }
381            BinaryPartition::Parts(left, right) => {
382                let left = Self::_from_partitioning(
383                    depth + 1,
384                    left,
385                    out_internals,
386                    out_leaves,
387                    partitioning,
388                );
389                let right = Self::_from_partitioning(
390                    depth + 1,
391                    right,
392                    out_internals,
393                    out_leaves,
394                    partitioning,
395                );
396                out_internals.push(BVTInternal {
397                    bounding_volume: bv,
398                    left,
399                    right,
400                });
401                BVTNodeId::Internal(out_internals.len() - 1)
402            }
403        }
404    }
405}
406
407impl<'a, T, BV> BVH<T, BV> for BVT<T, BV> {
408    type Node = BVTNodeId;
409
410    fn root(&self) -> Option<Self::Node> {
411        if self.leaves.len() != 0 {
412            Some(self.root)
413        } else {
414            None
415        }
416    }
417
418    fn num_children(&self, node: Self::Node) -> usize {
419        match node {
420            BVTNodeId::Internal(_) => 2,
421            BVTNodeId::Leaf(_) => 0,
422        }
423    }
424
425    fn child(&self, i: usize, node: Self::Node) -> Self::Node {
426        match node {
427            BVTNodeId::Internal(node_id) => {
428                if i == 0 {
429                    self.internals[node_id].left
430                } else {
431                    self.internals[node_id].right
432                }
433            }
434            BVTNodeId::Leaf(_) => panic!("DBVT child index out of bounds."),
435        }
436    }
437
438    fn content(&self, node: Self::Node) -> (&BV, Option<&T>) {
439        match node {
440            BVTNodeId::Internal(i) => {
441                let node = &self.internals[i];
442                (&node.bounding_volume, None)
443            }
444            BVTNodeId::Leaf(i) => {
445                let node = &self.leaves[i];
446                (&node.bounding_volume, Some(&node.data))
447            }
448        }
449    }
450}