rust_3d/
aa_bb_tree_3d.rs

1/*
2Copyright 2018 Martin Buck
3
4Permission is hereby granted, free of charge, to any person obtaining a copy
5of this software and associated documentation files (the "Software"),
6to deal in the Software without restriction, including without limitation the
7rights to use, copy, modify, merge, publish, distribute, sublicense,
8and/or sell copies of the Software, and to permit persons to whom the Software
9is furnished to do so, subject to the following conditions:
10
11The above copyright notice and this permission notice shall
12be included all copies or substantial portions of the Software.
13
14THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
15EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
17IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
18DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
20OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21*/
22
23//! AABBTree3D, an axis aligned bounding box tree in 3D for fast collision detection
24
25use crate::*;
26
27use std::marker::PhantomData;
28
29//------------------------------------------------------------------------------
30
31#[derive(Clone)]
32/// AABBTree3D, an axis aligned bounding box tree in 3D for fast collision detection
33pub enum AABBTree3D<HB>
34where
35    HB: HasBoundingBox3D + Clone,
36{
37    Empty,
38    Leaf(AABBTree3DLeaf<HB>),
39    Branch(AABBTree3DBranch<HB>),
40}
41
42//------------------------------------------------------------------------------
43
44// currently often calculates the bounding box, try cache it
45impl<HB> AABBTree3D<HB>
46where
47    HB: HasBoundingBox3D + Clone,
48{
49    pub fn new(data: Vec<HB>, maxdepth: usize, allowed_bucket_size: usize) -> Self {
50        Self::new_rec(data, maxdepth, allowed_bucket_size, 0)
51    }
52
53    pub fn any<'a>(&'a self, f: &dyn Fn(&HB) -> bool) -> bool {
54        match self {
55            Self::Empty => false,
56            Self::Leaf(leaf) => leaf.any(f),
57            Self::Branch(branch) => branch.any(f),
58        }
59    }
60
61    pub fn for_each_intersection_candidate<'a>(&'a self, line: &Line3D, f: &mut dyn FnMut(&HB)) {
62        match self {
63            Self::Empty => (),
64            Self::Leaf(leaf) => leaf.for_each_intersection_candidate(line, f),
65            Self::Branch(branch) => branch.for_each_intersection_candidate(line, f),
66        }
67    }
68
69    pub fn for_each_collision_candidate<'a>(&'a self, bb: &BoundingBox3D, f: &mut dyn FnMut(&HB)) {
70        match self {
71            Self::Empty => (),
72            Self::Leaf(leaf) => leaf.for_each_collision_candidate(bb, f),
73            Self::Branch(branch) => branch.for_each_collision_candidate(bb, f),
74        }
75    }
76
77    pub fn bb_colliding<'a>(&'a self, bb: &BoundingBox3D, result: &mut Vec<&'a HB>) {
78        match self {
79            Self::Empty => (),
80            Self::Leaf(leaf) => leaf.bb_colliding(bb, result),
81            Self::Branch(branch) => branch.bb_colliding(bb, result),
82        }
83    }
84
85    pub fn bb_crossing_x_value<'a>(&'a self, x: f64, result: &mut Vec<&'a HB>) {
86        match self {
87            Self::Empty => (),
88            Self::Leaf(leaf) => leaf.bb_crossing_x_value(x, result),
89            Self::Branch(branch) => branch.bb_crossing_x_value(x, result),
90        }
91    }
92
93    pub fn bb_crossing_y_value<'a>(&'a self, y: f64, result: &mut Vec<&'a HB>) {
94        match self {
95            Self::Empty => (),
96            Self::Leaf(leaf) => leaf.bb_crossing_y_value(y, result),
97            Self::Branch(branch) => branch.bb_crossing_y_value(y, result),
98        }
99    }
100
101    pub fn bb_crossing_z_value<'a>(&'a self, z: f64, result: &mut Vec<&'a HB>) {
102        match self {
103            Self::Empty => (),
104            Self::Leaf(leaf) => leaf.bb_crossing_z_value(z, result),
105            Self::Branch(branch) => branch.bb_crossing_z_value(z, result),
106        }
107    }
108
109    fn new_rec(data: Vec<HB>, maxdepth: usize, allowed_bucket_size: usize, depth: usize) -> Self {
110        match data.len() {
111            0 => AABBTree3D::Empty,
112            1 => {
113                let bb = Self::bb_of(&data).unwrap(); //unwrap fine, since data non empty and with valid bbs (see new)
114                AABBTree3D::Leaf(AABBTree3DLeaf::new(data, bb))
115            }
116            _ => {
117                if depth >= maxdepth || data.len() <= allowed_bucket_size {
118                    let bb = Self::bb_of(&data).unwrap(); //unwrap fine, since data non empty and with valid bbs (see new)
119                    AABBTree3D::Leaf(AABBTree3DLeaf::new(data, bb))
120                } else {
121                    let comp = match depth % 3 {
122                        0 => Compare::X,
123                        1 => Compare::Y,
124                        _ => Compare::Z,
125                    };
126                    let bb = Self::bb_of(&data).unwrap(); //unwrap fine due to early return in new and data not empty
127                    let center = bb.center_bb();
128
129                    let dleft = data
130                        .iter()
131                        .cloned()
132                        .filter(|x| Self::is_left_of(&comp, &x.bounding_box(), &center))
133                        .collect::<Vec<_>>();
134                    let dright = data
135                        .iter()
136                        .cloned()
137                        .filter(|x| Self::is_right_of(&comp, &x.bounding_box(), &center))
138                        .collect::<Vec<_>>();
139
140                    if (dleft.len() == dright.len()) && dleft.len() == data.len() {
141                        AABBTree3D::Leaf(AABBTree3DLeaf::new(data, bb))
142                    } else {
143                        let left = Box::new(Self::new_rec(
144                            dleft,
145                            maxdepth,
146                            allowed_bucket_size,
147                            depth + 1,
148                        ));
149                        let right = Box::new(Self::new_rec(
150                            dright,
151                            maxdepth,
152                            allowed_bucket_size,
153                            depth + 1,
154                        ));
155
156                        AABBTree3D::Branch(AABBTree3DBranch::new(left, right, bb))
157                    }
158                }
159            }
160        }
161    }
162
163    fn is_left_of(comp: &Compare, bb: &BoundingBox3D, center: &Point3D) -> bool {
164        match comp {
165            Compare::X => bb.min_p().x() < center.x(),
166            Compare::Y => bb.min_p().y() < center.y(),
167            Compare::Z => bb.min_p().z() < center.z(),
168        }
169    }
170
171    fn is_right_of(comp: &Compare, bb: &BoundingBox3D, center: &Point3D) -> bool {
172        match comp {
173            Compare::X => bb.max_p().x() >= center.x(),
174            Compare::Y => bb.max_p().y() >= center.y(),
175            Compare::Z => bb.max_p().z() >= center.z(),
176        }
177    }
178
179    fn bb_of(data: &Vec<HB>) -> Result<BoundingBox3D> {
180        if data.len() == 0 {
181            return Err(ErrorKind::TooFewPoints);
182        }
183        let mut result = data[0].bounding_box();
184        for x in data.iter() {
185            result.consume(x.bounding_box());
186        }
187
188        Ok(result)
189    }
190}
191
192//------------------------------------------------------------------------------
193
194enum Compare {
195    X,
196    Y,
197    Z,
198}
199
200#[derive(Clone)]
201/// Leaf of the AABBTree3D
202pub struct AABBTree3DLeaf<HB>
203where
204    HB: HasBoundingBox3D,
205{
206    data: Vec<HB>,
207    bb: BoundingBox3D,
208    _marker: PhantomData<HB>,
209}
210
211impl<HB> AABBTree3DLeaf<HB>
212where
213    HB: HasBoundingBox3D + Clone,
214{
215    pub fn new(data: Vec<HB>, bb: BoundingBox3D) -> Self {
216        AABBTree3DLeaf {
217            data,
218            bb,
219            _marker: PhantomData,
220        }
221    }
222
223    pub fn any<'a>(&'a self, f: &dyn Fn(&HB) -> bool) -> bool {
224        for x in self.data.iter() {
225            if f(x) {
226                //unwrap fine due to early return in new
227                return true;
228            }
229        }
230        false
231    }
232
233    pub fn for_each_intersection_candidate<'a>(&'a self, line: &Line3D, f: &mut dyn FnMut(&HB)) {
234        if intersection(line, &self.bb).is_none() {
235            return;
236        }
237        for x in self.data.iter() {
238            if intersection(line, &x.bounding_box()).is_some() {
239                f(x)
240            }
241        }
242    }
243
244    pub fn for_each_collision_candidate<'a>(&'a self, bb: &BoundingBox3D, f: &mut dyn FnMut(&HB)) {
245        if !self.bb.collides_with(bb) {
246            return;
247        }
248        for x in self.data.iter() {
249            if x.bounding_box().collides_with(bb) {
250                f(x)
251            }
252        }
253    }
254
255    pub fn bb_colliding<'a>(&'a self, bb: &BoundingBox3D, result: &mut Vec<&'a HB>) {
256        if self.bb.collides_with(bb) {
257            for x in self.data.iter() {
258                if x.bounding_box().collides_with(bb) {
259                    result.push(x)
260                }
261            }
262        }
263    }
264
265    pub fn bb_crossing_x_value<'a>(&'a self, x: f64, result: &mut Vec<&'a HB>) {
266        if self.bb.crossing_x_value(x) {
267            for d in self.data.iter() {
268                if d.bounding_box().crossing_x_value(x) {
269                    result.push(d)
270                }
271            }
272        }
273    }
274
275    pub fn bb_crossing_y_value<'a>(&'a self, y: f64, result: &mut Vec<&'a HB>) {
276        if self.bb.crossing_y_value(y) {
277            for d in self.data.iter() {
278                if d.bounding_box().crossing_y_value(y) {
279                    result.push(d)
280                }
281            }
282        }
283    }
284
285    pub fn bb_crossing_z_value<'a>(&'a self, z: f64, result: &mut Vec<&'a HB>) {
286        if self.bb.crossing_z_value(z) {
287            for d in self.data.iter() {
288                if d.bounding_box().crossing_z_value(z) {
289                    result.push(d)
290                }
291            }
292        }
293    }
294}
295
296//------------------------------------------------------------------------------
297
298#[derive(Clone)]
299/// Branch of the AABBTree3D
300pub struct AABBTree3DBranch<HB>
301where
302    HB: HasBoundingBox3D + Clone,
303{
304    left: Box<AABBTree3D<HB>>,
305    right: Box<AABBTree3D<HB>>,
306    bb: BoundingBox3D,
307    _marker: PhantomData<HB>,
308}
309
310impl<HB> AABBTree3DBranch<HB>
311where
312    HB: HasBoundingBox3D + Clone,
313{
314    pub fn new(left: Box<AABBTree3D<HB>>, right: Box<AABBTree3D<HB>>, bb: BoundingBox3D) -> Self {
315        AABBTree3DBranch {
316            left,
317            right,
318            bb,
319            _marker: PhantomData,
320        }
321    }
322
323    pub fn any<'a>(&'a self, f: &dyn Fn(&HB) -> bool) -> bool {
324        self.left.any(f) || self.right.any(f)
325    }
326
327    pub fn for_each_intersection_candidate<'a>(&'a self, line: &Line3D, f: &mut dyn FnMut(&HB)) {
328        if intersection(line, &self.bb).is_none() {
329            return;
330        }
331
332        self.left.for_each_intersection_candidate(line, f);
333        self.right.for_each_intersection_candidate(line, f);
334    }
335
336    pub fn for_each_collision_candidate<'a>(&'a self, bb: &BoundingBox3D, f: &mut dyn FnMut(&HB)) {
337        if !self.bb.collides_with(bb) {
338            return;
339        }
340
341        self.left.for_each_collision_candidate(bb, f);
342        self.right.for_each_collision_candidate(bb, f);
343    }
344
345    pub fn bb_colliding<'a>(&'a self, bb: &BoundingBox3D, result: &mut Vec<&'a HB>) {
346        if self.bb.collides_with(bb) {
347            self.left.bb_colliding(bb, result);
348            self.right.bb_colliding(bb, result);
349        }
350    }
351
352    pub fn bb_crossing_x_value<'a>(&'a self, x: f64, result: &mut Vec<&'a HB>) {
353        if self.bb.crossing_x_value(x) {
354            self.left.bb_crossing_x_value(x, result);
355            self.right.bb_crossing_x_value(x, result);
356        }
357    }
358
359    pub fn bb_crossing_y_value<'a>(&'a self, y: f64, result: &mut Vec<&'a HB>) {
360        if self.bb.crossing_y_value(y) {
361            self.left.bb_crossing_y_value(y, result);
362            self.right.bb_crossing_y_value(y, result);
363        }
364    }
365
366    pub fn bb_crossing_z_value<'a>(&'a self, z: f64, result: &mut Vec<&'a HB>) {
367        if self.bb.crossing_z_value(z) {
368            self.left.bb_crossing_z_value(z, result);
369            self.right.bb_crossing_z_value(z, result);
370        }
371    }
372}
373
374//------------------------------------------------------------------------------
375
376impl<HB> IsColliderContainer3D for AABBTree3D<HB>
377where
378    HB: Clone + HasColliders3D + Sized,
379{
380    fn any_element_collides_with_collider(&self, other: &dyn HasColliders3D) -> bool {
381        let mut any_collides = false;
382        self.for_each_collision_candidate(&other.bounding_box(), &mut |candidate| {
383            if !any_collides {
384                any_collides = candidate.collides_with(other);
385            }
386        });
387
388        any_collides
389    }
390
391    fn any_element_collides_with_bounding(&self, other: &dyn HasBoundingBox3D) -> bool {
392        let mut any_collides = false;
393        self.for_each_collision_candidate(&other.bounding_box(), &mut |_candidate| {
394            any_collides = true;
395        });
396
397        any_collides
398    }
399}
rust_3d/aa_bb_tree_3d.rs

rust_3d/
aa_bb_tree_3d.rs
