Tree.hxx

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco and Enrico Bertolazzi.                                      *
 *                                                                                               *
 * The AABBtree project is distributed under the BSD 2-Clause License.                           *
 *                                                                                               *
 * Davide Stocco                                                               Enrico Bertolazzi *
 * University of Trento                                                     University of Trento *
 * e-mail: davide.stocco@unitn.it                             e-mail: enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
#pragma once
 
#ifndef INCLUDE_AABBTREE_TREE_HXX
#define INCLUDE_AABBTREE_TREE_HXX
 
#include "AABBtree/Box.hxx"
#include "AABBtree/Tree.hxx"
 
namespace AABBtree {

  template <typename Real, Integer N>
  class Tree {
  public:
    static_assert(std::is_floating_point<Real>::value, "Tree Real type must be a floating-point type.");
    static_assert(std::is_integral<Integer>::value, "Tree dimension type must be an integer type.");
    static_assert(N > 0, "Tree dimension must be positive.");
 
    // Constants for numerical computations
    constexpr static Real EPS{std::numeric_limits<Real>::epsilon()};  
    constexpr static Real INF{std::numeric_limits<Real>::infinity()}; 
    constexpr static Real DUMMY_TOL{EPS*static_cast<Real>(100.0)};    

    struct Statistics {
      // Build statistics
      Integer objects{0};          
      Integer nodes{0};            
      Integer leafs{0};            
      Integer long_boxes{0};       
      Integer depth{0};            
      Integer left_nodes{0};       
      Integer left_leafs{0};       
      Integer left_long_boxes{0};  
      Integer left_depth{0};       
      Integer right_nodes{0};      
      Integer right_leafs{0};      
      Integer right_long_boxes{0}; 
      Integer right_depth{0};      
      Integer dump_counter{0};     
      Real    balance_ratio{0.0};  
      Real    depth_ratio{0.0};    
 
      // Query statistics
      Integer check_counter{0}; 

      void reset() noexcept {*this = Statistics{};}

      void print(OutStream & os) const {
        // Print the tree info
        os << "────────────────────────────────────────────────────────────────────────────────\n"
           << "AABB TREE INFO"
           << "\n\tAmbient dimension : " << N
           << "\n\tBasic type        : " << typeid(Real).name()
           << "\n\tObjects           : " << objects
           << "\n\tNodes             : " << nodes
           << "\n\tLeafs             : " << leafs
           << "\n\tLong boxes        : " << long_boxes
           << "\n\tDepth             : " << depth
           << "\n\tLeft nodes        : " << left_nodes
           << "\n\tLeft leafs        : " << left_leafs
           << "\n\tLeft long boxes   : " << left_long_boxes
           << "\n\tLeft depth        : " << left_depth
           << "\n\tRight nodes       : " << right_nodes
           << "\n\tRight leafs       : " << right_leafs
           << "\n\tRight long boxes  : " << right_long_boxes
           << "\n\tRight depth       : " << right_depth
           << "\n\tDump counter      : " << dump_counter
           << "\n\tBalance ratio     : " << balance_ratio
           << "\n\tDepth ratio       : " << depth_ratio
           << "\n\tCheck counter     : " << check_counter
           << "\n────────────────────────────────────────────────────────────────────────────────\n";
      }
 
    };
 
  private:
    // Basic types definitions
    using Box              = AABBtree::Box<Real, N>;
    using BoxUniquePtr     = AABBtree::BoxUniquePtr<Real, N>;
    using BoxUniquePtrList = AABBtree::BoxUniquePtrList<Real, N>;
    using Vector           = AABBtree::Vector<Real, N>;
    using Point            = AABBtree::Point<Real, N>;
    using QueueItem        = std::tuple<Integer, Integer, Real>;
    using PriorityQueue    = std::priority_queue<QueueItem, std::vector<QueueItem>, std::function<bool(QueueItem, QueueItem)>>;

    struct Node {
      Box     box;         
      Box     box_long;    
      Integer box_ptr;     
      Integer box_num;     
      Integer box_tot_num; 
      Integer parent;      
      Integer child_l;     
      Integer child_r;     
    };
 
    // Tree hierarchy
    std::unique_ptr<BoxUniquePtrList> m_boxes_ptr{nullptr}; 
    std::vector<Node>                 m_tree_structure;     
    IndexList                         m_tree_boxes_map;     
    bool                              m_dumping_mode{true}; 
 
