vox_geometry_rust 0.1.2

/*
 * // Copyright (c) 2021 Feng Yang
 * //
 * // I am making my contributions/submissions to this project solely in my
 * // personal capacity and am not conveying any rights to any intellectual
 * // property of any third parties.
 */

use crate::vector3::Vector3D;
use crate::ray3::Ray3D;
use crate::bounding_box3::BoundingBox3D;
use crate::intersection_query_engine3::*;
use crate::nearest_neighbor_query_engine3::*;
use std::cmp::Ordering;

#[derive(Clone)]
struct Node {
    first_child: usize,
    items: Vec<usize>,
}

impl Node {
    fn new() -> Node {
        return Node {
            first_child: usize::MAX,
            items: vec![],
        };
    }

    fn is_leaf(&self) -> bool {
        return self.first_child == usize::MAX;
    }
}

///
/// #  Generic Octree data structure.
///
/// This class is a generic Octree representation to store arbitrary spatial
/// data. The Octree supports closest neighbor search, overlapping test, and
/// ray intersection test.
///
/// - tparam     T     Value type.
///
pub struct Octree<T: Clone> {
    _max_depth: usize,
    _bbox: BoundingBox3D,
    _items: Vec<T>,
    _nodes: Vec<Node>,
}

impl<T: Clone> Octree<T> {
    /// Default constructor.
    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::octree::Octree;
    /// let octree:Octree<Vector3D> = Octree::new();
    /// assert_eq!(octree.number_of_nodes(), 0);
    /// ```
    pub fn new() -> Octree<T> {
        return Octree {
            _max_depth: 0,
            _bbox: BoundingBox3D::new_default(),
            _items: vec![],
            _nodes: vec![],
        };
    }

    /// Builds an Octree with given list of items, bounding box of the items,
    /// overlapping test function, and max depth of the tree.
    pub fn build<Func>(&mut self, items: &Vec<T>, bound: &BoundingBox3D,
                       test_func: &mut Func, max_depth: usize) where Func: BoxIntersectionTestFunc3<T> {
        // Reset items
        self._max_depth = max_depth;
        self._items = items.clone();
        self._nodes.clear();

        // Normalize bounding box
        self._bbox = bound.clone();
        let max_edge_len = crate::math_utils::max3(self._bbox.width(), self._bbox.height(), self._bbox.depth());
        self._bbox.upper_corner = self._bbox.lower_corner + Vector3D::new(max_edge_len as f64,
                                                                          max_edge_len as f64,
                                                                          max_edge_len as f64);

        // Build
        self._nodes.resize(1, Node::new());
        self._nodes[0].items = (0..self._items.len()).collect();

        let aabb = self._bbox.clone();
        self.build_internal(0, 1, &aabb, test_func);
    }

    /// Clears all the contents of this instance.
    pub fn clear(&mut self) {
        self._max_depth = 1;
        self._items.clear();
        self._nodes.clear();
        self._bbox = BoundingBox3D::new_default();
    }

    /// Returns the number of items.
    pub fn number_of_items(&self) -> usize {
        return self._items.len();
    }

    /// Returns the item at \p i.
    pub fn item(&self, i: usize) -> T {
        return self._items[i].clone();
    }

    /// Returns the number of Octree nodes.
    pub fn number_of_nodes(&self) -> usize {
        return self._nodes.len();
    }

    /// Returns the list of the items for given node index.
    pub fn items_at_node(&self, node_idx: usize) -> Vec<usize> {
        return self._nodes[node_idx].clone().items;
    }

    ///
    /// # Returns a child's index for given node.
    ///
    /// For a given node, its children is stored continuously, such that if the
    /// node's first child's index is i, then i + 1, i + 3, ... , i + 7 are the
    /// indices for its children. The order of octant is x-major.
    ///
    /// - parameter:  node_idx The node index.
    /// - parameter:  child_idx The child index (0 to 7).
    ///
    /// - return     Index of the selected child.
    ///
    pub fn child_index(&self, node_idx: usize, child_idx: usize) -> usize {
        return self._nodes[node_idx].first_child + child_idx;
    }

