vox_geometry_rust/bvh3.rs
1/*
2 * // Copyright (c) 2021 Feng Yang
3 * //
4 * // I am making my contributions/submissions to this project solely in my
5 * // personal capacity and am not conveying any rights to any intellectual
6 * // property of any third parties.
7 */
8
9use crate::bounding_box3::BoundingBox3D;
10use crate::nearest_neighbor_query_engine3::*;
11use crate::intersection_query_engine3::*;
12use crate::ray3::Ray3D;
13use crate::vector3::Vector3D;
14
15enum Info {
16 Child(usize, u8),
17 Item(usize),
18}
19
20struct Node {
21 info: Info,
22 bound: BoundingBox3D,
23}
24
25impl Node {
26 fn new() -> Node {
27 return Node {
28 info: Info::Child(usize::MAX, 0),
29 bound: BoundingBox3D::new_default(),
30 };
31 }
32
33 fn init_leaf(&mut self, it: usize, b: BoundingBox3D) {
34 self.info = Info::Item(it);
35 self.bound = b;
36 }
37 fn init_internal(&mut self, axis: u8, c: usize, b: BoundingBox3D) {
38 self.info = Info::Child(c, axis);
39 self.bound = b;
40 }
41}
42
43///
44/// # Bounding Volume Hierarchy (BVH) in 3D
45///
46/// This class implements the classic bounding volume hierarchy structure in 3D.
47/// It implements IntersectionQueryEngine3 in order to support box/ray
48/// intersection tests. Also, NearestNeighborQueryEngine3 is implemented to
49/// provide nearest neighbor query.
50///
51pub struct Bvh3<T: Clone> {
52 _bound: BoundingBox3D,
53 _items: Vec<T>,
54 _item_bounds: Vec<BoundingBox3D>,
55 _nodes: Vec<Node>,
56}
57
58impl<T: Clone> Bvh3<T> {
59 /// Default constructor.
60 /// ```
61 /// use vox_geometry_rust::bvh3::Bvh3;
62 /// use vox_geometry_rust::vector3::Vector3D;
63 /// let bvh:Bvh3<Vector3D> = Bvh3::new();
64 /// assert_eq!(bvh.number_of_nodes(), 0);
65 /// ```
66 pub fn new() -> Bvh3<T> {
67 return Bvh3 {
68 _bound: BoundingBox3D::new_default(),
69 _items: vec![],
70 _item_bounds: vec![],
71 _nodes: vec![],
72 };
73 }
74
75 /// Builds bounding volume hierarchy.
76 /// ```
77 /// use vox_geometry_rust::bvh3::Bvh3;
78 /// use vox_geometry_rust::vector3::Vector3D;
79 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
80 /// let mut bvh:Bvh3<Vector3D> = Bvh3::new();
81 ///
82 /// let points:Vec<Vector3D> = vec![Vector3D::new_default(), Vector3D::new(1.0, 1.0, 1.0)];
83 /// let mut rootBounds = BoundingBox3D::new_default();
84 /// let bounds = (0..points.len()).map(|index|{
85 /// let c = points[index];
86 /// let mut aabb = BoundingBox3D::new(c, c);
87 /// aabb.expand(0.1);
88 /// rootBounds.merge_box(&aabb);
89 /// return aabb.clone();
90 /// }).collect();
91 ///
92 /// bvh.build(&points, &bounds);
93 ///
94 /// assert_eq!(2, bvh.number_of_items());
95 /// assert_eq!(points[0], *bvh.item(0));
96 /// assert_eq!(points[1], *bvh.item(1));
97 /// assert_eq!(3, bvh.number_of_nodes());
98 /// assert_eq!(1, bvh.children(0).0);
99 /// assert_eq!(2, bvh.children(0).1);
100 /// assert_eq!(bvh.is_leaf(0), false);
101 /// assert_eq!(bvh.is_leaf(1), true);
102 /// assert_eq!(bvh.is_leaf(2), true);
103 /// assert_eq!(rootBounds.lower_corner, bvh.node_bound(0).lower_corner);
104 /// assert_eq!(rootBounds.upper_corner, bvh.node_bound(0).upper_corner);
105 /// assert_eq!(bounds[0].lower_corner, bvh.node_bound(1).lower_corner);
106 /// assert_eq!(bounds[0].upper_corner, bvh.node_bound(1).upper_corner);
107 /// assert_eq!(bounds[1].lower_corner, bvh.node_bound(2).lower_corner);
108 /// assert_eq!(bounds[1].upper_corner, bvh.node_bound(2).upper_corner);
109 /// ```
110 pub fn build(&mut self, items: &Vec<T>,
111 item_bounds: &Vec<BoundingBox3D>) {
112 self._items = items.clone();
113 self._item_bounds = item_bounds.clone();
114
115 if self._items.is_empty() {
116 return;
117 }
118
119 self._nodes.clear();
120 self._bound = BoundingBox3D::new_default();
121
122 for i in 0..self._items.len() {
123 self._bound.merge_box(&self._item_bounds[i]);
124 }
125
126 let mut item_indices: Vec<usize> = (0..self._items.len()).collect();
127
128 self.build_internal(0, &mut item_indices, 0, self._items.len(), 0);
129 }
130
131 /// Clears all the contents of this instance.
132 pub fn clear(&mut self) {
133 self._bound = BoundingBox3D::new_default();
134 self._items.clear();
135 self._item_bounds.clear();
136 self._nodes.clear();
137 }
138
139 fn build_internal(&mut self, node_index: usize, item_indices: &mut Vec<usize>, item_start: usize, n_items: usize,
140 current_depth: usize) -> usize {
141 // add a node
142 self._nodes.push(Node::new());
143
144 // initialize leaf node if termination criteria met
145 if n_items == 1 {
146 self._nodes[node_index].init_leaf(item_indices[item_start], self._item_bounds[item_indices[item_start]].clone());
147 return current_depth + 1;
148 }
149
150 // find the mid-point of the bounding box to use as a qsplit pivot
151 let mut node_bound = BoundingBox3D::new_default();
152 for i in 0..n_items {
153 node_bound.merge_box(&self._item_bounds[item_indices[item_start + i]]);
154 }
155
156 let d = node_bound.upper_corner - node_bound.lower_corner;
157
158 // choose which axis to split along
159 let axis: u8;
160 if d.x > d.y && d.x > d.z {
161 axis = 0;
162 } else {
163 axis = match d.y > d.z {
164 true => 1,
165 false => 2
166 };
167 }
168
169 let pivot = 0.5 * (node_bound.upper_corner[axis.into()] + node_bound.lower_corner[axis.into()]);
170
171 // classify primitives with respect to split
172 let mid_point = self.qsplit(item_indices, item_start, n_items, pivot, axis);
173
174 // recursively initialize children _nodes
175 let d0 = self.build_internal(node_index + 1, item_indices, item_start, mid_point, current_depth + 1);
176 let len = self._nodes.len();
177 self._nodes[node_index].init_internal(axis, len, node_bound);
178 let d1 = match self._nodes[node_index].info {
179 Info::Child(index, _flag) => {
180 self.build_internal(index, item_indices, item_start + mid_point,
181 n_items - mid_point, current_depth + 1)
182 }
183 Info::Item(_index) => { panic!() }
184 };
185
186 return usize::max(d0, d1);
187 }
188
189 fn qsplit(&self, item_indices: &mut Vec<usize>, item_start: usize, num_items: usize, pivot: f64,
190 axis: u8) -> usize {
191 let mut centroid: f64;
192 let mut ret = 0;
193 for i in 0..num_items {
194 let b = self._item_bounds[item_indices[item_start + i]].clone();
195 centroid = 0.5 * (b.lower_corner[axis.into()] + b.upper_corner[axis.into()]);
196 if centroid < pivot {
197 item_indices.swap(item_start + i, item_start + ret);
198 ret += 1;
199 }
200 }
201 if ret == 0 || ret == num_items {
202 ret = num_items >> 1;
203 }
204 return ret;
205 }
206
207 /// Returns the number of items.
208 pub fn number_of_items(&self) -> usize {
209 return self._items.len();
210 }
211
212 /// Returns the item at \p i.
213 pub fn item(&self, i: usize) -> &T {
214 return &self._items[i];
215 }
216
217 /// Returns the number of nodes.
218 pub fn number_of_nodes(&self) -> usize {
219 return self._nodes.len();
220 }
221
222 /// Returns the children indices of \p i-th node.
223 pub fn children(&self, i: usize) -> (usize, usize) {
224 return if self.is_leaf(i) {
225 (usize::MAX, usize::MAX)
226 } else {
227 match self._nodes[i].info {
228 Info::Child(index, _flag) => {
229 (i + 1, index)
230 }
231 Info::Item(_) => { panic!() }
232 }
233 };
234 }
235
236 /// Returns true if \p i-th node is a leaf node.
237 pub fn is_leaf(&self, i: usize) -> bool {
238 return match self._nodes[i].info {
239 Info::Child(_, _) => { false }
240 Info::Item(_) => { true }
241 };
242 }
243
244 /// Returns bounding box of \p i-th node.
245 pub fn node_bound(&self, i: usize) -> BoundingBox3D {
246 return self._nodes[i].bound.clone();
247 }
248
249 /// Returns bounding box of every items.
250 pub fn bounding_box(&self) -> BoundingBox3D {
251 return self._bound.clone();
252 }
253
254 /// Returns item of \p i-th node.
255 pub fn item_of_node(&self, i: usize) -> usize {
256 match self._nodes[i].info {
257 Info::Child(_, _) => {
258 return self._nodes.len();
259 }
260 Info::Item(index) => {
261 return index;
262 }
263 }
264 }
265}
266
267impl<T: Clone> IntersectionQueryEngine3<T> for Bvh3<T> {
268 /// ```
269 /// use vox_geometry_rust::bvh3::Bvh3;
270 /// use vox_geometry_rust::vector3::Vector3D;
271 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
272 /// use vox_geometry_rust::unit_tests_utils::*;
273 /// use vox_geometry_rust::nearest_neighbor_query_engine3::*;
274 /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
275 /// let mut bvh:Bvh3<Vector3D> = Bvh3::new();
276 ///
277 /// let mut overlaps_func = |pt:&Vector3D, bbox:&BoundingBox3D| {
278 /// let mut aabb = BoundingBox3D::new(*pt, *pt);
279 /// aabb.expand(0.1);
280 /// return bbox.overlaps(&aabb);
281 /// };
282 ///
283 /// let num_samples = get_number_of_sample_points3();
284 /// let points = (0..num_samples).map(|index| Vector3D::new_lst(get_sample_points3()[index])).collect();
285 ///
286 /// let bounds = (0..num_samples).map(|index| {
287 /// let c = Vector3D::new_lst(get_sample_points3()[index]);
288 /// let mut aabb = BoundingBox3D::new(c, c);
289 /// aabb.expand(0.1);
290 /// aabb
291 /// }).collect();
292 ///
293 /// bvh.build(&points, &bounds);
294 ///
295 /// let test_box = BoundingBox3D::new(Vector3D::new(0.25, 0.15, 0.3), Vector3D::new(0.5, 0.6, 0.4));
296 /// let mut has_overlaps = false;
297 /// for i in 0..num_samples {
298 /// has_overlaps |= overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box);
299 /// }
300 ///
301 /// assert_eq!(has_overlaps, bvh.intersects_aabb(&test_box, &mut overlaps_func));
302 ///
303 /// let test_box3 = BoundingBox3D::new(Vector3D::new(0.3, 0.2, 0.1), Vector3D::new(0.6, 0.5, 0.4));
304 /// has_overlaps = false;
305 /// for i in 0..num_samples {
306 /// has_overlaps |= overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box3);
307 /// }
308 ///
309 /// assert_eq!(has_overlaps, bvh.intersects_aabb(&test_box3, &mut overlaps_func));
310 /// ```
311 fn intersects_aabb<Callback>(&self, aabb: &BoundingBox3D, test_func: &mut Callback) -> bool
312 where Callback: BoxIntersectionTestFunc3<T> {
313 if !self._bound.overlaps(aabb) {
314 return false;
315 }
316
317 // prepare to traverse BVH for box
318 const K_MAX_TREE_DEPTH: usize = 8 * 33;
319 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
320 let mut todo_pos = 0;
321
322 // traverse BVH nodes for box
323 let mut node: usize = 0;
324 while node < self._nodes.len() {
325 match self._nodes[node].info {
326 Info::Child(index, _flag) => {
327 // get node children pointers for box
328 let first_child = node + 1;
329 let second_child = index;
330
331 // advance to next child node, possibly enqueue other child
332 if !self._nodes[first_child].bound.overlaps(aabb) {
333 node = second_child;
334 } else if !self._nodes[second_child].bound.overlaps(aabb) {
335 node = first_child;
336 } else {
337 // enqueue second_child in todo stack
338 todo[todo_pos] = Some(second_child);
339 todo_pos += 1;
340 node = first_child;
341 }
342 }
343 Info::Item(index) => {
344 if test_func(&self._items[index], aabb) {
345 return true;
346 }
347
348 // grab next node to process from todo stack
349 if todo_pos > 0 {
350 // Dequeue
351 todo_pos -= 1;
352 node = todo[todo_pos].unwrap();
353 } else {
354 break;
355 }
356 }
357 }
358 }
359
360 return false;
361 }
362
363 /// ```
364 /// use vox_geometry_rust::bvh3::Bvh3;
365 /// use vox_geometry_rust::vector3::Vector3D;
366 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
367 /// use vox_geometry_rust::ray3::Ray3D;
368 /// use vox_geometry_rust::unit_tests_utils::*;
369 /// use vox_geometry_rust::nearest_neighbor_query_engine3::*;
370 /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
371 /// let mut bvh:Bvh3<BoundingBox3D> = Bvh3::new();
372 ///
373 /// let mut intersects_func = |a:&BoundingBox3D, ray:&Ray3D| {
374 /// return a.intersects(ray);
375 /// };
376 ///
377 /// let num_samples = get_number_of_sample_points3();
378 /// let items = (0..num_samples / 2).map(|index| {
379 /// let c = Vector3D::new_lst(get_sample_points3()[index]);
380 /// let mut aabb = BoundingBox3D::new(c, c);
381 /// aabb.expand(0.1);
382 /// aabb
383 /// }).collect();
384 ///
385 /// bvh.build(&items, &items);
386 ///
387 /// for i in 0..num_samples / 2 {
388 /// let ray = Ray3D::new(Vector3D::new_lst(get_sample_dirs3()[i + num_samples / 2]),
389 /// Vector3D::new_lst(get_sample_dirs3()[i + num_samples / 2]));
390 /// // ad-hoc search
391 /// let mut ans_ints = false;
392 /// for j in 0..num_samples / 2 {
393 /// if intersects_func(&items[j], &ray) {
394 /// ans_ints = true;
395 /// break;
396 /// }
397 /// }
398 ///
399 /// // bvh search
400 /// let oct_ints = bvh.intersects_ray(&ray, &mut intersects_func);
401 ///
402 /// assert_eq!(ans_ints, oct_ints);
403 /// }
404 /// ```
405 fn intersects_ray<Callback>(&self, ray: &Ray3D, test_func: &mut Callback) -> bool
406 where Callback: RayIntersectionTestFunc3<T> {
407 if !self._bound.intersects(ray) {
408 return false;
409 }
410
411 // prepare to traverse BVH for ray
412 const K_MAX_TREE_DEPTH: usize = 8 * 33;
413 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
414 let mut todo_pos = 0;
415
416 // traverse BVH nodes for box
417 let mut node: usize = 0;
418 while node < self._nodes.len() {
419 match self._nodes[node].info {
420 Info::Child(index, flag) => {
421 // get node children pointers for ray
422 let first_child: usize;
423 let second_child: usize;
424 if ray.direction[flag.into()] > 0.0 {
425 first_child = node + 1;
426 second_child = index;
427 } else {
428 first_child = index;
429 second_child = node + 1;
430 }
431
432 // advance to next child node, possibly enqueue other child
433 if !self._nodes[first_child].bound.intersects(ray) {
434 node = second_child;
435 } else if !self._nodes[second_child].bound.intersects(ray) {
436 node = first_child;
437 } else {
438 // enqueue second_child in todo stack
439 todo[todo_pos] = Some(second_child);
440 todo_pos += 1;
441 node = first_child;
442 }
443 }
444 Info::Item(index) => {
445 if test_func(&self._items[index], ray) {
446 return true;
447 }
448
449 // grab next node to process from todo stack
450 if todo_pos > 0 {
451 // Dequeue
452 todo_pos -= 1;
453 node = todo[todo_pos].unwrap();
454 } else {
455 break;
456 }
457 }
458 }
459 }
460
461 return false;
462 }
463
464 /// ```
465 /// use vox_geometry_rust::bvh3::Bvh3;
466 /// use vox_geometry_rust::vector3::Vector3D;
467 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
468 /// use vox_geometry_rust::unit_tests_utils::*;
469 /// use vox_geometry_rust::nearest_neighbor_query_engine3::*;
470 /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
471 /// let mut bvh:Bvh3<Vector3D> = Bvh3::new();
472 ///
473 /// let mut overlaps_func = |pt:&Vector3D, bbox:&BoundingBox3D| {
474 /// return bbox.contains(pt);
475 /// };
476 ///
477 /// let overlaps_func3 = |pt:&Vector3D, bbox:&BoundingBox3D| {
478 /// return bbox.contains(pt);
479 /// };
480 ///
481 /// let num_samples = get_number_of_sample_points3();
482 /// let points = (0..num_samples).map(|index| Vector3D::new_lst(get_sample_points3()[index])).collect();
483 ///
484 /// let bounds = (0..num_samples).map(|index| {
485 /// let c = Vector3D::new_lst(get_sample_points3()[index]);
486 /// let mut aabb = BoundingBox3D::new(c, c);
487 /// aabb.expand(0.1);
488 /// aabb
489 /// }).collect();
490 ///
491 /// bvh.build(&points, &bounds);
492 ///
493 /// let test_box = BoundingBox3D::new(Vector3D::new(0.3, 0.2, 0.1), Vector3D::new(0.6, 0.5, 0.4));
494 /// let mut num_overlaps = 0;
495 /// for i in 0..num_samples {
496 /// num_overlaps += overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box) as usize;
497 /// }
498 ///
499 /// let mut measured = 0;
500 /// bvh.for_each_intersecting_item_aabb(&test_box, &mut overlaps_func, &mut |pt:&Vector3D| {
501 /// assert_eq!(overlaps_func3(pt, &test_box), true);
502 /// measured += 1;
503 /// });
504 ///
505 /// assert_eq!(num_overlaps, measured);
506 /// ```
507 fn for_each_intersecting_item_aabb<Callback, Visitor>(&self, aabb: &BoundingBox3D,
508 test_func: &mut Callback, visitor_func: &mut Visitor)
509 where Callback: BoxIntersectionTestFunc3<T>,
510 Visitor: IntersectionVisitorFunc3<T> {
511 if !self._bound.overlaps(aabb) {
512 return;
513 }
514
515 // prepare to traverse BVH for box
516 const K_MAX_TREE_DEPTH: usize = 8 * 33;
517 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
518 let mut todo_pos = 0;
519
520 // traverse BVH nodes for box
521 let mut node: usize = 0;
522 while node < self._nodes.len() {
523 match self._nodes[node].info {
524 Info::Child(index, _flag) => {
525 // get node children pointers for box
526 let first_child = node + 1;
527 let second_child = index;
528
529 // advance to next child node, possibly enqueue other child
530 if !self._nodes[first_child].bound.overlaps(aabb) {
531 node = second_child;
532 } else if !self._nodes[second_child].bound.overlaps(aabb) {
533 node = first_child;
534 } else {
535 // enqueue second_child in todo stack
536 todo[todo_pos] = Some(second_child);
537 todo_pos += 1;
538 node = first_child;
539 }
540 }
541 Info::Item(index) => {
542 if test_func(&self._items[index], aabb) {
543 visitor_func(&self._items[index]);
544 }
545
546 // grab next node to process from todo stack
547 if todo_pos > 0 {
548 // Dequeue
549 todo_pos -= 1;
550 node = todo[todo_pos].unwrap();
551 } else {
552 break;
553 }
554 }
555 }
556 }
557 }
558
559 fn for_each_intersecting_item_ray<Callback, Visitor>(&self, ray: &Ray3D,
560 test_func: &mut Callback, visitor_func: &mut Visitor)
561 where Callback: RayIntersectionTestFunc3<T>,
562 Visitor: IntersectionVisitorFunc3<T> {
563 if !self._bound.intersects(ray) {
564 return;
565 }
566
567 // prepare to traverse BVH for box
568 const K_MAX_TREE_DEPTH: usize = 8 * 33;
569 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
570 let mut todo_pos = 0;
571
572 // traverse BVH nodes for box
573 let mut node: usize = 0;
574 while node < self._nodes.len() {
575 match self._nodes[node].info {
576 Info::Child(index, flag) => {
577 // get node children pointers for ray
578 let first_child: usize;
579 let second_child: usize;
580 if ray.direction[flag.into()] > 0.0 {
581 first_child = node + 1;
582 second_child = index;
583 } else {
584 first_child = index;
585 second_child = node + 1;
586 }
587
588 // advance to next child node, possibly enqueue other child
589 if !self._nodes[first_child].bound.intersects(ray) {
590 node = second_child;
591 } else if !self._nodes[second_child].bound.intersects(ray) {
592 node = first_child;
593 } else {
594 // enqueue second_child in todo stack
595 todo[todo_pos] = Some(second_child);
596 todo_pos += 1;
597 node = first_child;
598 }
599 }
600 Info::Item(index) => {
601 if test_func(&self._items[index], ray) {
602 visitor_func(&self._items[index]);
603 }
604
605 // grab next node to process from todo stack
606 if todo_pos > 0 {
607 // Dequeue
608 todo_pos -= 1;
609 node = todo[todo_pos].unwrap();
610 } else {
611 break;
612 }
613 }
614 }
615 }
616 }
617
618 /// ```
619 /// use vox_geometry_rust::bvh3::Bvh3;
620 /// use vox_geometry_rust::vector3::Vector3D;
621 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
622 /// use vox_geometry_rust::ray3::Ray3D;
623 /// use vox_geometry_rust::unit_tests_utils::*;
624 /// use vox_geometry_rust::nearest_neighbor_query_engine3::*;
625 /// use vox_geometry_rust::intersection_query_engine3::*;
626 /// let mut bvh:Bvh3<BoundingBox3D> = Bvh3::new();
627 ///
628 /// let mut intersects_func = |a:&BoundingBox3D, ray:&Ray3D| {
629 /// let bbox_result = a.closest_intersection(ray);
630 /// return if bbox_result.is_intersecting {
631 /// bbox_result.t_near
632 /// } else {
633 /// f64::MAX
634 /// };
635 /// };
636 ///
637 /// let num_samples = get_number_of_sample_points3();
638 /// let items = (0..num_samples / 2).map(|index| {
639 /// let c = Vector3D::new_lst(get_sample_points3()[index]);
640 /// let mut aabb = BoundingBox3D::new(c, c);
641 /// aabb.expand(0.1);
642 /// aabb
643 /// }).collect();
644 ///
645 /// bvh.build(&items, &items);
646 ///
647 /// for i in 0..num_samples / 2 {
648 /// let ray = Ray3D::new(Vector3D::new_lst(get_sample_points3()[i + num_samples / 2]),
649 /// Vector3D::new_lst(get_sample_dirs3()[i + num_samples / 2]));
650 /// // ad-hoc search
651 /// let mut ans_ints:ClosestIntersectionQueryResult3<BoundingBox3D> = ClosestIntersectionQueryResult3::new();
652 /// for j in 0..num_samples / 2 {
653 /// let dist = intersects_func(&items[j], &ray);
654 /// if dist < ans_ints.distance {
655 /// ans_ints.distance = dist;
656 /// ans_ints.item = Some(bvh.item(j).clone());
657 /// }
658 /// }
659 ///
660 /// // bvh search
661 /// let bvh_ints = bvh.closest_intersection(&ray, &mut intersects_func);
662 ///
663 /// assert_eq!(ans_ints.distance, bvh_ints.distance);
664 /// let ans_aabb = ans_ints.item.unwrap_or(BoundingBox3D::new_default());
665 /// let oct_aabb = bvh_ints.item.unwrap_or(BoundingBox3D::new_default());
666 /// assert_eq!(ans_aabb.upper_corner, oct_aabb.upper_corner);
667 /// assert_eq!(ans_aabb.lower_corner, oct_aabb.lower_corner);
668 /// }
669 /// ```
670 fn closest_intersection<Callback>(&self, ray: &Ray3D, test_func: &mut Callback) -> ClosestIntersectionQueryResult3<T>
671 where Callback: GetRayIntersectionFunc3<T> {
672 let mut best: ClosestIntersectionQueryResult3<T> = ClosestIntersectionQueryResult3::new();
673 best.distance = f64::MAX;
674 best.item = None;
675
676 if !self._bound.intersects(ray) {
677 return best;
678 }
679
680 // prepare to traverse BVH for ray
681 const K_MAX_TREE_DEPTH: usize = 8 * 33;
682 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
683 let mut todo_pos = 0;
684
685 // traverse BVH nodes for ray
686 let mut node: usize = 0;
687 while node < self._nodes.len() {
688 match self._nodes[node].info {
689 Info::Child(index, flag) => {
690 // get node children pointers for ray
691 let first_child: usize;
692 let second_child: usize;
693 if ray.direction[flag.into()] > 0.0 {
694 first_child = node + 1;
695 second_child = index;
696 } else {
697 first_child = index;
698 second_child = node + 1;
699 }
700
701 // advance to next child node, possibly enqueue other child
702 if !self._nodes[first_child].bound.intersects(ray) {
703 node = second_child;
704 } else if !self._nodes[second_child].bound.intersects(ray) {
705 node = first_child;
706 } else {
707 // enqueue second_child in todo stack
708 todo[todo_pos] = Some(second_child);
709 todo_pos += 1;
710 node = first_child;
711 }
712 }
713 Info::Item(index) => {
714 let dist = test_func(&self._items[index], ray);
715 if dist < best.distance {
716 best.distance = dist;
717 best.item = Some(self._items[index].clone());
718 }
719
720 // grab next node to process from todo stack
721 if todo_pos > 0 {
722 // Dequeue
723 todo_pos -= 1;
724 node = todo[todo_pos].unwrap();
725 } else {
726 break;
727 }
728 }
729 }
730 }
731
732 return best;
733 }
734}
735
736impl<T: Clone> NearestNeighborQueryEngine3<T> for Bvh3<T> {
737 /// ```
738 /// use vox_geometry_rust::bvh3::Bvh3;
739 /// use vox_geometry_rust::vector3::Vector3D;
740 /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
741 /// use vox_geometry_rust::unit_tests_utils::*;
742 /// use vox_geometry_rust::nearest_neighbor_query_engine3::*;
743 /// let mut bvh:Bvh3<Vector3D> = Bvh3::new();
744 ///
745 /// let mut distance_func = |a:&Vector3D, b:&Vector3D| {
746 /// return a.distance_to(*b);
747 /// };
748 ///
749 /// let num_samples = get_number_of_sample_points3();
750 /// let points = (0..num_samples).map(|index| Vector3D::new_lst(get_sample_points3()[index])).collect();
751 ///
752 /// let bounds = (0..num_samples).map(|index| {
753 /// let c = Vector3D::new_lst(get_sample_points3()[index]);
754 /// let mut aabb = BoundingBox3D::new(c, c);
755 /// aabb.expand(0.1);
756 /// aabb
757 /// }).collect();
758 ///
759 /// bvh.build(&points, &bounds);
760 ///
761 /// let test_pt = Vector3D::new(0.5, 0.5, 0.5);
762 /// let nearest = bvh.nearest(&test_pt, &mut distance_func);
763 /// let mut answer_idx = 0;
764 /// let mut best_dist = test_pt.distance_to(points[answer_idx]);
765 /// for i in 1..num_samples {
766 /// let dist = test_pt.distance_to(Vector3D::new_lst(get_sample_points3()[i]));
767 /// if dist < best_dist {
768 /// best_dist = dist;
769 /// answer_idx = i;
770 /// }
771 /// }
772 ///
773 /// assert_eq!(nearest.item.unwrap(), *bvh.item(answer_idx));
774 /// ```
775 fn nearest<Callback>(&self, pt: &Vector3D, distance_func: &mut Callback) -> NearestNeighborQueryResult3<T>
776 where Callback: NearestNeighborDistanceFunc3<T> {
777 let mut best: NearestNeighborQueryResult3<T> = NearestNeighborQueryResult3::new();
778 best.distance = f64::MAX;
779 best.item = None;
780
781 // Prepare to traverse BVH
782 const K_MAX_TREE_DEPTH: usize = 8 * 33;
783 let mut todo: [Option<usize>; K_MAX_TREE_DEPTH] = [None; K_MAX_TREE_DEPTH];
784 let mut todo_pos = 0;
785
786 // Traverse BVH nodes
787 let mut node: usize = 0;
788 while node < self._nodes.len() {
789 match self._nodes[node].info {
790 Info::Child(index, _flag) => {
791 let best_dist_sqr = best.distance * best.distance;
792
793 let left = node + 1;
794 let right = index;
795
796 // If pt is inside the box, then the closest_left and Right will be
797 // identical to pt. This will make dist_min_left_sqr and
798 // dist_min_right_sqr zero, meaning that such a box will have higher
799 // priority.
800 let closest_left = self._nodes[left].bound.clamp(pt);
801 let closest_right = self._nodes[right].bound.clamp(pt);
802
803 let dist_min_left_sqr = closest_left.distance_squared_to(*pt);
804 let dist_min_right_sqr = closest_right.distance_squared_to(*pt);
805
806 let should_visit_left = dist_min_left_sqr < best_dist_sqr;
807 let should_visit_right = dist_min_right_sqr < best_dist_sqr;
808
809 let first_child: usize;
810 let second_child: usize;
811 if should_visit_left && should_visit_right {
812 if dist_min_left_sqr < dist_min_right_sqr {
813 first_child = left;
814 second_child = right;
815 } else {
816 first_child = right;
817 second_child = left;
818 }
819
820 // Enqueue second_child in todo stack
821 todo[todo_pos] = Some(second_child);
822 todo_pos += 1;
823 node = first_child;
824 } else if should_visit_left {
825 node = left;
826 } else if should_visit_right {
827 node = right;
828 } else {
829 if todo_pos > 0 {
830 // Dequeue
831 todo_pos -= 1;
832 node = todo[todo_pos].unwrap();
833 } else {
834 break;
835 }
836 }
837 }
838 Info::Item(index) => {
839 let dist = distance_func(&self._items[index], pt);
840 if dist < best.distance {
841 best.distance = dist;
842 best.item = Some(self._items[index].clone());
843 }
844
845 // Grab next node to process from todo stack
846 if todo_pos > 0 {
847 // Dequeue
848 todo_pos -= 1;
849 node = todo[todo_pos].unwrap();
850 } else {
851 break;
852 }
853 }
854 }
855 }
856
857 return best;
858 }
859}