    // Tree parameters
    Integer m_max_nodal_objects{10};            
    Real    m_separation_ratio_tolerance{0.25}; 
    Real    m_balance_ratio_tolerance{0.3};     
 
    // Statistics
    mutable Integer m_check_counter{0}; 
    mutable Integer m_dump_counter{0};  
 
    // Stack for non-recursive tree building and query
    mutable IndexList m_stack; 
    mutable PriorityQueue m_queue; 
 
  public:
    ~Tree() = default;

    Tree() = default;

    BoxUniquePtrList const & boxes() const {return *m_boxes_ptr;}

    BoxUniquePtr const & box(Integer const i) const {return (*m_boxes_ptr)[i];}

    void max_nodal_objects(Integer const n) {
      constexpr char CMD[]{"AABBtree::Tree::max_nodal_objects(...): "};
      AABBTREE_ASSERT(n > 0, CMD << "input must be a positive integer.");
      m_max_nodal_objects = n;
    }

    Integer max_nodal_objects() const {return m_max_nodal_objects;}

    void separation_ratio_tolerance(Real const ratio) {
      constexpr char CMD[]{"AABBtree::Tree::separation_ratio_tolerance(...): " };
      AABBTREE_ASSERT(ratio > 0.0 && ratio < 1.0, CMD << "input must be in the range [0, 1].");
      m_separation_ratio_tolerance = ratio;
    }

    Real separation_ratio_tolerance() const {return m_separation_ratio_tolerance;}

    std::vector<Node> const & structure() const {return m_tree_structure;}

    Node const & node(Integer const i) const {return m_tree_structure[i];}

    Integer size() const {return static_cast<Integer>(m_tree_structure.size());}

    void enable_dumping_mode() {m_dumping_mode = true;}

    void disable_dumping_mode() {m_dumping_mode = false;}

    void dumping_mode(bool const mode) {m_dumping_mode = mode;}

    bool is_empty() const {return m_tree_structure.empty();}

    void clear() {
      m_tree_structure.clear();
      m_tree_boxes_map.clear();
      m_stack.clear();
    }