    /// Returns the bounding box of this octree.
    pub fn bounding_box(&self) -> BoundingBox3D {
        return self._bbox.clone();
    }

    /// Returns the maximum depth of the tree.
    pub fn max_depth(&self) -> usize {
        return self._max_depth;
    }
}

impl<T: Clone> Octree<T> {
    fn build_internal<Func>(&mut self, node_idx: usize, depth: usize,
                            bound: &BoundingBox3D,
                            test_func: &mut Func)
        where Func: BoxIntersectionTestFunc3<T> {
        if depth < self._max_depth && !self._nodes[node_idx].items.is_empty() {
            let first_child = self._nodes.len();
            self._nodes[node_idx].first_child = self._nodes.len();
            self._nodes.resize(self._nodes[node_idx].first_child + 8, Node::new());

            let mut bbox_per_node: Vec<BoundingBox3D> = (0..8).map(|_| { BoundingBox3D::new_default() }).collect();

            for i in 0..8 {
                bbox_per_node[i] = BoundingBox3D::new(bound.corner(i), bound.mid_point());
            }

            for i in 0..self._nodes[node_idx].items.len() {
                let current_item = self._nodes[node_idx].items[i];
                for j in 0..8 {
                    if (*test_func)(&self._items[current_item], &bbox_per_node[j]) {
                        self._nodes[first_child + j].items.push(current_item);
                    }
                }
            }

            // Remove non-leaf data
            self._nodes[node_idx].items.clear();

            // Refine
            for i in 0..8 {
                self.build_internal(first_child + i, depth + 1, &bbox_per_node[i], test_func);
            }
        }
    }

    fn intersects_aabb<Func>(&self, aabb: &BoundingBox3D,
                             test_func: &mut Func, node_idx: usize,
                             bound: &BoundingBox3D) -> bool
        where Func: BoxIntersectionTestFunc3<T> {
        if !aabb.overlaps(bound) {
            return false;
        }

        let node = &self._nodes[node_idx];

        if node.items.len() > 0 {
            for item_idx in &node.items {
                if test_func(&self._items[*item_idx], aabb) {
                    return true;
                }
            }
        }

        if node.first_child != usize::MAX {
            for i in 0..8 {
                if self.intersects_aabb(aabb, test_func, node.first_child + i,
                                        &BoundingBox3D::new(bound.corner(i),
                                                            bound.mid_point())) {
                    return true;
                }
            }
        }

        return false;
    }

    fn intersects_ray<Func>(&self, ray: &Ray3D,
                            test_func: &mut Func, node_idx: usize,
                            bound: &BoundingBox3D) -> bool
        where Func: RayIntersectionTestFunc3<T> {
        if !bound.intersects(ray) {
            return false;
        }

        let node = &self._nodes[node_idx];

        if node.items.len() > 0 {
            for item_idx in &node.items {
                if test_func(&self._items[*item_idx], ray) {
                    return true;
                }
            }
        }

        if node.first_child != usize::MAX {
            for i in 0..8 {
                if self.intersects_ray(ray, test_func, node.first_child + i,
                                       &BoundingBox3D::new(bound.corner(i),
                                                           bound.mid_point())) {
                    return true;
                }
            }
        }

        return false;
    }

    fn for_each_intersecting_item_aabb<TestFunc, Visitor>(&self, aabb: &BoundingBox3D,
                                                          test_func: &mut TestFunc,
                                                          visitor_func: &mut Visitor,
                                                          node_idx: usize,
                                                          bound: &BoundingBox3D)
        where TestFunc: BoxIntersectionTestFunc3<T>,
              Visitor: IntersectionVisitorFunc3<T>, {
        if !aabb.overlaps(bound) {
            return;
        }

        let node = &self._nodes[node_idx];

        if node.items.len() > 0 {
            for item_idx in &node.items {
                if test_func(&self._items[*item_idx], aabb) {
                    visitor_func(&self._items[*item_idx]);
                }
            }
        }

        if node.first_child != usize::MAX {
            for i in 0..8 {
                self.for_each_intersecting_item_aabb(
                    aabb, test_func, visitor_func, node.first_child + i,
                    &BoundingBox3D::new(bound.corner(i), bound.mid_point()));
            }
        }
    }

