text: 
  #include "BoundingSphere.h"
  #include <cstring>
  
  #if defined(_OPENMP)
  #include "omp.h"
  #endif
  
  template<typename HullType> void
  KDTree<HullType>::construct()
  {
  	m_nodes.clear();
  	m_hulls.clear();
  	if (m_lst.empty()) return;
  
  	std::iota(m_lst.begin(), m_lst.end(), 0);
  
  	// Determine bounding box of considered domain.
  	auto box = AlignedBox3r{};
  	for (auto i = 0u; i < m_lst.size(); ++i)
  		box.extend(entity_position(i));
  
  	auto ni = add_node(0, static_cast<unsigned int>(m_lst.size()));
  	construct(ni, box, 0, static_cast<unsigned int>(m_lst.size()));
  }
  
  template<typename HullType> void
  KDTree<HullType>::construct(unsigned int node, AlignedBox3r const& box, unsigned int b,
  	unsigned int n)
  {
  	// If only one element is left end recursion.
  	//if (n == 1) return;
  	if (n < 10) return;
  
  	// Determine longest side of bounding box.
  	auto max_dir = 0;
  	auto d = box.diagonal().eval();
  	if (d(1) >= d(0) && d(1) >= d(2))
  		max_dir = 1;
  	else if (d(2) >= d(0) && d(2) >= d(1))
  		max_dir = 2;
  
  #ifdef _DEBUG
  	for (auto i = 0u; i < n; ++i)
  	{
  		if (!box.contains(entity_position(m_lst[b + i])))
  			std::cerr << "ERROR: Bounding box wrong!" << std::endl;
  	}
  #endif
  
  	// Sort range according to center of the longest side.
  	std::sort(m_lst.begin() + b, m_lst.begin() + b + n, 
  		[&](unsigned int a, unsigned int b)
  		{
  			return entity_position(a)(max_dir) < entity_position(b)(max_dir);
  		}
  	);
  
  	auto hal = n / 2;
  	auto n0 = add_node(b      , hal    );
  	auto n1 = add_node(b + hal, n - hal);
  	m_nodes[node].children[0] = n0;
  	m_nodes[node].children[1] = n1;
  
  	auto c = 0.5 * (
  		entity_position(m_lst[b + hal -1])(max_dir) + 
  		entity_position(m_lst[b + hal   ])(max_dir));
  	auto l_box = box; l_box.max()(max_dir) = c;
  	auto r_box = box; r_box.min()(max_dir) = c;
  
  	construct(m_nodes[node].children[0], l_box, b, hal);
  	construct(m_nodes[node].children[1], r_box, b + hal, n - hal);
  }
  
  template<typename HullType> void
  KDTree<HullType>::traverse_depth_first(TraversalPredicate pred, TraversalCallback cb,
  	TraversalPriorityLess const& pless) const
  {
  	if (m_nodes.empty())
  		return;
  
  	if (pred(0, 0)) 
  		traverse_depth_first(0, 0, pred, cb, pless);
  }
  
  template<typename HullType> void
  KDTree<HullType>::traverse_depth_first(unsigned int node_index, 
  	unsigned int depth, TraversalPredicate pred, TraversalCallback cb,
  	TraversalPriorityLess const& pless) const
  {
  	Node const& node = m_nodes[node_index];
  
  	cb(node_index, depth);
  	auto is_pred = pred(node_index, depth);
  	if (!node.is_leaf() && is_pred)
  	{
  		if (pless && !pless(node.children))
  		{
  			traverse_depth_first(m_nodes[node_index].children[1], depth + 1, pred, cb, pless);
  			traverse_depth_first(m_nodes[node_index].children[0], depth + 1, pred, cb, pless);
  		}
  		else
  		{
  			traverse_depth_first(m_nodes[node_index].children[0], depth + 1, pred, cb, pless);
  			traverse_depth_first(m_nodes[node_index].children[1], depth + 1, pred, cb, pless);
  		}
  	}
  }
  
  
  template <typename HullType> void
  KDTree<HullType>::traverse_breadth_first(TraversalPredicate const& pred, 
  	TraversalCallback const& cb, unsigned int start_node, TraversalPriorityLess const& pless,
  	TraversalQueue& pending) const
  {
  	cb(start_node, 0);
  	if (pred(start_node, 0)) pending.push({ start_node, 0 });
  	traverse_breadth_first(pending, pred, cb, pless);
  }
  
  
  template <typename HullType> void
  KDTree<HullType>::traverse_breadth_first_parallel(TraversalPredicate pred,
  	TraversalCallback cb) const
  {
  	auto start_nodes = std::vector<QueueItem>{};
  #if defined(_DEBUG) || !defined(_OPENMP)
  	const unsigned int maxThreads = 1;
  #else
  	const unsigned int maxThreads = omp_get_max_threads();
  #endif
  
  	// compute ceiling of Log2
  	// assuming double and long long have the same size.
  	double d = maxThreads - 1;
  	long long ll;  memcpy( &ll, &d, sizeof(d));
  	const unsigned int targetDepth = (ll >> 52) - 1022ll;
  	
  	traverse_breadth_first(
  		[&start_nodes, &maxThreads, &targetDepth](unsigned int node_index, unsigned int depth)
  		{			
  			return (depth < targetDepth) && (start_nodes.size() < maxThreads);
  		},
  		[&](unsigned int node_index, unsigned int depth)
  		{			
  			if ((depth == targetDepth) || (node(node_index).is_leaf()))
  				start_nodes.push_back({ node_index, depth });
  		}
  		);
  
  	#pragma omp parallel default(shared)
  	{
  		#pragma omp for schedule(static) 
  		for (int i = 0; i < start_nodes.size(); i++)
  		{
  			QueueItem const& qi = start_nodes[i];
  			cb(qi.n, qi.d);
  			traverse_depth_first(qi.n, qi.d, pred, cb, nullptr);
  		}
  	}
  }
  
  template <typename HullType> unsigned int
  KDTree<HullType>::add_node(unsigned int b, unsigned int n)
  {
  	HullType hull;
  	compute_hull(b, n, hull);
  	m_hulls.push_back(hull);
  	m_nodes.push_back({ b, n });
  	return static_cast<unsigned int>(m_nodes.size() - 1);
  }
  
  
  
  template <typename HullType>
  void 
  KDTree<HullType>::traverse_breadth_first(TraversalQueue& pending, 
  	TraversalPredicate const& pred, TraversalCallback const& cb, TraversalPriorityLess const& pless) const
  {
  	while (!pending.empty())
  	{
  		auto n = pending.front().n;
  		auto d = pending.front().d;
  		auto const& node = m_nodes[n];
  		pending.pop();
  
  		cb(n, d);
  		auto is_pred = pred(n, d);
  		if (!node.is_leaf() && is_pred)
  		{
  			if (pless && !pless(node.children))
  			{
  				pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
  				pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
  			}
  			else
  			{
  				pending.push({ static_cast<unsigned int>(node.children[0]), d + 1 });
  				pending.push({ static_cast<unsigned int>(node.children[1]), d + 1 });
  			}
  		}
  	}
  }
  
  template <typename HullType> void
  KDTree<HullType>::update()
  {
  	traverse_depth_first(
  		[&](unsigned int, unsigned int) { return true; },
  		[&](unsigned int node_index, unsigned int)
  	{
  		auto const& nd = node(node_index);
  		compute_hull_approx(nd.begin, nd.n, m_hulls[node_index]);
  	}
  	);
  }
  