zvxryb_broadphase/
geom.rs

1// mlodato, 20190806
2
3use crate::index::SpatialIndex;
4
5use cgmath::{Point2, Point3, Vector2, Vector3};
6use cgmath::prelude::*;
7use num_traits::{Float, One, PrimInt};
8use smallvec::SmallVec;
9
10use std::fmt::{Debug, Formatter};
11
12pub trait VecDim {
13    const DIM: usize;
14}
15
16impl<T> VecDim for Point2 <T> { const DIM: usize = 2; }
17impl<T> VecDim for Point3 <T> { const DIM: usize = 3; }
18impl<T> VecDim for Vector2<T> { const DIM: usize = 2; }
19impl<T> VecDim for Vector3<T> { const DIM: usize = 3; }
20
21fn fold_arr<Arr, State, F>(arr: Arr, init: State, f: F) -> State
22where
23    Arr: Array,
24    F: FnMut(State, Arr::Element) -> State
25{
26    (0..Arr::len()).map(|i| arr[i]).fold(init, f)
27}
28
29fn init_arr<Arr, F>(arr: &mut Arr, mut f: F)
30where
31    Arr: Array,
32    F: FnMut(usize) -> Arr::Element
33{
34    #[allow(clippy::needless_range_loop)]
35    for i in 0..Arr::len() {
36        arr[i] = f(i);
37    }
38}
39
40fn max_axis<Arr>(arr: Arr) -> Arr::Element
41where
42    Arr: Array,
43    Arr::Element: Bounded + Ord
44{
45    fold_arr(arr, Arr::Element::min_value(), std::cmp::max)
46}
47
48fn scale_at_depth(depth: u32) -> u32 {
49    if depth == 0 {
50        panic!("scale at zero depth would overflow integer");
51    }
52    1u32 << (32 - depth)
53}
54
55fn truncate_to_depth(x: u32, depth: u32) -> u32 {
56    if depth == 0 {
57        x
58    } else {
59        x & !(scale_at_depth(depth) - 1u32)
60    }
61}
62
63/// The set of indices for an object with known bounds
64/// 
65/// By default, index `depth` is chosen such that this returns no more than 4 (2D) or 8 (3D) indices.
66/// `min_depth` provides a lower-bound which enables quick partitioning (for parallel task generation)
67
68pub trait IndexGenerator<Index>
69where
70    Index: SpatialIndex,
71    Self::Output: IntoIterator<Item = Index>
72{
73    type Output;
74
75    fn indices(self, min_depth: Option<u32>) -> Self::Output;
76    fn indices_at_depth(self, depth: u32) -> Self::Output;
77}
78
79/// An axis-aligned bounding box
80/// 
81/// Min and max values are _inclusive_
82#[derive(Copy, Clone, Debug, PartialEq)]
83#[cfg_attr(any(test, feature="serde"), derive(Deserialize, Serialize))]
84pub struct Bounds<Point> {
85    pub min: Point,
86    pub max: Point
87}
88
89impl<Point> Bounds<Point>
90where
91    Point: EuclideanSpace + Array<Element = <Point as EuclideanSpace>::Scalar> + Copy
92{
93    pub fn new(min: Point, max: Point) -> Self {
94        Self{min, max}
95    }
96
97    pub fn sizef(self) -> Point::Diff
98    where
99        Point::Scalar: Float
100    {
101        self.max - self.min
102    }
103
104    pub fn sizei(self) -> Point::Diff
105    where
106        Point::Scalar: PrimInt + One,
107        Point::Diff: ElementWise<Point::Scalar>
108    {
109        (self.max - self.min).add_element_wise(Point::Scalar::one())
110    }
111
112    pub fn overlaps(self, other: Bounds<Point>) -> bool {
113        for i in 0..Point::len() {
114            if self.min[i] > other.max[i] || self.max[i] < other.min[i] {
115                return false;
116            }
117        }
118        true
119    }
120
121    pub fn contains(self, other: Bounds<Point>) -> bool {
122        for i in 0..Point::len() {
123            if self.min[i] > other.min[i] || self.max[i] < other.max[i] {
124                return false;
125            }
126        }
127        true
128    }
129
130    pub fn center(self) -> Point {
131        self.min.midpoint(self.max)
132    }
133}
134
135/// System bounds supporting conversions between local and global coordinates
136pub trait SystemBounds<PointGlobal, PointLocal> {
137    fn to_local(&self, global: Bounds<PointGlobal>) -> Bounds<PointLocal>;
138    fn to_global(&self, local: Bounds<PointLocal>) -> Bounds<PointGlobal>;
139}
140
141impl<PointGlobal, PointLocal> SystemBounds<PointGlobal, PointLocal> for Bounds<PointGlobal>
142where
143    PointGlobal: EuclideanSpace<Scalar = f32>,
144    PointGlobal::Diff: Array<Element = f32>,
145    PointLocal: EuclideanSpace<Scalar = u32>,
146    PointLocal::Diff: Array<Element = u32>
147{
148    fn to_local(&self, global: Bounds<PointGlobal>) -> Bounds<PointLocal> {
149        let size = self.sizef();
150        let to_local = |global: PointGlobal, i| {
151            // MAX_VALUE has 24 bits set because IEEE floats have 23 explicit + 1 implicit fractional bits
152            const MIN_VALUE: f32 = std::u32::MIN as f32;
153            const MAX_VALUE: f32 = 0xffff_ff00u32 as f32;
154            const RANGE: f32 = MAX_VALUE - MIN_VALUE;
155            ((global[i] - self.min[i]) / size[i] * RANGE + MIN_VALUE) as u32
156        };
157        let mut local = Bounds::new(
158            PointLocal::from_vec(PointLocal::Diff::zero()),
159            PointLocal::from_vec(PointLocal::Diff::zero()));
160        init_arr(&mut local.min, |i| to_local(global.min, i));
161        init_arr(&mut local.max, |i| to_local(global.max, i));
162        local
163    }
164
165    fn to_global(&self, local: Bounds<PointLocal>) -> Bounds<PointGlobal> {
166        let size = self.sizef();
167        let to_global = |local: PointLocal, i| {
168            // MAX_VALUE has 24 bits set because IEEE floats have 23 explicit + 1 implicit fractional bits
169            const MIN_VALUE: f32 = std::u32::MIN as f32;
170            const MAX_VALUE: f32 = 0xffff_ff00u32 as f32;
171            const RANGE: f32 = MAX_VALUE - MIN_VALUE;
172            self.min[i] + (local[i] as f32 - MIN_VALUE) / RANGE * size[i]
173        };
174        let mut global = Bounds::new(
175            PointGlobal::from_vec(PointGlobal::Diff::zero()),
176            PointGlobal::from_vec(PointGlobal::Diff::zero()));
177        init_arr(&mut global.min, |i| to_global(local.min, i));
178        init_arr(&mut global.max, |i| to_global(local.max, i));
179        global
180    }
181}
182
183impl<Index> IndexGenerator<Index> for Bounds<Point2<u32>>
184where
185    Index: SpatialIndex<Diff = Vector2<u32>, Point = Point2<u32>>
186{
187    type Output = SmallVec<[Index; 4]>;
188
189    fn indices(self, min_depth: Option<u32>) -> Self::Output {
190        let max_axis = max_axis(self.sizei());
191        let mut depth = (max_axis - 1u32).leading_zeros();
192        if let Some(min_depth) = min_depth {
193            if depth < min_depth {
194                depth = min_depth;
195            }
196        }
197        depth = Index::clamp_depth(depth);
198        
199        self.indices_at_depth(depth)
200    }
201
202    fn indices_at_depth(self, depth: u32) -> Self::Output {
203        if depth == 0 {
204            return smallvec![Index::default()];
205        }
206
207        let min = self.min.map(|scalar| truncate_to_depth(scalar, depth));
208        let max = self.max.map(|scalar| truncate_to_depth(scalar, depth));
209
210        let mut indices: Self::Output = Self::Output::new();
211
212        let step = scale_at_depth(depth);
213        let mut y = min.y;
214        loop {
215            let mut x = min.x;
216            loop {
217                indices.push(Index::default()
218                    .set_depth(depth)
219                    .set_origin(Point2::new(x, y)));
220
221                if x >= max.x {
222                    break;
223                }
224                x += step;
225            }
226
227            if y >= max.y {
228                break;
229            }
230            y += step;
231        }
232
233        if indices.len() > 4 {
234            warn!("indices_at_depth generated more than 4 indices; decrease min_depth or split large objects to avoid heap allocations");
235        }
236
237        indices
238    }
239}
240
241impl<Index> IndexGenerator<Index> for Bounds<Point3<u32>>
242where
243    Index: SpatialIndex<Diff = Vector3<u32>, Point = Point3<u32>>
244{
245    type Output = SmallVec<[Index; 8]>;
246
247    fn indices(self, min_depth: Option<u32>) -> Self::Output {
248        let max_axis = max_axis(self.sizei());
249        let mut depth = (max_axis - 1u32).leading_zeros();
250        if let Some(min_depth) = min_depth {
251            if depth < min_depth {
252                depth = min_depth;
253            }
254        }
255        depth = Index::clamp_depth(depth);
256        
257        self.indices_at_depth(depth)
258    }
259
260    fn indices_at_depth(self, depth: u32) -> Self::Output {
261        if depth == 0 {
262            return smallvec![Index::default()];
263        }
264
265        let min = self.min.map(|scalar| truncate_to_depth(scalar, depth));
266        let max = self.max.map(|scalar| truncate_to_depth(scalar, depth));
267
268        let mut indices: Self::Output = Self::Output::new();
269
270        let step = scale_at_depth(depth);
271        let mut z = min.z;
272        loop {
273            let mut y = min.y;
274            loop {
275                let mut x = min.x;
276                loop {
277                    indices.push(Index::default()
278                        .set_depth(depth)
279                        .set_origin(Point3::new(x, y, z)));
280
281                    if x >= max.x {
282                        break;
283                    }
284                    x += step;
285                }
286
287                if y >= max.y {
288                    break;
289                }
290                y += step;
291            }
292
293            if z >= max.z {
294                break;
295            }
296            z += step;
297        }
298
299        if indices.len() > 8 {
300            warn!("indices_at_depth generated more than 8 indices; decrease min_depth or split large objects to avoid heap allocations");
301        }
302
303        indices
304    }
305}
306
307impl<Index, Point> From<Index> for Bounds<Point>
308where
309    Index: SpatialIndex<Diff = Point::Diff, Point = Point>,
310    Point: EuclideanSpace<Scalar = u32> + ElementWise<u32>
311{
312    fn from(index: Index) -> Self {
313        let origin = index.origin();
314        let scale = scale_at_depth(index.depth());
315        Self{
316            min: origin,
317            max: origin.add_element_wise(scale-1)
318        }
319    }
320}
321
322/// [`Layer::test`]: struct.Layer.html#method.test
323/// A trait for implementing individual geometry tests
324/// 
325/// See [`Layer::test`]. This is a low-level interface and should generally not be directly
326/// implemented by users
327pub trait TestGeometry: Sized + Debug {
328    type SubdivideResult: AsRef<[Self]>;
329    type TestOrder: AsRef<[usize]>;
330
331    /// [`SpatialIndex::subdivide`]: trait.SpatialIndex.html#tymethod.subdivide
332    /// [`test_order`]: #tymethod.test_order
333    /// Subdivide this geometry
334    /// 
335    /// This is required to return results in the same order as [`SpatialIndex::subdivide`], both
336    /// results will be reordered as given by [`test_order`]
337    fn subdivide(&self) -> Self::SubdivideResult;
338
339    /// The order in which to test cells
340    /// 
341    /// This is used to optimize tests where only the single, nearest, result should be returned
342    fn test_order(&self) -> Self::TestOrder;
343
344    /// Return whether this geometry is valid and non-empty
345    /// 
346    /// `nearest` may be `std::f32::INFINITY`
347    fn should_test(&self, nearest: f32) -> bool;
348}
349
350/// [`TestGeometry`]: trait.TestGeometry.html
351/// A type implementing [`TestGeometry`] for bounding boxes
352#[derive(Clone, Debug)]
353pub struct BoxTestGeometry<Point>
354where
355    Point: EuclideanSpace<Scalar = f32>
356{
357    cell_bounds: Bounds<Point>,
358    test_bounds: Bounds<Point>,
359}
360
361impl<Point> BoxTestGeometry<Point>
362where
363    Point: EuclideanSpace<Scalar = f32>
364{
365    /// Construct box test geometry
366    pub fn with_system_bounds(
367        system_bounds: Bounds<Point>,
368        test_bounds: Bounds<Point>,) -> Self
369    where
370        Point: Debug,
371        Point::Diff: ElementWise + std::ops::Index<usize, Output = f32> + Debug,
372    {
373        Self{
374            cell_bounds: system_bounds,
375            test_bounds}
376    }
377}
378
379impl TestGeometry for BoxTestGeometry<Point2<f32>> {
380    type SubdivideResult = [Self; 4];
381    type TestOrder = [usize; 4];
382
383    fn subdivide(&self) -> Self::SubdivideResult {
384        let center = self.cell_bounds.center();
385        let mut results: [Self; 4] = [
386            self.clone(),
387            self.clone(),
388            self.clone(),
389            self.clone()
390        ];
391        for (cell, result) in results.iter_mut().enumerate() {
392            let bounds = &mut result.cell_bounds;
393            #[allow(clippy::needless_range_loop)]
394            for axis in 0..2 {
395                let side = cell & (1 << axis) != 0;
396                if side {
397                    bounds.min[axis] = center[axis];
398                } else {
399                    bounds.max[axis] = center[axis];
400                }
401            }
402        }
403        results
404    }
405
406    fn test_order(&self) -> Self::TestOrder {
407        [0, 1, 2, 3]
408    }
409
410    fn should_test(&self, nearest: f32) -> bool {
411        debug_assert!(!nearest.is_finite(), "BoxTestGeometry does not support \"pick\" operations");
412        self.cell_bounds.overlaps(self.test_bounds)
413    }
414}
415
416impl TestGeometry for BoxTestGeometry<Point3<f32>> {
417    type SubdivideResult = [Self; 8];
418    type TestOrder = [usize; 8];
419
420    fn subdivide(&self) -> Self::SubdivideResult {
421        let center = self.cell_bounds.center();
422        let mut results: [Self; 8] = [
423            self.clone(),
424            self.clone(),
425            self.clone(),
426            self.clone(),
427            self.clone(),
428            self.clone(),
429            self.clone(),
430            self.clone()
431        ];
432        for (cell, result) in results.iter_mut().enumerate() {
433            let bounds = &mut result.cell_bounds;
434            #[allow(clippy::needless_range_loop)]
435            for axis in 0..3 {
436                let side = cell & (1 << axis) != 0;
437                if side {
438                    bounds.min[axis] = center[axis];
439                } else {
440                    bounds.max[axis] = center[axis];
441                }
442            }
443        }
444        results
445    }
446
447    fn test_order(&self) -> Self::TestOrder {
448        [0, 1, 2, 3, 4, 5, 6, 7]
449    }
450
451    fn should_test(&self, nearest: f32) -> bool {
452        debug_assert!(!nearest.is_finite(), "BoxTestGeometry does not support \"pick\" operations");
453        self.cell_bounds.overlaps(self.test_bounds)
454    }
455}
456
457/// [`TestGeometry`]: trait.TestGeometry.html
458/// A type implementing [`TestGeometry`] for rays
459#[derive(Clone)]
460pub struct RayTestGeometry<Point>
461where
462    Point: EuclideanSpace<Scalar = f32>
463{
464    cell_bounds: Bounds<Point>,
465    origin: Point,
466    direction: Point::Diff,
467    range_min: f32,
468    range_max: f32
469}
470
471impl Debug for RayTestGeometry<Point2<f32>> {
472    fn fmt(&self, f: &mut Formatter) -> std::fmt::Result {
473        write!(f, "RayTestGeometry{{{{({:}, {:}) - ({:}, {:})}}, ({:}, {:}), ({:}, {:}), {{{:}-{:}}}}}",
474            self.cell_bounds.min.x,
475            self.cell_bounds.min.y,
476            self.cell_bounds.max.x,
477            self.cell_bounds.max.y,
478            self.origin.x,
479            self.origin.y,
480            self.direction.x,
481            self.direction.y,
482            self.range_min,
483            self.range_max)
484    }
485}
486
487impl Debug for RayTestGeometry<Point3<f32>> {
488    fn fmt(&self, f: &mut Formatter) -> std::fmt::Result {
489        write!(f, "RayTestGeometry{{{{({:}, {:}, {:}) - ({:}, {:}, {:})}}, ({:}, {:}, {:}), ({:}, {:}, {:}), {{{:}-{:}}}}}",
490            self.cell_bounds.min.x,
491            self.cell_bounds.min.y,
492            self.cell_bounds.min.z,
493            self.cell_bounds.max.x,
494            self.cell_bounds.max.y,
495            self.cell_bounds.max.z,
496            self.origin.x,
497            self.origin.y,
498            self.origin.z,
499            self.direction.x,
500            self.direction.y,
501            self.direction.z,
502            self.range_min,
503            self.range_max)
504    }
505}
506
507impl<Point> RayTestGeometry<Point>
508where
509    Point: EuclideanSpace<Scalar = f32> + VecDim
510{
511    /// Construct ray test geometry
512    /// 
513    /// `range_min` and `range_max` may be infinity or negative infinity, in which case
514    /// the ray will be clamped to system bounds
515    pub fn with_system_bounds(
516        system_bounds: Bounds<Point>,
517        origin: Point,
518        direction: Point::Diff,
519        mut range_min: f32,
520        mut range_max: f32) -> Self
521    where
522        Point: Debug,
523        Point::Diff: ElementWise + std::ops::Index<usize, Output = f32> + Debug,
524    {
525        let distance_0 = (system_bounds.min - origin).div_element_wise(direction);
526        let distance_1 = (system_bounds.max - origin).div_element_wise(direction);
527        for axis in 0..<Point as VecDim>::DIM {
528            let is_forward = direction[axis] > 0f32;
529            let (d0, d1) = if is_forward {
530                    (distance_0[axis], distance_1[axis])
531                } else {
532                    (distance_1[axis], distance_0[axis])
533                };
534            if d0.is_finite() { range_min = range_min.max(d0); }
535            if d1.is_finite() { range_max = range_max.min(d1); }
536        }
537
538        Self{
539            cell_bounds: system_bounds,
540            origin,
541            direction,
542            range_min,
543            range_max}
544    }
545}
546
547impl TestGeometry for RayTestGeometry<Point2<f32>> {
548    type SubdivideResult = [Self; 4];
549    type TestOrder = [usize; 4];
550
551    fn subdivide(&self) -> Self::SubdivideResult {
552        let center = self.cell_bounds.center();
553        let distance = (self.cell_bounds.center() - self.origin).div_element_wise(self.direction);
554        let mut results: [Self; 4] = [
555            self.clone(),
556            self.clone(),
557            self.clone(),
558            self.clone()
559        ];
560        for (cell, result) in results.iter_mut().enumerate() {
561            let range_min = &mut result.range_min;
562            let range_max = &mut result.range_max;
563            for axis in 0..2 {
564                let side = cell & (1 << axis) != 0;
565                if distance[axis].is_finite() {
566                    let is_towards = (self.direction[axis] > 0f32) != side;
567                    if is_towards {
568                        *range_max = range_max.min(distance[axis]);
569                    } else {
570                        *range_min = range_min.max(distance[axis]);
571                    }
572                } else if (self.origin[axis] > center[axis]) != side {
573                    *range_min = std::f32::INFINITY;
574                    *range_max = std::f32::NEG_INFINITY;
575                }
576            }
577            let bounds = &mut result.cell_bounds;
578            #[allow(clippy::needless_range_loop)]
579            for axis in 0..2 {
580                let side = cell & (1 << axis) != 0;
581                if side {
582                    bounds.min[axis] = center[axis];
583                } else {
584                    bounds.max[axis] = center[axis];
585                }
586            }
587        }
588        results
589    }
590
591    fn test_order(&self) -> Self::TestOrder {
592        let abs = self.direction.map(|x| x.abs());
593        #[allow(clippy::collapsible_if)]
594        let axes = if abs.x <= abs.y { [0, 1] } else { [1, 0] };
595
596        let mut order: [usize; 4] = [0; 4];
597        for (cell_src, cell_dst) in order.iter_mut().enumerate() {
598            let i0 = (cell_src & 1 != 0) == (self.direction[axes[0]] >= 0f32);
599            let i1 = (cell_src & 2 != 0) == (self.direction[axes[1]] >= 0f32);
600            *cell_dst =
601                ((i0 as usize) << axes[0]) |
602                ((i1 as usize) << axes[1]);
603        }
604
605        order
606    }
607
608    fn should_test(&self, nearest: f32) -> bool {
609        self.range_min < self.range_max && self.range_min < nearest
610    }
611}
612
613impl TestGeometry for RayTestGeometry<Point3<f32>> {
614    type SubdivideResult = [Self; 8];
615    type TestOrder = [usize; 8];
616
617    fn subdivide(&self) -> Self::SubdivideResult {
618        let center = self.cell_bounds.center();
619        let distance = (self.cell_bounds.center() - self.origin).div_element_wise(self.direction);
620        let mut results: [Self; 8] = [
621            self.clone(),
622            self.clone(),
623            self.clone(),
624            self.clone(),
625            self.clone(),
626            self.clone(),
627            self.clone(),
628            self.clone()
629        ];
630        for (cell, result) in results.iter_mut().enumerate() {
631            let range_min = &mut result.range_min;
632            let range_max = &mut result.range_max;
633            for axis in 0..3 {
634                let side = cell & (1 << axis) != 0;
635                if distance[axis].is_finite() {
636                    let is_towards = (self.direction[axis] > 0f32) != side;
637                    if is_towards {
638                        *range_max = range_max.min(distance[axis]);
639                    } else {
640                        *range_min = range_min.max(distance[axis]);
641                    }
642                } else if (self.origin[axis] > center[axis]) != side {
643                    *range_min = std::f32::INFINITY;
644                    *range_max = std::f32::NEG_INFINITY;
645                }
646            }
647            let bounds = &mut result.cell_bounds;
648            #[allow(clippy::needless_range_loop)]
649            for axis in 0..3 {
650                let side = cell & (1 << axis) != 0;
651                if side {
652                    bounds.min[axis] = center[axis];
653                } else {
654                    bounds.max[axis] = center[axis];
655                }
656            }
657        }
658        results
659    }
660
661    fn test_order(&self) -> Self::TestOrder {
662        let abs = self.direction.map(|x| x.abs());
663        #[allow(clippy::collapsible_if)]
664        let axes = if abs.x <= abs.y && abs.x <= abs.z {
665            if abs.y <= abs.z { [0, 1, 2] } else { [0, 2, 1] }
666        } else if abs.y <= abs.z {
667            if abs.x <= abs.z { [1, 0, 2] } else { [1, 2, 0] }
668        } else {
669            if abs.x <= abs.y { [2, 0, 1] } else { [2, 1, 0] }
670        };
671
672        let mut order: [usize; 8] = [0; 8];
673        for (cell_src, cell_dst) in order.iter_mut().enumerate() {
674            let i0 = (cell_src & 1 != 0) == (self.direction[axes[0]] >= 0f32);
675            let i1 = (cell_src & 2 != 0) == (self.direction[axes[1]] >= 0f32);
676            let i2 = (cell_src & 4 != 0) == (self.direction[axes[2]] >= 0f32);
677            *cell_dst =
678                ((i0 as usize) << axes[0]) |
679                ((i1 as usize) << axes[1]) |
680                ((i2 as usize) << axes[2])
681        }
682
683        order
684    }
685
686    fn should_test(&self, nearest: f32) -> bool {
687        self.range_min < self.range_max && self.range_min < nearest
688    }
689}
690
691#[cfg(test)]
692mod tests {
693    use super::*;
694
695    #[test]
696    fn system_bounds() {
697        let system_bounds = Bounds{
698            min: Point3::new(-64f32, -64f32, -64f32),
699            max: Point3::new( 64f32,  64f32,  64f32)};
700        let global = Bounds{
701            min: Point3::new(-32f32, -32f32, -32f32),
702            max: Point3::new( 32f32,  32f32,  32f32)};
703        let local: Bounds<Point3<u32>> = system_bounds.to_local(global);
704        let expected = global;
705        let actual = system_bounds.to_global(local);
706        assert_eq!(actual, expected);
707    }
708}
zvxryb_broadphase/geom.rs

zvxryb_broadphase/
geom.rs