    fn for_each_intersecting_item_ray<TestFunc, Visitor>(&self, ray: &Ray3D,
                                                         test_func: &mut TestFunc,
                                                         visitor_func: &mut Visitor,
                                                         node_idx: usize,
                                                         bound: &BoundingBox3D)
        where TestFunc: RayIntersectionTestFunc3<T>,
              Visitor: IntersectionVisitorFunc3<T> {
        if !bound.intersects(ray) {
            return;
        }

        let node = &self._nodes[node_idx];

        if node.items.len() > 0 {
            for item_idx in &node.items {
                if test_func(&self._items[*item_idx], ray) {
                    visitor_func(&self._items[*item_idx]);
                }
            }
        }

        if node.first_child != usize::MAX {
            for i in 0..8 {
                self.for_each_intersecting_item_ray(
                    ray, test_func, visitor_func, node.first_child + i,
                    &BoundingBox3D::new(bound.corner(i), bound.mid_point()));
            }
        }
    }

    fn closest_intersection<Func>(&self, ray: &Ray3D, test_func: &mut Func,
                                  node_idx: usize, bound: &BoundingBox3D,
                                  best: ClosestIntersectionQueryResult3<T>) -> ClosestIntersectionQueryResult3<T>
        where Func: GetRayIntersectionFunc3<T> {
        let mut best = best;

        if !bound.intersects(ray) {
            return best;
        }

        let node = &self._nodes[node_idx];

        if node.items.len() > 0 {
            for item_idx in &node.items {
                let dist = test_func(&self._items[*item_idx], ray);
                if dist < best.distance {
                    best.distance = dist;
                    best.item = Some(self._items[*item_idx].clone());
                }
            }
        }

        if node.first_child != usize::MAX {
            for i in 0..8 {
                best = self.closest_intersection(
                    ray, test_func, node.first_child + i,
                    &BoundingBox3D::new(bound.corner(i), bound.mid_point()), best);
            }
        }

        return best;
    }
}