    void build(std::unique_ptr<BoxUniquePtrList> boxes_ptr) {
 
      // Collect the original object boxes
      m_boxes_ptr = std::move(boxes_ptr);
 
      BoxUniquePtrList & boxes{*m_boxes_ptr};
 
      // Clear tree structure
      Integer num_boxes{static_cast<Integer>(boxes.size())};
      m_tree_structure.clear();
      m_tree_structure.reserve(2*num_boxes + 1);
 
      // Setup the root node
      Node root;
      root.parent      = -1;
      root.child_l     = -1;
      root.child_r     = -1;
      root.box_ptr     = 0;
      root.box_num     = 0;
      root.box_tot_num = num_boxes;
 
      // Setup the boxes map and compute the root box
      Integer depth{std::ceil(std::log2(num_boxes))};
      root.box.set_empty();
      m_tree_boxes_map.reserve(2*depth);
      for (BoxUniquePtr const & box : boxes) {
        root.box.extend(*box);
        m_tree_boxes_map.emplace_back(root.box_num++);
      }
      m_tree_structure.emplace_back(root);
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(2*num_boxes + 1);
      m_stack.emplace_back(0);
 
      // Main loop that divide the nodes iteratively until all constraints satisfied
      IndexList m_map(m_tree_boxes_map.size());
      while (!m_stack.empty()) {
        // Pop the node from stack
        Integer const id{m_stack.back()}; m_stack.pop_back();
 
        // Get the node
        Node & node{m_tree_structure[id]};
 
        // If the node has less than the maximum number of objects, skip it
        if (node.box_num < m_max_nodal_objects) {continue;}
 
        // Compute the separation line and tolerance
        Vector  sizes{node.box.max() - node.box.min()};
        Integer sorting[N]; std::iota(sorting, sorting + N, 0);
        auto compare = [&sizes] (Integer i, Integer j) -> bool {return sizes[i] > sizes[j];};
        std::sort(sorting, sorting + N, compare);
 
        Integer axis, n_long, n_left, n_right, id_ini, id_end;
        Real    separation_line, separation_tolerance;
 
        Integer n_long_saved{node.box_num+1};
        Integer n_diff_saved{node.box_num+1};
        Integer axis_saved{sorting[0]};
 
        for (Integer dump{0}; dump < N; ++dump) {
          axis                 = sorting[dump];
          separation_line      = node.box.baricenter(axis);
          separation_tolerance = sizes[axis] * m_separation_ratio_tolerance;
 
          // Separate short and long boxes and compute short boxes baricenter
          n_long = n_left = n_right = 0;
          id_ini = node.box_ptr;
          id_end = node.box_ptr + node.box_num;
 
          Real baricenter{0.0};
          while (id_ini < id_end) {
            Box const & box_id{*boxes[m_tree_boxes_map[id_ini]]};
            typename Box::Side const side{box_id.which_side(separation_line, separation_tolerance, axis)};
            switch (side) {
              case Box::Side::LEFT: // Left boxes are moved to the end
                ++n_left;  --id_end; std::swap(m_tree_boxes_map[id_ini], m_tree_boxes_map[id_end]);
                break;
              case Box::Side::RIGHT: // Right boxes are moved to the end
                ++n_right; --id_end; std::swap(m_tree_boxes_map[id_ini], m_tree_boxes_map[id_end]);
                break;
              default:
                ++n_long; ++id_ini;
            }
            baricenter += box_id.max()[axis] + box_id.min()[axis];
          }
          baricenter /= 2.0*node.box_num;
 
          // Save the best solution found if it has a better n_long and n_diff
          Integer n_diff{std::abs(n_left - n_right)};
          if (n_long_saved > n_long || n_diff_saved > n_diff) {
            n_long_saved = n_long;
            n_diff_saved = n_diff;
            axis_saved   = axis;
            std::copy_n(m_tree_boxes_map.data()+node.box_ptr, node.box_num, m_map.data()+node.box_ptr);
          }
 
          // If the number of long boxes is too high, skip the next iterations
          if (n_long > m_max_nodal_objects) {continue;}
 
          // If the dumping mode is not enabled, end the loop
          if (!m_dumping_mode) {break;}
 
          // If the left and right children are yet not balanced, dump the splitting axis
          if (std::min(n_left, n_right) >= m_balance_ratio_tolerance * std::max(n_left, n_right)) {break;}
          ++m_dump_counter;
        }
 
        // Use the best solution found
        std::copy_n(m_map.data()+node.box_ptr, node.box_num, m_tree_boxes_map.data()+node.box_ptr);
        n_long               = n_long_saved;
        axis                 = axis_saved;
        separation_line      = node.box.baricenter(axis);
        separation_tolerance = sizes[axis] * m_separation_ratio_tolerance;
 
        // Separate the left and right boxes
        n_left = n_right = 0;
        id_ini = node.box_ptr + n_long;
        id_end = node.box_ptr + node.box_num;
        while (id_ini < id_end) {
          Box const & box_id{*boxes[m_tree_boxes_map[id_ini]]};
          typename Box::Side const side{box_id.which_side(separation_line, separation_tolerance, axis)};
          switch (side) {
          case Box::Side::LEFT: // Left boxes are moved to the end
            ++id_ini; ++n_left; // In right position do nothing
            break;
          case Box::Side::RIGHT: // Right boxes are moved to the end
            --id_end; ++n_right; // In right position swap the current box with the last one
            std::swap(m_tree_boxes_map[id_ini], m_tree_boxes_map[id_end]);
            break;
          default:
            break;
          }
        }
 
        // If the left and right children have few boxes, they are leafs
        if (n_left <= m_max_nodal_objects && n_right <= m_max_nodal_objects) {continue;}
 
        // Set the left and right children indexes
        node.child_l = static_cast<Integer>(this->size() + 0);
        node.child_r = static_cast<Integer>(this->size() + 1);
 
        // Finalize the root node setup (left and right children)
        Node node_l;
        node_l.parent      = id; // Current node
        node_l.child_l     = -1; // Default (leaf)
        node_l.child_r     = -1; // Default (leaf)
        node_l.box_num     = n_left;
        node_l.box_tot_num = n_left;
 
        Node node_r;
        node_r.parent      = id; // Current node
        node_r.child_l     = -1; // Default (leaf)
        node_r.child_r     = -1; // Default (leaf)
        node_r.box_num     = n_right;
        node_r.box_tot_num = n_right;
 
        // Compute the bounding box of the long boxes, and left and right children
        Integer j{node.box_ptr};
 
        node.box_num = n_long;
        node.box_long.set_empty();
        for (Integer i{0}; i < n_long; ++i) {node.box_long.extend(*boxes[m_tree_boxes_map[j++]]);}
 
        node_l.box.set_empty();
        node_l.box_ptr = j;
        for (Integer i{0}; i < n_left; ++i) {node_l.box.extend(*boxes[m_tree_boxes_map[j++]]);}
        node_l.box_long = node_l.box;
 
        node_r.box.set_empty();
        node_r.box_ptr = j;
        for (Integer i{0}; i < n_right; ++i) {node_r.box.extend(*boxes[m_tree_boxes_map[j++]]);}
        node_r.box_long = node_r.box;
 
        // Push nodes on tree structure
        m_tree_structure.emplace_back(node_l);
        m_tree_structure.emplace_back(node_r);
 
        // Push children on stack
        m_stack.emplace_back(node.child_l);
        m_stack.emplace_back(node.child_r);
      }
    }

