ncollide3d/pipeline/broad_phase/
dbvt_broad_phase.rs

Help
1use crate::bounding_volume::BoundingVolume;
2use crate::math::Point;
3use crate::partitioning::{DBVTLeaf, DBVTLeafId, BVH, DBVT};
4use crate::pipeline::broad_phase::{
5    BroadPhase, BroadPhaseInterferenceHandler, BroadPhaseProxyHandle,
6};
7use crate::query::visitors::{
8    BoundingVolumeInterferencesCollector, PointInterferencesCollector, RayInterferencesCollector,
9    RayIntersectionCostFnVisitor,
10};
11use crate::query::{PointQuery, Ray, RayCast, RayIntersection};
12use crate::utils::{DeterministicState, SortedPair};
13use na::RealField;
14use slab::Slab;
15use std::any::Any;
16use std::collections::hash_map::Entry;
17use std::collections::{HashMap, VecDeque};
18
19#[derive(Copy, Clone, Debug, PartialEq, Eq)]
20enum ProxyStatus {
21    OnStaticTree(DBVTLeafId),
22    OnDynamicTree(DBVTLeafId, usize),
23    // The usize is the location of the corresponding on proxies_to_update
24    Detached(Option<usize>),
25    Deleted,
26}
27
28struct DBVTBroadPhaseProxy<T> {
29    data: T,
30    status: ProxyStatus,
31    updated: bool,
32}
33
34impl<T> DBVTBroadPhaseProxy<T> {
35    fn new(data: T) -> DBVTBroadPhaseProxy<T> {
36        DBVTBroadPhaseProxy {
37            data,
38            status: ProxyStatus::Detached(None),
39            updated: true,
40        }
41    }
42
43    fn is_detached(&self) -> bool {
44        match self.status {
45            ProxyStatus::Detached(_) => true,
46            _ => false,
47        }
48    }
49}
50
51const DEACTIVATION_THRESHOLD: usize = 100;
52
53/// Broad phase based on a Dynamic Bounding Volume Tree.
54///
55/// It uses two separate trees: one for static objects and which is never updated, and one for
56/// moving objects.
57pub struct DBVTBroadPhase<N: RealField + Copy, BV, T> {
58    proxies: Slab<DBVTBroadPhaseProxy<T>>,
59    // DBVT for moving objects.
60    tree: DBVT<N, BroadPhaseProxyHandle, BV>,
61    // DBVT for static objects.
62    stree: DBVT<N, BroadPhaseProxyHandle, BV>,
63    // Pairs detected.
64    pairs: HashMap<SortedPair<BroadPhaseProxyHandle>, bool, DeterministicState>,
65    // The margin added to each bounding volume.
66    margin: N,
67    purge_all: bool,
68
69    // Just to avoid dynamic allocations.
70    collector: Vec<BroadPhaseProxyHandle>,
71    leaves_to_update: Vec<DBVTLeaf<N, BroadPhaseProxyHandle, BV>>,
72    proxies_to_update: VecDeque<(BroadPhaseProxyHandle, BV)>,
73}
74
75impl<N, BV, T> DBVTBroadPhase<N, BV, T>
76where
77    N: RealField + Copy,
78    BV: 'static + BoundingVolume<N> + Clone,
79{
80    /// Creates a new broad phase based on a Dynamic Bounding Volume Tree.
81    pub fn new(margin: N) -> DBVTBroadPhase<N, BV, T> {
82        DBVTBroadPhase {
83            proxies: Slab::new(),
84            tree: DBVT::new(),
85            stree: DBVT::new(),
86            pairs: HashMap::with_hasher(DeterministicState::new()),
87            purge_all: false,
88            collector: Vec::new(),
89            leaves_to_update: Vec::new(),
90            proxies_to_update: VecDeque::new(),
91            margin,
92        }
93    }
94
95    /// Number of interferences detected by this broad phase.
96    #[inline]
97    pub fn num_interferences(&self) -> usize {
98        self.pairs.len()
99    }
100
101    fn purge_some_contact_pairs(&mut self, handler: &mut dyn BroadPhaseInterferenceHandler<T>) {
102        let purge_all = self.purge_all;
103        let proxies = &self.proxies;
104        let stree = &self.stree;
105        let tree = &self.tree;
106        self.pairs.retain(|pair, up_to_date| {
107            let mut retain = true;
108
109            if purge_all || !*up_to_date {
110                *up_to_date = true;
111
112                let proxy1 = proxies
113                    .get(pair.0.uid())
114                    .expect("DBVT broad phase: internal error.");
115                let proxy2 = proxies
116                    .get(pair.1.uid())
117                    .expect("DBVT broad phase: internal error.");
118
119                if purge_all || proxy1.updated || proxy2.updated {
120                    if handler.is_interference_allowed(&proxy1.data, &proxy2.data) {
121                        let l1 = match proxy1.status {
122                            ProxyStatus::OnStaticTree(leaf) => &stree[leaf],
123                            ProxyStatus::OnDynamicTree(leaf, _) => &tree[leaf],
124                            _ => panic!("DBVT broad phase: internal error."),
125                        };
126
127                        let l2 = match proxy2.status {
128                            ProxyStatus::OnStaticTree(leaf) => &stree[leaf],
129                            ProxyStatus::OnDynamicTree(leaf, _) => &tree[leaf],
130                            _ => panic!("DBVT broad phase: internal error."),
131                        };
132
133                        if !l1.bounding_volume.intersects(&l2.bounding_volume) {
134                            handler.interference_stopped(&proxy1.data, &proxy2.data);
135                            retain = false;
136                        }
137                    } else {
138                        handler.interference_stopped(&proxy1.data, &proxy2.data);
139                        retain = false;
140                    }
141                }
142            }
143
144            *up_to_date = false;
145            retain
146        });
147    }
148
149    fn update_activation_states(&mut self) {
150        /*
151         * Update activation states.
152         * FIXME: could we avoid having to iterate through _all_ the proxies at each update?
153         */
154        for (_, proxy) in self.proxies.iter_mut() {
155            if let ProxyStatus::OnDynamicTree(leaf, energy) = proxy.status {
156                if energy == 1 {
157                    let old_leaf = self.tree.remove(leaf);
158                    let new_leaf = self.stree.insert(old_leaf);
159                    proxy.status = ProxyStatus::OnStaticTree(new_leaf);
160                } else {
161                    proxy.status = ProxyStatus::OnDynamicTree(leaf, energy - 1)
162                }
163            }
164        }
165    }
166}
167
168impl<N, BV, T> BroadPhase<N, BV, T> for DBVTBroadPhase<N, BV, T>
169where
170    N: RealField + Copy,
171    BV: BoundingVolume<N> + RayCast<N> + PointQuery<N> + Any + Send + Sync + Clone,
172    T: Any + Send + Sync + Clone,
173{
174    fn update(&mut self, handler: &mut dyn BroadPhaseInterferenceHandler<T>) {
175        /*
176         * Remove from the trees all nodes that have been deleted or modified.
177         */
178        for (handle, bv) in self.proxies_to_update.drain(..) {
179            if let Some(proxy) = self.proxies.get_mut(handle.uid()) {
180                let mut set_status = true;
181                match proxy.status {
182                    ProxyStatus::OnStaticTree(leaf) => {
183                        let mut leaf = self.stree.remove(leaf);
184                        leaf.bounding_volume = bv;
185                        self.leaves_to_update.push(leaf);
186                    }
187                    ProxyStatus::OnDynamicTree(leaf, _) => {
188                        let mut leaf = self.tree.remove(leaf);
189                        leaf.bounding_volume = bv;
190                        self.leaves_to_update.push(leaf);
191                    }
192                    ProxyStatus::Detached(None) => {
193                        let leaf = DBVTLeaf::new(bv, handle);
194                        self.leaves_to_update.push(leaf);
195                    }
196                    ProxyStatus::Detached(Some(id)) => {
197                        let leaf = DBVTLeaf::new(bv, handle);
198                        self.leaves_to_update[id] = leaf;
199                        set_status = false;
200                    }
201                    ProxyStatus::Deleted => {
202                        panic!("DBVT broad phase internal error: the proxy was deleted.")
203                    }
204                }
205
206                proxy.updated = true;
207
208                if set_status {
209                    proxy.status = ProxyStatus::Detached(Some(self.leaves_to_update.len() - 1));
210                }
211            }
212        }
213
214        /*
215         * Re-insert outdated nodes one by one and collect interferences at the same time.
216         */
217        let some_leaves_updated = self.leaves_to_update.len() != 0;
218        for leaf in self.leaves_to_update.drain(..) {
219            {
220                let proxy1 = &self.proxies[leaf.data.uid()];
221                {
222                    let mut visitor = BoundingVolumeInterferencesCollector::new(
223                        &leaf.bounding_volume,
224                        &mut self.collector,
225                    );
226
227                    self.tree.visit(&mut visitor);
228                    self.stree.visit(&mut visitor);
229                }
230
231                // Event generation.
232                for proxy_key2 in self.collector.iter() {
233                    let proxy2 = &self.proxies[proxy_key2.uid()];
234
235                    if handler.is_interference_allowed(&proxy1.data, &proxy2.data) {
236                        match self.pairs.entry(SortedPair::new(leaf.data, *proxy_key2)) {
237                            Entry::Occupied(entry) => *entry.into_mut() = true,
238                            Entry::Vacant(entry) => {
239                                handler.interference_started(&proxy1.data, &proxy2.data);
240                                let _ = entry.insert(true);
241                            }
242                        }
243                    }
244                }
245
246                self.collector.clear();
247            }
248
249            let proxy1 = &mut self.proxies[leaf.data.uid()];
250            assert!(proxy1.is_detached());
251            let leaf = self.tree.insert(leaf);
252            proxy1.status = ProxyStatus::OnDynamicTree(leaf, DEACTIVATION_THRESHOLD);
253        }
254
255        if some_leaves_updated {
256            self.purge_some_contact_pairs(handler);
257        }
258        self.update_activation_states();
259    }
260
261    /// Retrieves the bounding volume and data associated to the given proxy.
262    fn proxy(&self, handle: BroadPhaseProxyHandle) -> Option<(&BV, &T)> {
263        let proxy = self.proxies.get(handle.uid())?;
264        match proxy.status {
265            ProxyStatus::OnDynamicTree(id, _) => {
266                Some((&self.tree.get(id)?.bounding_volume, &proxy.data))
267            }
268            ProxyStatus::OnStaticTree(id) => {
269                Some((&self.stree.get(id)?.bounding_volume, &proxy.data))
270            }
271            _ => None,
272        }
273    }
274
275    fn create_proxy(&mut self, bv: BV, data: T) -> BroadPhaseProxyHandle {
276        let proxy = DBVTBroadPhaseProxy::new(data);
277        let handle = BroadPhaseProxyHandle(self.proxies.insert(proxy));
278        self.proxies_to_update.push_back((handle, bv));
279        handle
280    }
281
282    fn remove(&mut self, handles: &[BroadPhaseProxyHandle], handler: &mut dyn FnMut(&T, &T)) {
283        for handle in handles {
284            if let Some(proxy) = self.proxies.get_mut(handle.uid()) {
285                match proxy.status {
286                    ProxyStatus::OnStaticTree(leaf) => {
287                        let _ = self.stree.remove(leaf);
288                    }
289                    ProxyStatus::OnDynamicTree(leaf, _) => {
290                        let _ = self.tree.remove(leaf);
291                    }
292                    _ => {}
293                }
294
295                proxy.status = ProxyStatus::Deleted;
296            } else {
297                panic!("Attempting to remove an object that does not exist.");
298            }
299        }
300
301        {
302            let proxies = &self.proxies;
303            self.pairs.retain(|pair, _| {
304                let proxy1 = proxies
305                    .get(pair.0.uid())
306                    .expect("DBVT broad phase: internal error.");
307                let proxy2 = proxies
308                    .get(pair.1.uid())
309                    .expect("DBVT broad phase: internal error.");
310
311                if proxy1.status == ProxyStatus::Deleted || proxy2.status == ProxyStatus::Deleted {
312                    handler(&proxy1.data, &proxy2.data);
313                    false
314                } else {
315                    true
316                }
317            });
318        }
319
320        for handle in handles {
321            let _ = self.proxies.remove(handle.uid());
322        }
323    }
324
325    fn deferred_set_bounding_volume(&mut self, handle: BroadPhaseProxyHandle, bounding_volume: BV) {
326        if let Some(proxy) = self.proxies.get(handle.uid()) {
327            let needs_update = match proxy.status {
328                ProxyStatus::OnStaticTree(leaf) => {
329                    !self.stree[leaf].bounding_volume.contains(&bounding_volume)
330                }
331                ProxyStatus::OnDynamicTree(leaf, _) => {
332                    !self.tree[leaf].bounding_volume.contains(&bounding_volume)
333                }
334                ProxyStatus::Detached(_) => true,
335                ProxyStatus::Deleted => {
336                    panic!("DBVT broad phase: internal error, proxy not found.")
337                }
338            };
339
340            if needs_update {
341                let new_bv = bounding_volume.loosened(self.margin);
342                self.proxies_to_update.push_back((handle, new_bv));
343            }
344        } else {
345            panic!("Attempting to set the bounding volume of an object that does not exist.");
346        }
347    }
348
349    fn deferred_recompute_all_proximities_with(&mut self, handle: BroadPhaseProxyHandle) {
350        if let Some(proxy) = self.proxies.get(handle.uid()) {
351            let bv = match proxy.status {
352                ProxyStatus::OnStaticTree(leaf) => self.stree[leaf].bounding_volume.clone(),
353                ProxyStatus::OnDynamicTree(leaf, _) => self.tree[leaf].bounding_volume.clone(),
354                ProxyStatus::Detached(_) => return,
355                ProxyStatus::Deleted => {
356                    panic!("DBVT broad phase: internal error, proxy not found.")
357                }
358            };
359
360            self.proxies_to_update.push_front((handle, bv));
361        }
362    }
363
364    fn deferred_recompute_all_proximities(&mut self) {
365        for (handle, proxy) in self.proxies.iter() {
366            let bv;
367            match proxy.status {
368                ProxyStatus::OnStaticTree(leaf) => {
369                    bv = self.stree[leaf].bounding_volume.clone();
370                }
371                ProxyStatus::OnDynamicTree(leaf, _) => {
372                    bv = self.tree[leaf].bounding_volume.clone();
373                }
374                ProxyStatus::Detached(_) => continue,
375                ProxyStatus::Deleted => {
376                    panic!("DBVT broad phase: internal error, proxy not found.")
377                }
378            }
379
380            self.proxies_to_update
381                .push_front((BroadPhaseProxyHandle(handle), bv));
382        }
383
384        self.purge_all = true;
385    }
386
387    fn interferences_with_bounding_volume<'a>(&'a self, bv: &BV, out: &mut Vec<&'a T>) {
388        let mut collector = Vec::new();
389
390        {
391            let mut visitor = BoundingVolumeInterferencesCollector::new(bv, &mut collector);
392
393            self.tree.visit(&mut visitor);
394            self.stree.visit(&mut visitor);
395        }
396
397        for l in collector.into_iter() {
398            out.push(&self.proxies[l.uid()].data)
399        }
400    }
401
402    fn interferences_with_ray<'a>(&'a self, ray: &Ray<N>, max_toi: N, out: &mut Vec<&'a T>) {
403        let mut collector = Vec::new();
404
405        {
406            let mut visitor = RayInterferencesCollector::new(ray, max_toi, &mut collector);
407
408            self.tree.visit(&mut visitor);
409            self.stree.visit(&mut visitor);
410        }
411
412        for l in collector.into_iter() {
413            out.push(&self.proxies[l.uid()].data)
414        }
415    }
416
417    fn interferences_with_point<'a>(&'a self, point: &Point<N>, out: &mut Vec<&'a T>) {
418        let mut collector = Vec::new();
419
420        {
421            let mut visitor = PointInterferencesCollector::new(point, &mut collector);
422
423            self.tree.visit(&mut visitor);
424            self.stree.visit(&mut visitor);
425        }
426
427        for l in collector.into_iter() {
428            out.push(&self.proxies[l.uid()].data)
429        }
430    }
431
432    /// Returns the first object that interferes with a ray.
433    fn first_interference_with_ray<'a, 'b>(
434        &'a self,
435        ray: &'b Ray<N>,
436        max_toi: N,
437        cost_fn: &'a dyn Fn(T, &'b Ray<N>, N) -> Option<(T, RayIntersection<N>)>,
438    ) -> Option<(T, RayIntersection<N>)> {
439        let res = {
440            let mut visitor =
441                RayIntersectionCostFnVisitor::<'a, 'b, N, T, BV>::new(ray, max_toi, self, cost_fn);
442
443            let dynamic_hit = self.tree.best_first_search(&mut visitor);
444            let static_hit = self.stree.best_first_search(&mut visitor);
445
446            // The static hit must be better than the dynamic hit as it uses the
447            // same visitor so give it priority
448            if static_hit.is_some() {
449                static_hit
450            } else {
451                dynamic_hit
452            }
453        };
454
455        if let Some((_node, res)) = res {
456            Some(res)
457        } else {
458            None
459        }
460    }
461}
ncollide3d/pipeline/broad_phase/dbvt_broad_phase.rs

ncollide3d/pipeline/broad_phase/
dbvt_broad_phase.rs