impl<T: Clone> IntersectionQueryEngine3<T> for Octree<T> {
    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
    /// use vox_geometry_rust::octree::Octree;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine3::NearestNeighborQueryEngine3;
    /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
    /// let mut octree:Octree<Vector3D> = Octree::new();
    ///
    /// let mut overlaps_func = |pt:&Vector3D, bbox:&BoundingBox3D| {
    ///         return bbox.contains(pt);
    ///     };
    ///
    /// let num_samples = get_number_of_sample_points3();
    /// let points = (0..100).map(|index| Vector3D::new_lst(get_sample_points3()[index])).collect();
    ///
    /// octree.build(&points, &BoundingBox3D::new(Vector3D::new_default(), Vector3D::new(1.0, 1.0, 1.0)), &mut overlaps_func, 5);
    ///
    /// let test_box = BoundingBox3D::new(Vector3D::new(0.25, 0.15, 0.3), Vector3D::new(0.5, 0.6, 0.4));
    /// let mut has_overlaps = false;
    /// for i in 0..num_samples {
    ///     has_overlaps |= overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box);
    /// }
    ///
    /// assert_eq!(has_overlaps, octree.intersects_aabb(&test_box, &mut overlaps_func));
    ///
    /// let test_box3 = BoundingBox3D::new(Vector3D::new(0.3, 0.2, 0.1), Vector3D::new(0.6, 0.5, 0.4));
    /// has_overlaps = false;
    /// for i in 0..num_samples {
    ///     has_overlaps |= overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box3);
    /// }
    ///
    /// assert_eq!(has_overlaps, octree.intersects_aabb(&test_box3, &mut overlaps_func));
    /// ```
    fn intersects_aabb<Callback>(&self, aabb: &BoundingBox3D, test_func: &mut Callback) -> bool
        where Callback: BoxIntersectionTestFunc3<T> {
        return self.intersects_aabb(aabb, test_func, 0, &self._bbox);
    }

    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
    /// use vox_geometry_rust::ray3::Ray3D;
    /// use vox_geometry_rust::octree::Octree;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine3::NearestNeighborQueryEngine3;
    /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
    /// let mut octree:Octree<BoundingBox3D> = Octree::new();
    ///
    /// let mut overlaps_func = |a:&BoundingBox3D, bbox:&BoundingBox3D| {
    ///     return bbox.overlaps(a);
    /// };
    ///
    /// let mut intersects_func = |a:&BoundingBox3D, ray:&Ray3D| {
    ///     return a.intersects(ray);
    /// };
    ///
    /// let num_samples = get_number_of_sample_points3();
    /// let items = (0..num_samples / 2).map(|index| {
    ///     let c = Vector3D::new_lst(get_sample_points3()[index]);
    ///     let mut aabb = BoundingBox3D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// octree.build(&items, &BoundingBox3D::new(Vector3D::new_default(), Vector3D::new(1.0, 1.0, 1.0)), &mut overlaps_func, 5);
    /// for i in 0..num_samples / 2 {
    ///     let ray = Ray3D::new(Vector3D::new_lst(get_sample_points3()[i + num_samples / 2]),
    ///               Vector3D::new_lst(get_sample_dirs3()[i + num_samples / 2]));
    ///     // ad-hoc search
    ///     let mut ans_ints = false;
    ///     for j in 0..num_samples / 2 {
    ///         if intersects_func(&items[j], &ray) {
    ///             ans_ints = true;
    ///             break;
    ///         }
    ///     }
    ///
    ///     // Octree search
    ///     let oct_ints = octree.intersects_ray(&ray, &mut intersects_func);
    ///
    ///     assert_eq!(ans_ints, oct_ints);
    /// }
    /// ```
    fn intersects_ray<Callback>(&self, ray: &Ray3D, test_func: &mut Callback) -> bool
        where Callback: RayIntersectionTestFunc3<T> {
        return self.intersects_ray(ray, test_func, 0, &self._bbox);
    }

    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
    /// use vox_geometry_rust::octree::Octree;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine3::NearestNeighborQueryEngine3;
    /// use vox_geometry_rust::intersection_query_engine3::IntersectionQueryEngine3;
    /// let mut octree:Octree<Vector3D> = Octree::new();
    ///
    /// let mut overlaps_func = |pt:&Vector3D, bbox:&BoundingBox3D| {
    ///         return bbox.contains(pt);
    /// };
    ///
    /// let overlaps_func3 = |pt:&Vector3D, bbox:&BoundingBox3D| {
    ///         return bbox.contains(pt);
    /// };
    ///
    /// let num_samples = get_number_of_sample_points3();
    /// let points = (0..100).map(|index| Vector3D::new_lst(get_sample_points3()[index])).collect();
    ///
    /// octree.build(&points, &BoundingBox3D::new(Vector3D::new_default(), Vector3D::new(1.0, 1.0, 1.0)), &mut overlaps_func, 5);
    ///
    /// let test_box = BoundingBox3D::new(Vector3D::new(0.3, 0.2, 0.1), Vector3D::new(0.6, 0.5, 0.4));
    /// let mut num_overlaps = 0;
    /// for i in 0..num_samples {
    ///     num_overlaps += overlaps_func(&Vector3D::new_lst(get_sample_points3()[i]), &test_box) as usize;
    /// }
    ///
    /// let mut measured = 0;
    /// octree.for_each_intersecting_item_aabb(&test_box, &mut overlaps_func,
    ///                              &mut |pt:&Vector3D| {
    ///                                  assert_eq!(overlaps_func3(pt, &test_box), true);
    ///                                  measured += 1;
    ///                              });
    ///
    /// assert_eq!(num_overlaps, measured);
    /// ```
    fn for_each_intersecting_item_aabb<Callback, Visitor>(&self, aabb: &BoundingBox3D, test_func: &mut Callback, visitor_func: &mut Visitor)
        where Callback: BoxIntersectionTestFunc3<T>,
              Visitor: IntersectionVisitorFunc3<T> {
        self.for_each_intersecting_item_aabb(aabb, test_func, visitor_func, 0, &self._bbox);
    }

