Tree.hxx

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2026, 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                                 *
 * davide.stocco@unitn.it                         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.");

  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};      
    Integer node_check_counter{0}; 
    Integer long_check_counter{0}; 
    Integer objs_check_counter{0}; 

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

    void print(OutStream &os) const {
      // Print the tree info
      os << "AABB TREE INFO" << std::endl
         << "\tAmbient dimension : " << N << std::endl
         << "\tBasic type        : " << typeid(Real).name() << std::endl
         << "\tObjects           : " << objects << std::endl
         << "\tNodes             : " << nodes << std::endl
         << "\tLeafs             : " << leafs << std::endl
         << "\tLong boxes        : " << long_boxes << std::endl
         << "\tDepth             : " << depth << std::endl
         << "\tLeft nodes        : " << left_nodes << std::endl
         << "\tLeft leafs        : " << left_leafs << std::endl
         << "\tLeft long boxes   : " << left_long_boxes << std::endl
         << "\tLeft depth        : " << left_depth << std::endl
         << "\tRight nodes       : " << right_nodes << std::endl
         << "\tRight leafs       : " << right_leafs << std::endl
         << "\tRight long boxes  : " << right_long_boxes << std::endl
         << "\tRight depth       : " << right_depth << std::endl
         << "\tDump counter      : " << dump_counter << std::endl
         << "\tBalance ratio     : " << balance_ratio << std::endl
         << "\tDepth ratio       : " << depth_ratio << std::endl
         << "\tCheck counter     : " << check_counter << std::endl;
    }
  };
 
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; 
 
  // Tree parameters
  Integer m_max_dumpings{N};       
  Integer m_max_nodal_objects{50}; 
  Real m_separation_ratio_tolerance{0.1}; 
 
  // Statistics
  mutable Integer m_node_check_counter{0}; 
  mutable Integer m_long_check_counter{0}; 
  mutable Integer m_objs_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_dumpings(Integer const max_dumpings) {
    constexpr char CMD[]{"AABBtree::Tree::max_dumpings(...): "};
    AABBTREE_ASSERT(max_dumpings > 0 && max_dumpings <= N,
                    CMD << "input must be a positive integer in the range [1, "
                        << N << "].");
    m_max_dumpings = max_dumpings;
  }

  Integer max_dumpings() const { return m_max_dumpings; }

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

  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()); }

  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) {
    this->clear();
    m_boxes_ptr = std::move(boxes_ptr);
    this->build();
  }

  void build() {
 
    // Check if the boxes pointer is valid
    AABBTREE_ASSERT(m_boxes_ptr != nullptr,
                    "AABBtree::Tree::build(...): The boxes pointer is "
                    "not valid. Please provide a valid pointer to the "
                    "bounding boxes before building the tree.");
 
    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{static_cast<Integer>(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 one object, skip it
      if (node.box_num <= 1) {
        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]};
      bool valid_axis_found{false};
      for (Integer dump{0}; dump < m_max_dumpings; ++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 first and the best solution found so far
        Integer n_diff{std::abs(n_left - n_right)};
        if ((n_long <= n_long_saved && n_long <= m_max_nodal_objects &&
             n_diff < n_diff_saved) ||
            !valid_axis_found) {
          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);
          valid_axis_found = true;
        }
 
        ++m_dump_counter;
      }
 
      // Check if a valid solution has been found
      AABBTREE_ASSERT_WARNING(
          valid_axis_found,
          "AABBtree::Tree::build(...): Failed to find a valid "
          "splitting axis.");
 
      // Terminate splitting if the number of long boxes if cannot be split
      if (n_long_saved == node.box_num) {
        continue;
      }
 
      // 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;
        }
      }
 
      // 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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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_node_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_long_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_objs_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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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};
 
    // using Tuple = std::tuple<Integer, Integer, Integer>; // (box_num,
    // id_s1, id_s2) auto cmp = [](const Tuple & a, const Tuple & b) {return
    // std::get<0>(a) > std::get<0>(b);}; std::priority_queue<Tuple,
    // std::vector<Tuple>, decltype(cmp)> queue(cmp);
    //  Setup the stack
    m_stack.clear();
    m_stack.reserve(this->size() + tree.size() + 2);
 
    // 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; };
 
    // Compute the priority of the nodes based on the number of boxes
    auto stack_emplace_back = [this](Integer const node_1,
                                     Integer const node_2) {
      // Integer const id_1{node_1 < 0 ? negate(node_1) : node_1};
      // Integer const id_2{node_2 < 0 ? negate(node_2) : node_2};
      // queue.emplace(
      //   std::abs(m_tree_structure[id_1].box_tot_num -
      //   tree.m_tree_structure[id_2].box_tot_num)*
      //   std::min(m_tree_structure[id_1].box_num,
      //   tree.m_tree_structure[id_2].box_num), node_1, node_2);
      m_stack.emplace_back(node_1);
      m_stack.emplace_back(node_2);
    };
    stack_emplace_back(0, 0);
 
    // Main loop that checks the intersection iteratively
    candidates.clear();
    while (!m_stack.empty()) {
      // while (!queue.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};
      // Tuple top = queue.top(); queue.pop();
      // Integer const id_s1{std::get<1>(top)};
      // Integer const id_1{id_s1 < 0 ? negate(id_s1) : id_s1};
      // Integer const id_s2{std::get<2>(top)};
      // Integer const id_2{id_s2 < 0 ? negate(id_s2) : id_s2};
 
      // 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_node_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_long_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_objs_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_objs_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_objs_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_objs_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
 
      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(IndexMap &candidates) const {
    // Reset statistics
    m_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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};
 
    // using Tuple = std::tuple<Integer, Integer, Integer>; // (box_num,
    // id_s1, id_s2)
    m_stack.clear();
    m_stack.reserve(this->size() + this->size() + 2);
 
    // 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; };
 
    // Compute the priority of the nodes based on the number of boxes
    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);
    };
    stack_emplace_back(0, 0);
 
    // 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{m_tree_structure[id_2]};
 
      // If the boxes are not intersecting, skip the nodes
      if (++m_node_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_long_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_objs_check_counter;
                  !boxes[pos_1]->intersects(node_2.box_long)) {
                continue;
              }
              for (Integer j{id_2_ini}; j < id_2_end; ++j) {
                Integer const pos_2{m_tree_boxes_map[j]};
                if (pos_1 == pos_2) {
                  continue;
                } // Do not intersect with itself
                if (++m_objs_check_counter;
                    boxes[pos_1]->intersects(*boxes[pos_2]))
                  candidates[pos_1].insert(pos_2);
              }
            }
          } else {
            for (Integer j{id_2_ini}; j < id_2_end; ++j) {
              Integer const pos_2{m_tree_boxes_map[j]};
              if (++m_objs_check_counter;
                  !boxes[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 (pos_1 == pos_2) {
                  continue;
                } // Do not intersect with itself
                if (++m_objs_check_counter;
                    boxes[pos_1]->intersects(*boxes[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
 
      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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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{std::numeric_limits<Real>::infinity()};
    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_node_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_node_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_node_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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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);
 
    // 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; };
 
    // Setup the stack emplace function
    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);
    };
    stack_emplace_back(0, 0);
 
    // Main loop that checks the intersection iteratively
    Real distance{std::numeric_limits<Real>::infinity()};
    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_node_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_node_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_node_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_node_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_node_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_node_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
      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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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{
        std::numeric_limits<Real>::infinity()}; // 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_node_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_node_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_node_check_counter = 0;
    m_long_check_counter = 0;
    m_objs_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_node_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_node_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 {
        b += node.box_num;
        m_stack.emplace_back(node.child_l);
      }
      if (node.child_r == -1) {
        ++l;
      } else {
        b += node.box_num;
        m_stack.emplace_back(node.child_r);
      }
      ++n;
    }
  }

  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_node_check_counter + m_long_check_counter + m_objs_check_counter;
    stats.node_check_counter = m_node_check_counter;
    stats.long_check_counter = m_long_check_counter;
    stats.objs_check_counter = m_objs_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