    template <typename Object>
    bool intersect(Object const & obj, IndexSet & candidates) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return if the tree is empty
      if (this->is_empty()) {return false;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes{*m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(0);
 
      // Main loop that checks the intersection iteratively
      candidates.clear();
      while (!m_stack.empty()) {
 
        // Pop the node from stack
        Integer const id{m_stack.back()}; m_stack.pop_back();
 
        // Get the node
        Node const & node{m_tree_structure[id]};
 
        // If the object do not intersects the box, skip the node
        if (++m_check_counter; !node.box.intersects(obj)) {continue;}
 
        // Intersect the object with the long boxes on the node.
        // If it is a leaf long boxes are also the nodes of the leaf.
        if (node.box_num > 0) {
          if (++m_check_counter; node.box_long.intersects(obj)) {
            Integer const id_ini{node.box_ptr};
            Integer const id_end{node.box_ptr + node.box_num};
            for (Integer i{id_ini}; i < id_end; ++i) {
              Integer const pos{m_tree_boxes_map[i]};
              if (++m_check_counter; boxes[pos]->intersects(obj)) candidates.insert(pos);
            }
          }
        }
 
        // Push children on the stack if they are not leafs
        if (node.child_l > 0) {m_stack.emplace_back(node.child_l);}
        if (node.child_r > 0) {m_stack.emplace_back(node.child_r);}
      }
 
      // Return true if the object intersects the tree
      return !candidates.empty();
    }

    bool intersect(Tree const & tree, IndexMap & candidates) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return if the tree is empty
      if (this->is_empty() || tree.is_empty()) {return false;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes_1{*m_boxes_ptr};
      BoxUniquePtrList const & boxes_2{*tree.m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(this->size() + tree.size() + 2);
      m_stack.emplace_back(0);
      m_stack.emplace_back(0);
 
      // Negate the id of the node to distinguish the tree: if 0 --> -1, 1 --> -2, 2 --> -3
      auto negate = [] (Integer const id) {return -1-id;};
 
      // Main loop that checks the intersection iteratively
      candidates.clear();
      while (!m_stack.empty()) {
 
        // Pop the node from stack (reversed order)
        Integer const id_s2{m_stack.back()}; m_stack.pop_back();
        Integer const id_2{id_s2 < 0 ? negate(id_s2) : id_s2};
        Integer const id_s1{m_stack.back()}; m_stack.pop_back();
        Integer const id_1{id_s1 < 0 ? negate(id_s1) : id_s1};
 
        // Get the node
        Node const & node_1{m_tree_structure[id_1]};
        Node const & node_2{tree.m_tree_structure[id_2]};
 
        // If the boxes are not intersecting, skip the nodes
        if (++m_check_counter; !node_1.box.intersects(node_2.box)) {continue;}
 
        // Intersect the long boxes on the nodes: if both are leaf then intersect the corresponding boxes
        if (node_1.box_num > 0 && node_2.box_num > 0) {
          if (++m_check_counter; node_1.box_long.intersects(node_2.box_long)) {
            Integer const id_1_ini{node_1.box_ptr};
            Integer const id_1_end{node_1.box_ptr + node_1.box_num};
            Integer const id_2_ini{node_2.box_ptr};
            Integer const id_2_end{node_2.box_ptr + node_2.box_num};
            if (node_1.box_num < node_2.box_num) {
              for (Integer i{id_1_ini}; i < id_1_end; ++i) {
                Integer const pos_1{m_tree_boxes_map[i]};
                if (++m_check_counter; !boxes_1[pos_1]->intersects(node_2.box_long)) {continue;}
                for (Integer j{id_2_ini}; j < id_2_end; ++j) {
                  Integer const pos_2{tree.m_tree_boxes_map[j]};
                  if (++m_check_counter; boxes_1[pos_1]->intersects(*boxes_2[pos_2])) candidates[pos_1].insert(pos_2);
                }
              }
            } else {
              for (Integer j{id_2_ini}; j < id_2_end; ++j) {
                Integer const pos_2{tree.m_tree_boxes_map[j]};
                if (++m_check_counter; !boxes_2[pos_2]->intersects(node_1.box_long)) {continue;}
                for (Integer i{id_1_ini}; i < id_1_end; ++i) {
                  Integer const pos_1{m_tree_boxes_map[i]};
                    if (++m_check_counter; boxes_1[pos_1]->intersects(*boxes_2[pos_2])) candidates[pos_1].insert(pos_2);
                }
              }
            }
          }
        }
 
        // Check if the nodes are leafs
        bool const leaf_1{node_1.child_l < 0};
        bool const leaf_2{node_2.child_l < 0};
        if (leaf_1 && leaf_2) {continue;} // We are done on this branch
 
        // Push children of both trees on the stack if they are not leafs
        auto stack_emplace_back = [this](Integer const node_1, Integer const node_2) {
          m_stack.emplace_back(node_1); m_stack.emplace_back(node_2);
        };
 
        if (leaf_1) {
          stack_emplace_back(id_1, node_2.child_l);
          stack_emplace_back(id_1, node_2.child_r);
        } else if (leaf_2) {
          stack_emplace_back(node_1.child_l, id_2);
          stack_emplace_back(node_1.child_r, id_2);
        } else {
          if (node_1.box_tot_num > node_2.box_tot_num) { // split first larger tree
            if (id_s1 >= 0) {
              stack_emplace_back(node_1.child_l, id_s2);
              stack_emplace_back(node_1.child_r, id_s2);
              if (node_1.box_num > 0) stack_emplace_back(negate(id_1), id_2);
            } else if (id_s2 >= 0) { // and id_s1 < 0
              stack_emplace_back(id_s1, node_2.child_l);
              stack_emplace_back(id_s1, node_2.child_r);
            }
          } else {
            if (id_s2 >= 0) {
              stack_emplace_back(id_s1, node_2.child_l);
              stack_emplace_back(id_s1, node_2.child_r);
              if (node_2.box_num > 0) stack_emplace_back(id_1, negate(id_2));
            } else if (id_s1 >= 0) { // and id_s2 < 2
              stack_emplace_back(node_1.child_l, id_s2);
              stack_emplace_back(node_1.child_r, id_s2);
            }
          }
        }
      }
 
      // Return true if the trees intersect
      return !candidates.empty();
    }

    bool self_intersect(IndexSet & candidates) const {
      IndexMap candidates_map;
      bool intersects{this->intersect(*this, candidates_map)};
      candidates.clear();
      for (const auto & [key, values] : candidates_map) {
        for (int value : values) {candidates.emplace(key); candidates.emplace(value);}
      }
      return intersects;
    }

    template <typename Object>
    Real distance(Object const & obj, IndexSet & candidates) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return a negative value if the tree is empty
      if (this->is_empty()) {return -1.0;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes{*m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(0);
      m_stack.emplace_back(0);
 
      // Main loop that checks the intersection iteratively
      Real distance{INF};
      candidates.clear();
      while (!m_stack.empty())
      {
        // Pop the node from stack
        Integer const id{m_stack.back()}; m_stack.pop_back();
 
        // Get the node
        Node const & node {m_tree_structure[id]};
 
        // Compute the distance between the object and the bounding box
        ++m_check_counter;
        Real tmp_distance{node.box.interior_distance(obj)};
 
        // If the distance is greater than the temporary minimum distance, skip the node
        if (tmp_distance > distance) {continue;}
 
        // Compute the distance between the object and the long boxes on the node
        if (node.box_num > 0) {
          if (++m_check_counter; node.box_long.interior_distance(obj) <= distance) {
            Integer const id_ini{node.box_ptr};
            Integer const id_end{node.box_ptr + node.box_num};
            for (Integer i{id_ini}; i < id_end; ++i) {
              Integer const pos{m_tree_boxes_map[i]};
              ++m_check_counter;
              tmp_distance = boxes[pos]->interior_distance(obj);
              if (tmp_distance < distance) {
                candidates.clear(); candidates.insert(pos); distance = tmp_distance;
              } else if (tmp_distance == distance) {
                candidates.insert(pos);
              }
            }
          }
        }
 
        // Push children on the stack if thay are not leafs
        if (node.child_l > 0) {m_stack.emplace_back(node.child_l);}
        if (node.child_r > 0) {m_stack.emplace_back(node.child_r);}
      }
 
      // Return the distance between the point and the tree
      return distance;
    }

    Real distance(Tree const & tree, IndexMap & candidates) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return if the tree is empty
      if (this->is_empty() || tree.is_empty()) {return -1.0;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes_1{*m_boxes_ptr};
      BoxUniquePtrList const & boxes_2{*tree.m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(this->size() + tree.size() + 2);
      m_stack.emplace_back(0);
      m_stack.emplace_back(0);
 
      // Negate the id of the node to distinguish the tree: if 0 --> -1, 1 --> -2, 2 --> -3
      auto negate = [] (Integer const id) {return -1-id;};
 
      // Main loop that checks the intersection iteratively
      Real distance{INF};
      candidates.clear();
      while (!m_stack.empty())
      {
        // Pop the node from stack (reversed order)
        Integer const id_s2{m_stack.back()}; m_stack.pop_back();
        Integer const id_2{id_s2 >= 0 ? id_s2 : negate(id_s2)};
        Integer const id_s1{m_stack.back()}; m_stack.pop_back();
        Integer const id_1{id_s1 >= 0 ? id_s1 : negate(id_s1)};
 
        // Get the node
        Node const & node_1{m_tree_structure[id_1]};
        Node const & node_2{tree.m_tree_structure[id_2]};
 
        // Compute the distance between the bounding boxes
        ++m_check_counter;
        Real tmp_distance{node_1.box.interior_distance(node_2.box)};
 
        // If the distance is greater than the temporary minimum distance, skip the nodes
        if (tmp_distance > distance) {continue;}
 
        // Compute the distance between the long boxes on the nodes
        if (node_1.box_num > 0 && node_2.box_num > 0) {
          if (++m_check_counter; node_1.box_long.interior_distance(node_2.box_long) <= distance) {
            Integer const id_1_ini{node_1.box_ptr};
            Integer const id_1_end{node_1.box_ptr + node_1.box_num};
            Integer const id_2_ini{node_2.box_ptr};
            Integer const id_2_end{node_2.box_ptr + node_2.box_num};
            if (node_1.box_num < node_2.box_num) {
              for (Integer i{id_1_ini}; i < id_1_end; ++i) {
                Integer const pos_1{m_tree_boxes_map[i]};
                ++m_check_counter; tmp_distance = boxes_1[pos_1]->interior_distance(node_2.box_long);
                if (tmp_distance > distance) {continue;}
                for (Integer j{id_2_ini}; j < id_2_end; ++j) {
                  Integer const pos_2{tree.m_tree_boxes_map[j]};
                  ++m_check_counter; tmp_distance = boxes_1[pos_1]->interior_distance(*boxes_2[pos_2]);
                  if (tmp_distance < distance) {
                    candidates.clear(); candidates[pos_1].insert(pos_2); distance = tmp_distance;
                  } else if (tmp_distance == distance) {
                    candidates[pos_1].insert(pos_2);
                  }
                }
              }
            } else {
              for (Integer j{id_2_ini}; j < id_2_end; ++j) {
                Integer const pos_2{tree.m_tree_boxes_map[j]};
                ++m_check_counter; tmp_distance = boxes_2[pos_2]->interior_distance(node_1.box_long);
                if (tmp_distance > distance) {continue;}
                for (Integer i{id_1_ini}; i < id_1_end; ++i) {
                  Integer const pos_1{m_tree_boxes_map[i]};
                  ++m_check_counter; tmp_distance = boxes_1[pos_1]->interior_distance(*boxes_2[pos_2]);
                  if (tmp_distance < distance) {
                    candidates.clear(); candidates[pos_1].insert(pos_2); distance = tmp_distance;
                  } else if (tmp_distance == distance) {
                    candidates[pos_1].insert(pos_2);
                  }
                }
              }
            }
          }
        }
 
        // Check if the nodes are leafs
        bool const leaf_1{node_1.child_l < 0};
        bool const leaf_2{node_2.child_l < 0};
        if (leaf_1 && leaf_2) {continue;} // We are done on this branch
 
        // Push children of both trees on the stack if thay are not leafs
        auto stack_emplace_back = [this](Integer const node_1, Integer const node_2) {
          m_stack.emplace_back(node_1); m_stack.emplace_back(node_2);
        };
 
        if (leaf_1) {
          stack_emplace_back(id_1, node_2.child_l);
          stack_emplace_back(id_1, node_2.child_r);
        } else if (leaf_2) {
          stack_emplace_back(node_1.child_l, id_2);
          stack_emplace_back(node_1.child_r, id_2);
        } else {
          if (node_1.box_tot_num > node_2.box_tot_num) { // split first larger tree
            if (id_s1 >= 0) {
              stack_emplace_back(node_1.child_l, id_s2);
              stack_emplace_back(node_1.child_r, id_s2);
              if (node_1.box_num > 0) stack_emplace_back(negate(id_1), id_2);
            } else if (id_s2 >= 0) { // and id_s1 < 0
              stack_emplace_back(id_s1, node_2.child_l);
              stack_emplace_back(id_s1, node_2.child_r);
            }
          } else {
            if (id_s2 >= 0) {
              stack_emplace_back(id_s1, node_2.child_l);
              stack_emplace_back(id_s1, node_2.child_r);
              if (node_2.box_num > 0) stack_emplace_back(id_1, negate(id_2));
            } else if (id_s1 >= 0) { // and id_s2 < 2
              stack_emplace_back(node_1.child_l, id_s2);
              stack_emplace_back(node_1.child_r, id_s2);
            }
          }
        }
      }
 
      // Return the distance between the trees
      return distance;
    }

    template <typename Object, typename Function = std::function<Real(Object const &, Box const &)>>
    Real closest(Object const & obj, Integer const n, IndexSet & candidates, Function distance_function = []
      (Object const & o, Box const & b) {return b.interior_distance(o);}) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return if the tree is empty
      if (this->is_empty()) {return -1.0;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes{*m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(0);
 
      // Candidate vector distance and index
      using Pair = std::pair<Real, Integer>;
      auto cmp = [](const Pair & a, const Pair & b) {return a.first < b.first;};
      std::priority_queue<Pair, std::vector<Pair>, decltype(cmp)> queue(cmp);
 
      // Main loop that checks the intersection iteratively
      Real distance{INF}; // Maximum distance in the queue
      candidates.clear();
      while (!m_stack.empty())
      {
        // Pop the node from stack
        Integer const id{m_stack.back()}; m_stack.pop_back();
 
        // Get the node
        Node const & node{m_tree_structure[id]};
 
        // Compute the distance between the object and the bounding box
        ++m_check_counter;
        Real tmp_distance{distance_function(obj, node.box)};
 
        // If the distance is greater than the maximum distance, skip the node
        if (tmp_distance > distance) {continue;}
 
        // Compute the distance between the object and the long boxes on the node
        if (node.box_num > 0) {
          Integer const id_ini{node.box_ptr};
          Integer const id_end{node.box_ptr + node.box_num};
          for (Integer i{id_ini}; i < id_end; ++i) {
            Integer const pos{m_tree_boxes_map[i]};
            ++m_check_counter;
            tmp_distance = distance_function(obj, *boxes[pos]);
            if (tmp_distance < distance) {
              if (static_cast<Integer>(queue.size()) < n) {
                queue.emplace(tmp_distance, pos);
              } else {
                queue.pop(); queue.emplace(tmp_distance, pos);
              }
              distance = queue.top().first;
            }
          }
        }
 
        // Push children on the stack if thay are not leafs
        if (node.child_l > 0) {m_stack.emplace_back(node.child_l);}
        if (node.child_r > 0) {m_stack.emplace_back(node.child_r);}
      }
 
      // Extract indices into candidates
      Real min_distance{queue.empty() ? distance : queue.top().first};
      while (!queue.empty()) {candidates.insert(queue.top().second); queue.pop();}
      return min_distance;
    }

    template <typename Object, typename Function = std::function<Real(Object const &, Box const &)>>
    bool within_distance(Object const & obj, Real const max_distance, IndexSet & candidates,
      Function distance_function = [] (Object const & o, Box const & b) {return b.interior_distance(o);}
    ) const {
      // Reset statistics
      m_check_counter = 0;
 
      // Return if the tree is empty
      if (this->is_empty()) {return false;}
 
      // Collect the original object boxes
      BoxUniquePtrList const & boxes{*m_boxes_ptr};
 
      // Setup the stack
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(0);
 
      // Main loop that checks the intersection iteratively
      candidates.clear();
      while (!m_stack.empty())
      {
        // Pop the node from stack
        Integer const id{m_stack.back()}; m_stack.pop_back();
 
        // Get the node
        Node const & node{m_tree_structure[id]};
 
        // Compute the distance between the object and the bounding box
        ++m_check_counter;
        Real distance{distance_function(obj, node.box)};
 
        // If the distance is greater than the maximum distance, skip the node
        if (distance > max_distance) {continue;}
 
        // Compute the distance between the object and the long boxes on the node
        if (node.box_num > 0) {
          Integer const id_ini{node.box_ptr};
          Integer const id_end{node.box_ptr + node.box_num};
          for (Integer i{id_ini}; i < id_end; ++i) {
            Integer const pos{m_tree_boxes_map[i]};
            ++m_check_counter;
            distance = distance_function(obj, *boxes[pos]);
            if (distance <= max_distance) {candidates.insert(pos);}
          }
        }
 
        // Push children on the stack if thay are not leafs
        if (node.child_l > 0) {m_stack.emplace_back(node.child_l);}
        if (node.child_r > 0) {m_stack.emplace_back(node.child_r);}
      }
 
      // Return true if at least one candidate is within the given distance
      return !candidates.empty();
    }

    void depth(Integer const i, Integer & d) const {
      d = 0;
      if (i < 0) {return;}
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(i);
      std::vector<Integer> depth_stack;
      depth_stack.reserve(2*this->size() + 1);
      depth_stack.emplace_back(0);
      Integer depth{0};
      while (!m_stack.empty())
      {
        Integer const id{m_stack.back()}; m_stack.pop_back();
        Node const & node {m_tree_structure[id]};
        depth = static_cast<Integer>(depth_stack.back()); depth_stack.pop_back();
        if (node.child_l == -1) {d = std::max(d, depth);}
        else {m_stack.emplace_back(node.child_l); depth_stack.emplace_back(depth+1);}
        if (node.child_r == -1) {d = std::max(d, depth);}
        else {m_stack.emplace_back(node.child_r); depth_stack.emplace_back(depth+1);}
      }
    }

    void nodes(Integer const i, Integer & l, Integer & n, Integer & b) const {
      l = n = b = 0;
      if (i < 0) {return;}
      m_stack.clear();
      m_stack.reserve(2*this->size() + 1);
      m_stack.emplace_back(i);
      while (!m_stack.empty())
      {
        Integer const id{m_stack.back()}; m_stack.pop_back();
        Node const & node {m_tree_structure[id]};
        if (node.child_l == -1) {++l;} else {m_stack.emplace_back(node.child_l);}
        if (node.child_r == -1) {++l;} else {m_stack.emplace_back(node.child_r);}
        ++n; b += node.box_num;
      }
    }

    void stats(Statistics & stats) const {
      // Reset statistics
      stats.reset();
 
      // Compute/copy the build statistics
      stats.objects = static_cast<Integer>(m_boxes_ptr->size());
      this->nodes(0, stats.leafs, stats.nodes, stats.long_boxes);
      this->depth(0, stats.depth);
      this->nodes(m_tree_structure[0].child_l, stats.left_leafs, stats.left_nodes, stats.left_long_boxes);
      this->depth(m_tree_structure[0].child_l, stats.left_depth);
      this->nodes(m_tree_structure[0].child_r, stats.right_leafs, stats.right_nodes, stats.right_long_boxes);
      this->depth(m_tree_structure[0].child_r, stats.right_depth);
      stats.dump_counter  = m_dump_counter;
      stats.balance_ratio = static_cast<Real>(stats.left_leafs)/static_cast<Real>(stats.right_leafs);
      stats.depth_ratio   = static_cast<Real>(stats.left_depth)/static_cast<Real>(stats.right_depth);
 
      // Copy the check counter
      stats.check_counter = m_check_counter;
    }

    void print(OutStream & os) const  {
      // Retrieve the statistics
      Statistics stats; this->stats(stats);
      // Print the tree info
      stats.print(os);
    }
 
  }; // class Tree
 
} // namespace AABBtree
 
#endif // INCLUDE_AABBTREE_TREE_HXX