    fn for_each_intersecting_item_ray<Callback, Visitor>(&self, ray: &Ray3D, test_func: &mut Callback, visitor_func: &mut Visitor)
        where Callback: RayIntersectionTestFunc3<T>,
              Visitor: IntersectionVisitorFunc3<T> {
        self.for_each_intersecting_item_ray(ray, test_func, visitor_func, 0, &self._bbox);
    }

    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::bounding_box3::{BoundingBox3D, BoundingBox3};
    /// use vox_geometry_rust::ray3::Ray3D;
    /// use vox_geometry_rust::octree::Octree;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine3::NearestNeighborQueryEngine3;
    /// use vox_geometry_rust::intersection_query_engine3::*;
    /// let mut octree:Octree<BoundingBox3D> = Octree::new();
    ///
    /// let mut overlaps_func = |a:&BoundingBox3D, bbox:&BoundingBox3D| {
    ///     return bbox.overlaps(a);
    /// };
    ///
    /// let mut intersects_func = |a:&BoundingBox3D, ray:&Ray3D| {
    ///     let bbox_result = a.closest_intersection(ray);
    ///     return if bbox_result.is_intersecting {
    ///          bbox_result.t_near
    ///     } else {
    ///          f64::MAX
    ///     };
    /// };
    ///
    /// let num_samples = get_number_of_sample_points3();
    /// let items = (0..num_samples / 2).map(|index| {
    ///     let c = Vector3D::new_lst(get_sample_points3()[index]);
    ///     let mut aabb = BoundingBox3D::new(c, c);
    ///     aabb.expand(0.1);
    ///     aabb
    /// }).collect();
    ///
    /// octree.build(&items, &BoundingBox3D::new(Vector3D::new_default(), Vector3D::new(1.0, 1.0, 1.0)), &mut overlaps_func, 5);
    ///
    /// for i in 0..num_samples / 2 {
    ///     let ray = Ray3D::new(Vector3D::new_lst(get_sample_points3()[i + num_samples / 2]),
    ///               Vector3D::new_lst(get_sample_dirs3()[i + num_samples / 2]));
    ///     // ad-hoc search
    ///     let mut ans_ints:ClosestIntersectionQueryResult3<BoundingBox3D> = ClosestIntersectionQueryResult3::new();
    ///     for j in 0..num_samples / 2 {
    ///         let dist = intersects_func(&items[j], &ray);
    ///         if dist < ans_ints.distance {
    ///             ans_ints.distance = dist;
    ///             ans_ints.item = Some(octree.item(j));
    ///         }
    ///     }
    ///
    ///     // Octree search
    ///     let oct_ints = octree.closest_intersection(&ray, &mut intersects_func);
    ///
    ///     assert_eq!(ans_ints.distance, oct_ints.distance);
    ///     let ans_aabb = ans_ints.item.unwrap_or(BoundingBox3D::new_default());
    ///     let oct_aabb = oct_ints.item.unwrap_or(BoundingBox3D::new_default());
    ///     assert_eq!(ans_aabb.upper_corner, oct_aabb.upper_corner);
    ///     assert_eq!(ans_aabb.lower_corner, oct_aabb.lower_corner);
    /// }
    /// ```
    fn closest_intersection<Callback>(&self, ray: &Ray3D, test_func: &mut Callback) -> ClosestIntersectionQueryResult3<T>
        where Callback: GetRayIntersectionFunc3<T> {
        let mut best = ClosestIntersectionQueryResult3::new();
        best.distance = f64::MAX;
        best.item = None;

        return self.closest_intersection(ray, test_func, 0, &self._bbox, best);
    }
}

