zvxryb_broadphase/
layer.rs

1// mlodato, 20190806
2
3use crate::geom::{
4    Bounds,
5    BoxTestGeometry,
6    IndexGenerator,
7    RayTestGeometry,
8    SystemBounds,
9    TestGeometry,
10    VecDim,
11};
12use crate::index::SpatialIndex;
13use crate::traits::ObjectID;
14
15use cgmath::prelude::*;
16use rustc_hash::FxHashSet;
17use smallvec::SmallVec;
18
19use std::fmt::Debug;
20use std::ops::DerefMut;
21
22#[cfg(feature="parallel")]
23use rayon::prelude::*;
24
25#[cfg(feature="parallel")]
26use std::cell::{RefMut, RefCell};
27
28#[cfg(feature="parallel")]
29use thread_local::CachedThreadLocal;
30
31/// [`SpatialIndex`]: trait.SpatialIndex.html
32/// [`Index64_3D`]: struct.Index64_3D.html
33
34/// A group of collision data
35/// 
36/// `Index` must be a type implmenting [`SpatialIndex`], such as [`Index64_3D`]
37/// 
38/// `ID` is the type representing object IDs
39
40#[derive(Default)]
41#[cfg_attr(any(test, feature="serde"), derive(Deserialize, Serialize))]
42pub struct Layer<Index, ID>
43where
44    Index: SpatialIndex,
45    ID: ObjectID,
46    Bounds<Index::Point>: IndexGenerator<Index>
47{
48    // persistant state:
49    min_depth: u32,
50    tree: (Vec<(Index, ID)>, bool),
51
52    // temporary data used within a method:
53    #[cfg_attr(any(test, feature="serde"), serde(skip))]
54    collisions: Vec<(ID, ID)>,
55
56    #[cfg_attr(any(test, feature="serde"), serde(skip))]
57    test_results: Vec<ID>,
58
59    #[cfg_attr(any(test, feature="serde"), serde(skip))]
60    processed: FxHashSet<ID>,
61
62    #[cfg_attr(any(test, feature="serde"), serde(skip))]
63    invalid: Vec<ID>,
64
65    #[cfg(feature="parallel")]
66    #[cfg_attr(any(test, feature="serde"), serde(skip))]
67    collisions_tls: CachedThreadLocal<RefCell<Vec<(ID, ID)>>>,
68}
69
70impl<Index, ID> Layer<Index, ID>
71where
72    Index: SpatialIndex,
73    ID: ObjectID,
74    Bounds<Index::Point>: IndexGenerator<Index>
75{
76    /// Iterate over all indices in the `Layer`
77    /// 
78    /// This is primarily intended for visualization + debugging
79    pub fn iter(&self) -> std::slice::Iter<'_, (Index, ID)> {
80        self.tree.0.iter()
81    }
82
83    /// Clear all index-ID pairs
84    pub fn clear(&mut self) {
85        let (tree, sorted) = &mut self.tree;
86        tree.clear();
87        *sorted = true;
88    }
89
90    /// Append multiple objects to the `Layer`
91    /// 
92    /// Complex geometry may provide multiple bounds for a single object ID; this usage would be common
93    /// for static geometry, as it prevents extraneous self-collisions
94    pub fn extend<Iter, Point_>(&mut self, system_bounds: Bounds<Point_>, objects: Iter)
95    where
96        Iter: std::iter::Iterator<Item = (Bounds<Point_>, ID)>,
97        Point_: EuclideanSpace<Scalar = f32>,
98        Point_::Diff: ElementWise,
99        Bounds<Point_>: SystemBounds<Point_, Index::Point>
100    {
101        let (tree, sorted) = &mut self.tree;
102
103        if let (_, Some(max_objects)) = objects.size_hint() {
104            tree.reserve(max_objects);
105        }
106
107        for (bounds, id) in objects {
108            if !system_bounds.contains(bounds) {
109                self.invalid.push(id);
110                continue
111            }
112
113            tree.extend(system_bounds
114                .to_local(bounds)
115                .indices(Some(self.min_depth))
116                .into_iter()
117                .map(|index| (index, id)));
118
119            *sorted = false;
120        }
121    }
122
123    /// Merge another `Layer` into this `Layer`
124    /// 
125    /// This may be used, for example, to merge static scene `Layer` into the current
126    /// frames' dynamic `Layer` without having to recalculate indices for the static data
127    pub fn merge(&mut self, other: &Layer<Index, ID>) {
128        let (lhs_tree, sorted) = &mut self.tree;
129        let (rhs_tree, _) = &other.tree;
130
131        if other.min_depth < self.min_depth {
132            warn!("merging layer of lesser min_depth (lhs: {}, rhs: {})", self.min_depth, other.min_depth);
133            self.min_depth = other.min_depth;
134        }
135
136        lhs_tree.extend(rhs_tree.iter());
137        *sorted = false;
138    }
139
140    /// [`par_scan_filtered`]: struct.Layer.html#method.par_scan_filtered
141    /// [`par_scan`]: struct.Layer.html#method.par_scan
142    /// Sort indices to ready data for detection (parallel)
143    /// 
144    /// This will be called implicitly when necessary (i.e. by [`par_scan_filtered`], [`par_scan`], etc.)
145    #[cfg(feature="parallel")]
146    pub fn par_sort(&mut self) {
147        let (tree, sorted) = &mut self.tree;
148        if !*sorted {
149            tree.par_sort_unstable();
150            *sorted = true;
151        }
152    }
153
154    /// [`scan_filtered`]: struct.Layer.html#method.scan_filtered
155    /// [`scan`]: struct.Layer.html#method.scan
156    /// Sort indices to ready data for detection
157    /// 
158    /// This will be called implicitly when necessary (i.e. by [`scan_filtered`], [`scan`], etc.)
159    pub fn sort(&mut self) {
160        let (tree, sorted) = &mut self.tree;
161        if !*sorted {
162            tree.sort_unstable();
163            *sorted = true;
164        }
165    }
166
167    fn test_impl<TestGeom, Callback>(
168        tree: &[(Index, ID)],
169        cell: Index,
170        test_geom: &TestGeom,
171        mut nearest: f32,
172        max_depth: Option<u32>,
173        callback: &mut Callback) -> f32
174    where
175        TestGeom: TestGeometry,
176        Callback: FnMut(&TestGeom, f32, ID) -> f32
177    {
178        use std::cmp::Ordering::{Less, Greater};
179
180        if tree.is_empty() || !test_geom.should_test(nearest) {
181            return nearest;
182        }
183
184        if tree.first().unwrap().0 < cell || !cell.overlaps(tree.last().unwrap().0) {
185            panic!("test_impl called with non-overlapping indices");
186        }
187
188        let depth = cell.depth();
189        if let Some(max_depth) = max_depth {
190            if depth >= max_depth {
191                return tree.iter()
192                    .map(|(_, id)| *id)
193                    .fold(nearest, |nearest, id|
194                        callback(test_geom, nearest, id).min(nearest));
195            }
196        }
197
198        if let Some(sub_cells) = cell.subdivide() {
199            let mut sub_trees = sub_cells.as_ref().iter()
200                .map(|cell| Some(*cell))
201                .chain((0..1).map(|_| None))
202                .scan(tree, |tree, cell| {
203                    if let Some(cell) = cell {
204                        let i = tree.binary_search_by(|&(index, _)| {
205                            if index < cell { Less } else { Greater }
206                        }).err().unwrap();
207                        let (head, tail) = tree.split_at(i);
208                        *tree = tail;
209                        Some(head)
210                    } else {
211                        Some(tree)
212                    }
213                });
214            nearest = sub_trees.next().unwrap().iter()
215                .map(|(_, id)| *id)
216                .fold(nearest, |nearest, id|
217                    callback(test_geom, nearest, id).min(nearest));
218
219            let sub_trees: SmallVec<[_; 8]> = sub_trees.collect();
220            let sub_tests = test_geom.subdivide();
221
222            for &i in test_geom.test_order().as_ref() {
223                nearest = Self::test_impl(
224                    sub_trees[i],
225                    sub_cells.as_ref()[i],
226                    &sub_tests.as_ref()[i],
227                    nearest,
228                    max_depth,
229                    callback);
230            }
231
232            nearest
233        } else {
234            tree.iter()
235                .map(|(_, id)| *id)
236                .fold(nearest, |nearest, id|
237                    callback(test_geom, nearest, id).min(nearest))
238        }
239    }
240
241    /// Run a single test on some geometry
242    /// 
243    /// This occurs by repeatedly subdividing both this `Layer`'s index-ID list and the provided
244    /// `test_geom`, returning any items at a given depth where both the resulting index list
245    /// is non-empty and [`TestGeometry::subdivide`] returns a result
246    /// 
247    /// _note: this method may do an implicit, non-parallel sort; you may call [`par_sort`] prior
248    /// to calling this method to perform a parallel sort instead_
249    /// 
250    /// [`TestGeometry::subdivide`]: trait.TestGeometry.html#tymethod.subdivide
251    /// [`par_sort`]: #method.par_sort
252    pub fn test<'a, TestGeom>(
253        &'a mut self,
254        test_geom: &TestGeom,
255        max_depth: Option<u32>) -> &'a Vec<ID>
256    where
257        TestGeom: TestGeometry
258    {
259        self.sort();
260
261        self.test_results.clear();
262
263        let (tree, _) = &self.tree;
264        let results = &mut self.test_results;
265        Self::test_impl(
266            tree,
267            Index::default(),
268            test_geom,
269            std::f32::INFINITY,
270            max_depth,
271            &mut |_, nearest, id| {
272                results.push(id);
273                nearest
274            });
275
276        results.sort();
277        results.dedup();
278
279        results
280    }
281
282    /// A special case of [`test`] for bounding box tests, see [`BoxTestGeometry`]
283    /// 
284    /// The `system_bounds` provided to this method should, in most cases, be identical to the
285    /// `system_bounds` provided to [`extend`]
286    /// 
287    /// _note: this method may do an implicit, non-parallel sort; you may call [`par_sort`] prior
288    /// to calling this method to perform a parallel sort instead_
289    /// 
290    /// [`test`]: #method.test
291    /// [`extend`]: #method.extend
292    /// [`par_sort`]: #method.par_sort
293    /// [`BoxTestGeometry`]: struct.BoxTestGeometry.html
294    pub fn test_box<'a, Point_>(
295        &'a mut self,
296        system_bounds: Bounds<Point_>,
297        test_bounds: Bounds<Point_>,
298        max_depth: Option<u32>) -> &'a Vec<ID>
299    where
300        Point_: EuclideanSpace<Scalar = f32> + Debug,
301        Point_::Diff: ElementWise + std::ops::Index<usize, Output = f32> + Debug,
302        BoxTestGeometry<Point_>: TestGeometry
303    {
304        let test_geom = BoxTestGeometry::with_system_bounds(
305            system_bounds,
306            test_bounds);
307
308        self.test(
309            &test_geom,
310            max_depth);
311
312        &self.test_results
313    }
314
315    /// A special case of [`test`] for ray-testing, see [`RayTestGeometry`]
316    /// 
317    /// The `system_bounds` provided to this method should, in most cases, be identical to the
318    /// `system_bounds` provided to [`extend`]
319    /// 
320    /// _note: this method may do an implicit, non-parallel sort; you may call [`par_sort`] prior
321    /// to calling this method to perform a parallel sort instead_
322    /// 
323    /// [`test`]: #method.test
324    /// [`extend`]: #method.extend
325    /// [`par_sort`]: #method.par_sort
326    /// [`RayTestGeometry`]: struct.RayTestGeometry.html
327    pub fn test_ray<'a, Point_>(
328        &'a mut self,
329        system_bounds: Bounds<Point_>,
330        origin   : Point_,
331        direction: Point_::Diff,
332        range_min: f32,
333        range_max: f32,
334        max_depth: Option<u32>) -> &'a Vec<ID>
335    where
336        Point_: EuclideanSpace<Scalar = f32> + VecDim + Debug,
337        Point_::Diff: ElementWise + std::ops::Index<usize, Output = f32> + Debug,
338        RayTestGeometry<Point_>: TestGeometry
339    {
340        let test_geom = RayTestGeometry::with_system_bounds(
341            system_bounds,
342            origin,
343            direction,
344            range_min,
345            range_max);
346
347        self.test(
348            &test_geom,
349            max_depth);
350
351        &self.test_results
352    }
353
354    /// Run a picking or hit-test operation
355    /// 
356    /// This is implemented similarly to [`test`], but differs in that it returns only the nearest
357    /// result and may stop searching as soon as the nearest result is found
358    /// 
359    /// _note: this method may do an implicit, non-parallel sort; you may call [`par_sort`] prior
360    /// to calling this method to perform a parallel sort instead_
361    /// 
362    /// [`test`]: #method.test
363    /// [`par_sort`]: #method.par_sort
364    pub fn pick<TestGeom, GetDist>(
365        &mut self,
366        test_geom: &TestGeom,
367        max_dist: f32,
368        max_depth: Option<u32>,
369        mut get_dist: GetDist) -> Option<(f32, ID)>
370    where
371        TestGeom: TestGeometry,
372        GetDist: FnMut(&TestGeom, f32, ID) -> f32
373    {
374        self.sort();
375
376        self.processed.clear();
377
378        let (tree, _) = &self.tree;
379        let processed = &mut self.processed;
380        let mut result: Option<ID> = None;
381        let dist = Self::test_impl(
382            tree,
383            Index::default(),
384            test_geom,
385            max_dist,
386            max_depth,
387            &mut |test_geom, nearest, id| {
388                if processed.insert(id) {
389                    let dist = get_dist(test_geom, nearest, id);
390                    if dist.is_finite() {
391                        if dist < nearest {
392                            result = Some(id);
393                        }
394                        dist
395                    } else {
396                        std::f32::INFINITY
397                    }
398                } else {
399                    std::f32::INFINITY
400                }
401            });
402
403        result.map(|id| (dist, id))
404    }
405
406    /// A special case of [`pick`] for ray-testing, see [`RayTestGeometry`]
407    /// 
408    /// The `system_bounds` provided to this method should, in most cases, be identical to the
409    /// `system_bounds` provided to [`extend`]
410    /// 
411    /// _note: this method may do an implicit, non-parallel sort; you may call [`par_sort`] prior
412    /// to calling this method to perform a parallel sort instead_
413    /// 
414    /// [`pick`]: #method.pick
415    /// [`extend`]: #method.extend
416    /// [`par_sort`]: #method.par_sort
417    /// [`RayTestGeometry`]: struct.RayTestGeometry.html
418    pub fn pick_ray<Point_, GetDist>(
419        &mut self,
420        system_bounds: Bounds<Point_>,
421        origin   : Point_,
422        direction: Point_::Diff,
423        max_dist: f32,
424        max_depth: Option<u32>,
425        mut get_dist: GetDist) -> Option<(f32, ID, Point_)>
426    where
427        Point_: EuclideanSpace<Scalar = f32> + VecDim + Debug,
428        Point_::Diff: VectorSpace<Scalar = f32> + ElementWise + std::ops::Index<usize, Output = f32> + Debug,
429        RayTestGeometry<Point_>: TestGeometry,
430        GetDist: FnMut(&Point_, &Point_::Diff, f32, ID) -> f32
431    {
432        let test_geom = RayTestGeometry::with_system_bounds(
433            system_bounds,
434            origin,
435            direction,
436            0f32,
437            max_dist);
438
439        self.pick(&test_geom, max_dist, max_depth, |_, max_dist, id| {
440                get_dist(&origin, &direction, max_dist, id)
441            })
442            .map(|(dist, id)| {
443                let point = origin + direction * dist;
444                (dist, id, point)
445            })
446    }
447
448    /// Detects collisions between all objects in the `Layer`
449    pub fn scan<'a>(&'a mut self)
450        -> &'a Vec<(ID, ID)>
451    {
452        self.scan_filtered(|_, _| true)
453    }
454
455    /// Detects collisions between all objects in the `Layer`, returning only those which pass a user-specified test
456    /// 
457    /// Collisions are filtered prior to duplicate removal.  This may be faster or slower than filtering
458    /// post-duplicate-removal (i.e. by `scan().iter().filter()`) depending on the complexity
459    /// of the filter.
460    pub fn scan_filtered<'a, F>(&'a mut self, filter: F)
461        -> &'a Vec<(ID, ID)>
462    where
463        F: FnMut(ID, ID) -> bool
464    {
465        self.sort();
466        
467        self.collisions.clear();
468        self.invalid.clear();
469
470        let (tree, _) = &self.tree;
471        Self::scan_impl(tree.as_slice(), &mut self.collisions, filter);
472
473        self.collisions.sort_unstable();
474        self.collisions.dedup();
475
476        &self.collisions
477    }
478
479    /// [`scan`]: struct.Layer.html#method.scan
480    /// Parallel version of [`scan`]
481    #[cfg(feature="parallel")]
482    pub fn par_scan<'a>(&'a mut self)
483        -> &'a Vec<(ID, ID)>
484    where
485        Index: Send + Sync
486    {
487        self.par_scan_filtered(|_, _| true)
488    }
489
490    /// [`scan_filtered`]: struct.Layer.html#method.scan_filtered
491    /// Parallel version of [`scan_filtered`]
492    #[cfg(feature="parallel")]
493    pub fn par_scan_filtered<'a, F>(&'a mut self, filter: F)
494        -> &'a Vec<(ID, ID)>
495    where
496        Index: Send + Sync,
497        F: Copy + Send + Sync + FnMut(ID, ID) -> bool
498    {
499        self.par_sort();
500
501        self.collisions.clear();
502        self.invalid.clear();
503        for set in self.collisions_tls.iter_mut() {
504            set.borrow_mut().clear();
505        }
506
507        self.par_scan_impl(rayon::current_num_threads(), self.tree.0.as_slice(), filter);
508
509        for set in self.collisions_tls.iter_mut() {
510            use std::borrow::Borrow;
511            let set_: RefMut<Vec<(ID, ID)>> = set.borrow_mut();
512            let set__: &Vec<(ID, ID)> = set_.borrow();
513            self.collisions.extend(set__.iter());
514        }
515
516        self.collisions.par_sort_unstable();
517        self.collisions.dedup();
518
519        &self.collisions
520    }
521
522    #[cfg(feature="parallel")]
523    fn par_scan_impl<F>(&self, threads: usize, tree: &[(Index, ID)], filter: F)
524    where
525        Index: Send + Sync,
526        F: Copy + Send + Sync + FnMut(ID, ID) -> bool
527    {
528        const SPLIT_THRESHOLD: usize = 64;
529        if threads <= 1 || tree.len() <= SPLIT_THRESHOLD {
530            let collisions = self.collisions_tls.get_or(|| RefCell::new(Vec::new()));
531            Self::scan_impl(tree, collisions.borrow_mut(), filter);
532        } else {
533            let n = tree.len();
534            let mut i = n / 2;
535            while i < n {
536                let (last, _) = tree[i-1];
537                let (next, _) = tree[i];
538                if !Index::same_cell_at_depth(last, next, self.min_depth) {
539                    break;
540                }
541                i += 1;
542            }
543            let (head, tail) = tree.split_at(i);
544            rayon::join(
545                || self.par_scan_impl(threads >> 1, head, filter),
546                || self.par_scan_impl(threads >> 1, tail, filter));
547        }
548    }
549
550    fn scan_impl<C, F>(tree: &[(Index, ID)], mut collisions: C, mut filter: F)
551    where
552        C: DerefMut<Target = Vec<(ID, ID)>>,
553        F: FnMut(ID, ID) -> bool
554    {
555        let mut stack: SmallVec<[(Index, ID); 256]> = SmallVec::new();
556        for &(index, id) in tree {
557            while let Some(&(index_, _)) = stack.last() {
558                if index.overlaps(index_) {
559                    break;
560                }
561                stack.pop();
562            }
563            if stack.iter().any(|&(_, id_)| id == id_) {
564                continue;
565            }
566            for &(_, id_) in &stack {
567                if id != id_ && filter(id, id_) {
568                    collisions.push((id, id_));
569                }
570            }
571            stack.push((index, id))
572        }
573    }
574}
575
576impl<Index, ID> PartialEq<Self> for Layer<Index, ID>
577where
578    Index: SpatialIndex,
579    ID: ObjectID,
580    Bounds<Index::Point>: IndexGenerator<Index>
581{
582    fn eq(&self, other: &Self) -> bool {
583        self.min_depth == other.min_depth &&
584        self.tree      == other.tree
585    }
586}
587
588impl<Index, ID> Eq for Layer<Index, ID>
589where
590    Index: SpatialIndex,
591    ID: ObjectID,
592    Bounds<Index::Point>: IndexGenerator<Index>
593{}
594
595impl<Index, ID> Clone for Layer<Index, ID>
596where
597    Index: SpatialIndex,
598    ID: ObjectID,
599    Bounds<Index::Point>: IndexGenerator<Index>
600{
601    fn clone(&self) -> Self {
602        Layer{
603            min_depth: self.min_depth,
604            tree: self.tree.clone(),
605
606            // don't bother cloning the contents of temporary buffers
607            collisions: Vec::with_capacity(self.collisions.capacity()),
608            test_results: Vec::with_capacity(self.test_results.capacity()),
609            processed: FxHashSet::default(),
610            invalid: Vec::new(),
611
612            #[cfg(feature="parallel")]
613            collisions_tls: CachedThreadLocal::new()
614        }
615    }
616}
617
618/// A builder for `Layer`s
619#[derive(Default)]
620pub struct LayerBuilder {
621    min_depth: u32,
622    index_capacity: Option<usize>,
623    collision_capacity: Option<usize>,
624    test_capacity: Option<usize>
625}
626
627impl LayerBuilder {
628    pub fn new() -> Self {
629        Self::default()
630    }
631
632    /// Set a minimum depth for index generation.
633    /// 
634    /// This parameter is important for parallel processing.  A higher value improves the partitioning of data and
635    /// improves workload balancing.  However, it can also create many more indices/object than is necessary.  A
636    /// setting which is too high may result in an excessive number of dynamic allocations and duplication of
637    /// intermediate collision pairs, ultimately hurting worst-case performance.
638    /// 
639    /// A value of zero is the safest performance-wise for _single-threaded_ operations.
640    /// 
641    /// When using multi-threaded methods, try a value that between
642    /// _log<sub>4</sub> number_of_processors_ (2D) or
643    /// _log<sub>8</sub> number_of_processors_ (3D) and
644    /// _&minus;log<sub>2</sub>(max_object_size/system_bounds_size)_
645    /// 
646    /// __It is generally better to set this too low than too high__
647    pub fn with_min_depth(&mut self, depth: u32) -> &mut Self {
648        self.min_depth = depth;
649        self
650    }
651
652    /// Set an _initial_ capacity for the index list.
653    pub fn with_index_capacity(&mut self, capacity: usize) -> &mut Self {
654        self.index_capacity = Some(capacity);
655        self
656    }
657
658    /// Set an _initial_ capacity for the collision results list, used by `Layer::scan`.
659    pub fn with_collision_capacity(&mut self, capacity: usize) -> &mut Self {
660        self.collision_capacity = Some(capacity);
661        self
662    }
663
664    /// Set an _initial_ capacity for the test results list, used by `Layer::test` and `Layer::pick`.
665    pub fn with_test_capacity(&mut self, capacity: usize) -> &mut Self {
666        self.test_capacity = Some(capacity);
667        self
668    }
669
670    pub fn build<Index, ID>(&self) -> Layer<Index, ID>
671    where
672        Index: SpatialIndex,
673        ID: ObjectID,
674        Bounds<Index::Point>: IndexGenerator<Index>
675    {
676        Layer{
677            min_depth: self.min_depth,
678            tree: (match self.index_capacity {
679                    Some(capacity) => Vec::with_capacity(capacity),
680                    None => Vec::new()
681                }, true),
682            collisions: match self.collision_capacity {
683                    Some(capacity) => Vec::with_capacity(capacity),
684                    None => Vec::new()
685                },
686            test_results: match self.test_capacity {
687                    Some(capacity) => Vec::with_capacity(capacity),
688                    None => Vec::new()
689                },
690            processed: FxHashSet::default(),
691            invalid: Vec::new(),
692            #[cfg(feature="parallel")]
693            collisions_tls: CachedThreadLocal::new()
694        }
695    }
696}
zvxryb_broadphase/layer.rs

zvxryb_broadphase/
layer.rs