impl<T: Clone> NearestNeighborQueryEngine3<T> for Octree<T> {
    /// ```
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::bounding_box3::BoundingBox3D;
    /// use vox_geometry_rust::octree::Octree;
    /// use vox_geometry_rust::unit_tests_utils::*;
    /// use vox_geometry_rust::nearest_neighbor_query_engine3::NearestNeighborQueryEngine3;
    /// let mut octree:Octree<Vector3D> = Octree::new();
    ///
    /// let mut overlaps_func = |pt:&Vector3D, bbox:&BoundingBox3D| {
    ///     return bbox.contains(pt);
    /// };
    ///
    /// // Single point
    /// octree.build(&vec![Vector3D::new(0.2, 0.7, 0.3)], &BoundingBox3D::new(Vector3D::new_default(), Vector3D::new(0.9, 0.8, 1.0)),
    ///                &mut overlaps_func, 3);
    ///
    /// assert_eq!(3, octree.max_depth());
    /// assert_eq!(Vector3D::new_default(), octree.bounding_box().lower_corner);
    /// assert_eq!(Vector3D::new(1.0, 1.0, 1.0), octree.bounding_box().upper_corner);
    /// assert_eq!(17, octree.number_of_nodes());
    ///
    /// let mut child = octree.child_index(0, 2);
    /// assert_eq!(3, child);
    ///
    /// child = octree.child_index(child, 0);
    /// assert_eq!(9, child);
    ///
    /// let the_non_empty_leaf_node = child + 4;
    /// for i in 0..17 {
    ///     if i == the_non_empty_leaf_node {
    ///         assert_eq!(1, octree.items_at_node(i).len());
    ///     } else {
    ///         assert_eq!(0, octree.items_at_node(i).len());
    ///     }
    /// }
    ///
    /// ```
    fn nearest<Callback>(&self, pt: &Vector3D, distance_func: &mut Callback) -> NearestNeighborQueryResult3<T>
        where Callback: NearestNeighborDistanceFunc3<T> {
        let mut best = NearestNeighborQueryResult3::new();
        best.distance = f64::MAX;
        best.item = None;

        // Prepare to traverse octree
        let mut todo: Vec<(Option<usize>, BoundingBox3D)> = Vec::new();

        // Traverse octree nodes
        let mut node = 0;
        let mut bound = self._bbox.clone();
        while node < self._nodes.len() {
            if self._nodes[node].is_leaf() {
                for item_idx in &self._nodes[node].items {
                    let d = distance_func(&self._items[*item_idx], pt);
                    if d < best.distance {
                        best.distance = d;
                        best.item = Some(self._items[*item_idx].clone());
                    }
                }

                // Grab next node to process from todo stack
                if todo.is_empty() {
                    break;
                } else {
                    node = todo.first().unwrap().0.unwrap();
                    bound = todo.first().unwrap().1.clone();
                    todo.pop();
                }
            } else {
                let best_dist_sqr = best.distance * best.distance;
                let mut child_dist_sqr_pairs: Vec<(usize, f64, BoundingBox3D)> = (0..8).map(|_| { (0, 0.0, BoundingBox3D::new_default()) }).collect();
                let mid_point = bound.mid_point();
                for i in 0..8 {
                    let child = self._nodes[node].first_child + i;
                    let child_bound = BoundingBox3D::new(bound.corner(i), mid_point);
                    let cp = child_bound.clamp(pt);
                    let dist_min_sqr = cp.distance_squared_to(*pt);

                    child_dist_sqr_pairs[i] = (child, dist_min_sqr, child_bound);
                }
                child_dist_sqr_pairs.sort_by(|a: &(usize, f64, BoundingBox3D), b: &(usize, f64, BoundingBox3D)| {
                    return match a.1 > b.1 {
                        true => Ordering::Greater,
                        false => Ordering::Less
                    };
                });

                for i in 0..8 {
                    let child_pair = child_dist_sqr_pairs[i].clone();
                    if child_pair.1 < best_dist_sqr {
                        todo.push((Some(child_pair.0), child_pair.2));
                    }
                }

                if todo.is_empty() {
                    break;
                }

                node = todo.first().unwrap().0.unwrap();
                bound = todo.first().unwrap().1.clone();
                todo.pop();
            }
        }

        return best;
    }
}