KDTree.hpp

Go to the documentation of this file.

/**--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants 
Licensee a limited, non-exclusive, non-transferable, royalty-free license 
(without the right to sublicense) to use the API solely for the purpose of 
Licensee's internal development efforts to develop applications for which 
the API was provided.
 
The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Tree
File: KDTree
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include "TreeIterator.h>
#include "Node.h>
#include "Tree.h>
#include "Base\Headers\BaseValues.h>
#include "Base\Headers\Buffer.h>
#include "Base\Headers\Bounds.h>
#include "Base\Headers\Median.h>
#include "Base\Headers\ProgressInfo.h>
#include "Base\Headers\BoolBuffer.h>
#include "Base\Headers\Vector.h>
#include "Base\Headers\VectorFunctions.h>
#include "Base\Headers\intersection.h>
#include "Base\Headers\BinaryHeap.h>
 
 
namespace NDEVR
{
    template<uint01 t_dims, class t_type>
    class KDNode : public TreeNode
    {
    public:
        KDNode(const KDNode& node)
            : TreeNode(node)
            , m_split(0)
            , m_split_dim(0)
        {}
        explicit KDNode(uint04 index_start)
            : TreeNode(index_start)
            , m_split(0)
            , m_split_dim(0)
        {}
 
        void clear(uint04 index_start, const Bounds<t_dims, t_type>& bounds)
        {
            setBegin(index_start);
            //TreeNode::template m_cache_bounds = bounds;
            m_element_size = 0;
        }
 
        uint04 left() const
        {
            return uint04(m_child_start);
        }
        uint04 right() const
        {
            return uint04(m_child_start) + 1;
        }
 
        const t_type& split() const
        {
            return m_split;
        }
        const uint01& splitDim() const
        {
            return m_split_dim;
        }
        void setSplit(uint01 split_dim, t_type split)
        {
            m_split = split;
            m_split_dim = split_dim;
        }
    protected:
        t_type m_split;
        uint01 m_split_dim;
    };
 
 
 
    template<uint01 t_dims, class t_type>
    class KDTreeBase : public TreeBase<KDNode<t_dims, t_type>, t_type, 2>
    {
    public:
        explicit KDTreeBase<t_dims, t_type>(uint04 bucket_size)
            : TreeBase(bucket_size)
        {}
        KDTreeBase<t_dims, t_type>(const KDTreeBase<t_dims, t_type>& tree)
            : TreeBase(tree)
        {}
        KDTreeBase<t_dims, t_type>(KDTreeBase<t_dims, t_type>&& tree)
            : TreeBase(std::move(tree))
        {}
    protected:
        KDTreeBase<t_dims, t_type>(const Buffer<KDNode<t_dims, t_type>>& nodes, const Buffer<uint04>& indices, bool is_read_only)
            : TreeBase(nodes, indices, is_read_only)
        {}
    public:
        template<class t_node_type>
        bool validate(const Buffer<t_node_type>& elements)
        {
            TreeIterator iterator(root_node);
 
            Buffer<bool> values;
            values.setSize(elements.size());
 
            while (iterator.valid())
            {
 
                uint04 node_index = iterator.get();
                KDNode<t_dims, t_type>& node = m_nodes[node_index];
                iterator.pop();
 
                values.setAllToValue(false);
                _getAllT(node_index, values);
                if (node.size() != values.numSet())
                    return false;
            }
            return true;
        }
        
    protected:
        template<class t_node_type, class t_buffer_type>
        uint04 _calculateAndSplit(uint04 node_index, Bounds<t_dims, t_type> node_bounds, const t_buffer_type& elements, Buffer<uint04>& indices, uint04 start, uint04 end, bool is_precise)
        {
            Vector<t_dims, uint04> location;
            Vector<t_dims, t_type> d_size;//The difference between the right and left size if a given node is chosen
            for (uint01 dim = 0; dim < t_dims; ++dim)
            {
                if (is_precise)
                    location[dim] = median(elements, indices, start, end, dim);
                else
                    location[dim] = apxMedian(elements, indices, start, end, dim);
                Bounds<t_dims, t_type> bounds(elements[indices[location[dim]]]);
                d_size[dim] = getMin(
                      abs(bounds.center()[dim] - node_bounds[MIN][dim])
                    , abs(node_bounds[MAX][dim] - bounds.center()[dim]));
            }
            const uint01 split_dim = d_size.getDimension<MAX>();//maximize our smallest split dimension
            if (is_precise)
                location[split_dim] = median(elements, indices, start, end, split_dim);
            
            const uint04 new_left = sortAboutValue<t_type>(location[split_dim], elements, indices, start, end, split_dim);
 
            splitLeafNode(node_index);
            m_nodes[node_index].setSplit(split_dim, elements[indices[new_left]][split_dim]);
 
            return new_left;
        }
 
 
        template<class t_node_type, class t_buffer_type>
        bool _balanceLeafNode(uint04 top_index, const t_buffer_type& elements, Buffer<uint04>& indices, uint04 top_start, uint04 top_end, bool is_precise, ProgressInfo* progress = nullptr)
        {
            uint04 start_stack[256];
            uint04 end_stack[256];
            Bounds<t_dims, t_type> node_bounds[256];
            start_stack[0] = top_start;
            end_stack[0] = top_end;
            node_bounds[0] = _getBounds<t_dims>(indices, elements, top_start, top_end);
            TreeIterator iterator(top_index);
            uint04 size = 0;
            const uint04 update_size = getMax(uint04(1000), (top_end - top_start) / 10000);
 
            while (iterator.valid())
            {
                uint01 level = iterator.depth();
                uint04 start = start_stack[level];
                uint04 end = end_stack[level];
                uint04 node_index = iterator.get();
                Bounds<t_dims, t_type> bounds = node_bounds[level];
 
                iterator.pop();
                m_nodes[node_index].setSize(end - start);
 
                if ((end - start) > m_bucket_size)
                {
                    const uint04 new_left = _calculateAndSplit(node_index, bounds, elements, indices, start, end, is_precise);
 
                    iterator.addAndGoToIndex(m_nodes[node_index].right());
                    start_stack[iterator.depth()] = new_left;
                    end_stack[iterator.depth()] = end;
                    node_bounds[iterator.depth()] = bounds;
                    node_bounds[iterator.depth()][MIN][m_nodes[node_index].splitDim()] = m_nodes[node_index].split();
 
                    iterator.addAndGoToIndex(m_nodes[node_index].left());
                    start_stack[iterator.depth()] = start;
                    end_stack[iterator.depth()] = new_left;
                    node_bounds[iterator.depth()] = bounds;
                    node_bounds[iterator.depth()][MAX][m_nodes[node_index].splitDim()] = m_nodes[node_index].split();
                }
                else
                {
                    m_indices.setAll(indices.begin(start), m_nodes[node_index].begin(), (end - start));
                    size += (end - start);
 
                    if (progress != nullptr && node_index % update_size == 0)
                    {
                        if (progress->setProgress(cast<fltp04>(size) / elements.size()))
                        {
                            clear();
                            return false;
                        }
                    }
                }
            }
            if (progress != nullptr)
            {
                if (progress->setProgress(1.0f))
                {
                    clear();
                    return false;
                }
            }
            return true;
        }
        
 
        template<class t_node_type, class t_buffer_type>
        bool _getClosestElement(uint04 top_index, const Vector<t_dims, t_type>& point, const t_buffer_type& elements, t_type& min_distance, const t_type epsilon, uint04& min_index) const
        {
            TreeIterator iterator(top_index);
            t_type split_values[256];
            while (iterator.valid())
            {
                const uint04 index = iterator.get();
                if (split_values[iterator.depth()] >= min_distance)
                {
                    iterator.pop();
                    continue;
                }
                iterator.pop();
                
                const KDNode<t_dims, t_type>& node = m_nodes[index];
                
                if (node.isLeaf())
                {
                    const uint04 end = node.end();
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        const uint04 check_index = m_indices[i];
                        t_type distance = distanceSquared(point, elements[check_index]);
                        if (distance < min_distance)
                        {
                            min_distance = distance;
                            min_index = check_index;
                            if (distance <= epsilon)
                                return true;
                        }
                    }
                }
                else
                {
                    const t_type distance = point[node.splitDim()] - node.split();
                    if (distance > 0)
                    {
                        iterator.addAndGoToIndex(node.left());
                        split_values[iterator.depth()] = distance * distance;
 
                        iterator.addAndGoToIndex(node.right());
                        split_values[iterator.depth()] = 0;
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.right());
                        split_values[iterator.depth()] = distance * distance;
 
                        iterator.addAndGoToIndex(node.left());
                        split_values[iterator.depth()] = 0;
                    }
                }
            }
            return false;
        }
 
        template<class t_node_type, class t_buffer_type>
        void _getClosestElements(uint04 top_index, const Vector<t_dims, t_type>& point, const t_buffer_type& elements, t_type& min_distance, const t_type& epsilon, MinHeap<t_type, uint04>& heap, uint04 size) const
        {
            TreeIterator iterator(top_index);
            t_type split_values[255];
            while (iterator.valid())
            {
                const uint04 index = iterator.get();
                if (split_values[iterator.depth()] >= min_distance)
                {
                    iterator.pop();
                    continue;
                }
                iterator.pop();
                
                const KDNode<m_bucket_size, t_dims, t_type>& node = m_nodes[index];
                if (node.isLeaf())
                {
                    const uint04 end = node.end();
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        uint04 check_index = m_indices[i];
                        t_type distance = distanceSquared(point, elements[check_index]);
                        if (distance < min_distance)
                        {
                            heap.insert(distance, check_index);
                            if (heap.size() == size + 1)
                            {
                                heap.deleteMax();
                                min_distance = heap.max();
                            }
                        }
                    }
                    if (min_distance <= epsilon)
                        return;
                }
                else
                {
                    const t_type distance = point[node.splitDim()] - node.split();
                    if (distance > 0)
                    {
                        iterator.addAndGoToIndex(node.left());
                        split_values[iterator.depth()] = distance * distance;
 
 
                        iterator.addAndGoToIndex(node.right());
                        split_values[iterator.depth()] = 0;
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.right());
                        split_values[iterator.depth()] = distance * distance;
 
 
                        iterator.addAndGoToIndex(node.left());
                        split_values[iterator.depth()] = 0;
                    }
                }
            }
        }
 
        template<class t_node_type, class t_buffer_type>
        bool _getClosestElement(uint04 top_index, const LineSegment<t_dims, t_type>& top_line, const t_buffer_type& elements, t_type& min_distance, t_type epsilon, uint04& min_index) const
        {
            TreeIterator iterator(top_index);
            t_type split_values[256];
 
            Vector<3, uint01> dim_min;
            Vector<3, uint01> dim_max;
            for (uint01 i = 0; i < t_dims; i++)
            {
                dim_min[i] = top_line[0][i] < top_line[1][i] ? MIN : MAX;
                dim_max[i] = dim_min[i] != MIN ? MIN : MAX;
            }
 
            split_values[0] = 0;
            t_type sqrt_min_dist = sqrt(min_index);
            CachedLineSegment<t_dims, t_type> cached_line(top_line);
 
            while (iterator.valid())
            {
                const uint04 index = iterator.get();
                if (abs(split_values[iterator.depth()]) >= min_distance)
                {
                    iterator.pop();
                    continue;
                }
                //const LineSegment<t_dims, t_type> line = lines[iterator.depth()];
 
                iterator.pop();
                const KDNode<t_dims, t_type>& node = m_nodes[index];
                
                if (node.isLeaf())
                {
                    const uint04 end = node.end();
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        uint04 check_index = m_indices[i];
                        t_type distance = distanceSquared(cached_line, elements[check_index]);
                        if (distance < min_distance)
                        {
                            min_distance = distance;
                            min_index = check_index;
                            sqrt_min_dist = sqrt(min_index);
                        }
                    }
                    if (distance <= epsilon)
                        return true;
                }
                else
                {
                    const uint01 split_dim = m_nodes[index].splitDim();
                    const uint01 min = dim_min[split_dim];
                    const uint01 max = dim_max[split_dim];
                    if (cached_line[max][split_dim] >= m_nodes[index].split())
                    {
                        if (cached_line[min][split_dim] <= m_nodes[index].split())
                        {
                            iterator.addAndGoToIndex(node.right());
                            split_values[iterator.depth()] = 0;
                            
                            iterator.addAndGoToIndex(node.left());
                            split_values[iterator.depth()] = 0;
                        }
                        else
                        {
                            iterator.addAndGoToIndex(node.left());
                            split_values[iterator.depth()] = cached_line[min][split_dim] - m_nodes[index].split();
                            split_values[iterator.depth()] *= split_values[iterator.depth()];
 
                            iterator.addAndGoToIndex(node.right());
                            split_values[iterator.depth()] = 0;
                        }
                    }
                    else if (cached_line[min][split_dim] <= m_nodes[index].split())
                    {
                        iterator.addAndGoToIndex(node.right());
                        split_values[iterator.depth()] = m_nodes[index].split() - cached_line[max][split_dim];
                        split_values[iterator.depth()] *= -split_values[iterator.depth()];
 
                        iterator.addAndGoToIndex(node.left());
                        split_values[iterator.depth()] = 0;
                    }
                }
            }
            return false;
        }
 
        template<class t_node_type, class t_buffer_type>
        bool _getClosestElement(uint04 top_index, const Triangle<t_dims, t_type>& triangle, const t_buffer_type& elements, t_type& min_distance, t_type epsilon, uint04& min_index) const
        {
            TreeIterator iterator(top_index);
 
            CachedTriangle<t_dims, t_type> cached_tri(triangle);
 
            while (iterator.valid())
            {
                const uint04 index = iterator.get();
                iterator.pop();
                const KDNode<m_bucket_size, t_dims, t_type>& node = m_nodes[index];
                if (!node.bounds().expand(sqrt(min_distance)).doBoundsIntersect<t_dims>(cached_tri))
                    continue;
                if (node.isLeaf())
                {
                    const uint04 end = node.end();
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        const uint04 check_index = m_indices[i];
                        const t_type distance = distanceSquared(cached_tri, elements[check_index]);
                        if (distance < min_distance)
                        {
                            min_distance = distance;
                            min_index = check_index;
                        }
                    }
                    if (distance <= epsilon)
                        return true;
                }
                else
                {
                    if (m_nodes[node.left()].bounds().contains(cached_tri.centroid()))
                        iterator.addAndGoToIndex(node.right(), node.left());
                    else
                        iterator.addAndGoToIndex(node.left(), node.right());
                }
            }
            return false;
        }
 
        template<class t_node_type, class t_area_type, class t_buffer_type>
        void _getEnclosedElements(uint04 top_node, t_area_type area, Buffer<bool>& indices, const t_buffer_type& elements) const
        {
            Bounds<t_dims, t_type> bounds[256];
            bounds[0] = Constant<Bounds<t_dims, t_type>>::Max;
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                uint04 index = iterator.get();
                const KDNode<t_dims, t_type>& node = m_nodes[index];
                const Bounds<t_dims, t_type> box = bounds[iterator.depth()];
                iterator.pop();
                switch (classify(area, box))
                {
                case IntersectionTypes::inside:
                    _getAll<false>(index, indices);
                    break;
                case IntersectionTypes::outside:
                    break;
                case IntersectionTypes::mixed:
                    if (node.isLeaf())
                    {
                        const uint04 end = node.begin() + node.size();
                        for (uint04 i = node.begin(); i < end; i++)
                        {
                            if (classify(area, elements[m_indices[i]]) != outside)
                            {
                                indices.set(m_indices[i], true);
                            }
                        }
                    }
                    else
                    {
                        Bounds<t_dims, t_type> left = box;
                        left[MAX][node.splitDim()] = node.split();
                        Bounds<t_dims, t_type> right = box;
 
                        Bounds<t_dims, t_type> left = box;
                        right[MIN][node.splitDim()] = node.split();
 
                        iterator.addAndGoToIndex(node.left());
                        bounds[iterator.depth()] = left;
 
                        iterator.addAndGoToIndex(node.right());
                        bounds[iterator.depth()] = right;
                    }
                    break;
                }
            }
        }
 
        template<class t_area_type, class t_node_type, class t_buffer_type>
        uint04 _getNumberOfEnclosedElements(uint04 top_node, const t_area_type& area, const t_buffer_type& elements) const
        {
            TreeIterator iterator(top_node);
            uint04 size = 0;
            while (iterator.valid())
            {
                uint04 index = iterator.get();
                const KDNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                switch (classify(area, node.bounds()))
                {
                case IntersectionTypes::inside:
                    size += node.size();
                    break;
                case IntersectionTypes::outside:
                    break;
                case IntersectionTypes::mixed:
                    if (node.isLeaf())
                    {
                        const uint04 end = node.begin() + node.size();
                        for (uint04 i = node.begin(); i < end; i++)
                        {
                            if (classify(area, elements[m_indices[i]]) != outside)
                            {
                                ++size;
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                    break;
                }
            }
            return size;
        }
 
        
        template<class t_node_type, class t_buffer_type>
        void _removeValue(uint04 top_node, uint04 index, const t_buffer_type& elements)
        {
            bool recalculate_bounds = false;
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                const uint04 node_index = iterator.get();
                KDNode<t_dims, t_type>& node = m_nodes[node_index];
                if (node.isLeaf())
                {
                    uint04 end = node.end();
                    Bounds<t_dims, t_type> bounds(Constant<Bounds<t_dims, t_type>>::Min);
                    uint04 index_location = Constant<uint04>::NaN;
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        uint04 check_index = m_indices[i];
                        if (index != check_index)
                            bounds.addToBounds(elements[check_index]);
                        else
                            index_location = i;
                    }
                    node.setSize(node.size() - 1);
                    if (node.size() != 0)
                        m_indices.swapIndices(index_location, node.end());
                    if (classify(node.bounds(), bounds) == inside)
                        recalculate_bounds = true;
                    node.setBounds(bounds);
                    iterator.pop();
                    break;
                }
                else
                {
 
                    bool needs_rebalance = false;
                    if (classify(m_nodes[node.left()].bounds(), elements[index]) == inside)
                    {
                        if (needsRebalance(m_nodes[node.left()].size() - 1, m_nodes[node.right()].size()))
                            needs_rebalance = true;
                        else
                            iterator.addAndGoToIndex(node.left());
                    }
                    else
                    {
                        if (needsRebalance(m_nodes[node.left()].size(), m_nodes[node.right()].size() - 1))
                            needs_rebalance = true;
                        else
                            iterator.addAndGoToIndex(node.right());
                    }
                    if (needs_rebalance || node.size() - 1 == m_bucket_size)
                    {
                        Buffer<uint04> indices(node.size());
                        _getAll(node_index, indices);
                        node.setSize(node.size() - 1);
                        indices.removeElement(index);
                        reclaimChildren(node_index);
                        _balanceLeafNode(node_index, elements, indices, 0, indices.size(), false);
                        recalculate_bounds = true;
                        break;
                    }
                    else
                    {
                        node.setSize(node.size() - 1);
                    }
                }
            }
            if (recalculate_bounds)
            {
                while (iterator.valid())
                {
                    if (!recalculateBounds(iterator.get(), elements))
                        return;
                    iterator.pop();
                }
            }
        }
 
 
        template<class t_node_type, class t_buffer_type>
        void _moveValue(uint04 top_node, uint04 index, const t_node_type& new_location, const t_buffer_type& elements)
        {
            TreeIterator iterator(top_node);
            bool recalculate_bounds = false;
            while (iterator.valid())
            {
                const uint04 node_index = iterator.get();
                KDNode<t_dims, t_type>& node = m_nodes[node_index];
                if (node.isLeaf())
                {
                    if (classify(m_nodes[node.left()].bounds(), new_location) == inside)
                        return;
                    uint04 end = node.end();
                    Bounds<t_dims, t_type> bounds(Constant<Bounds<t_dims, t_type>>::Min);
                    uint04 index_location = Constant<uint04>::NaN;
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        uint04 check_index = m_indices[i];
                        if (index != check_index)
                            bounds.addToBounds(elements[check_index]);
                        else
                            index_location = i;
                    }
                    node.setSize(node.size() - 1);
                    if (node.size() != 0)
                        m_indices.swapIndices(index_location, node.end());
                    recalculate_bounds = (classify(node.bounds(), bounds) != inside);
                    node.setBounds(bounds);
                    iterator.pop();
                    break;
                }
                else
                {
 
                    bool needs_rebalance = false;
                    if (classify(m_nodes[node.left()].bounds(), elements[index]) == inside)
                    {
                        if (needsRebalance(m_nodes[node.left()].size() - 1, m_nodes[node.right()].size()))
                            needs_rebalance = true;
                        else
                            iterator.addAndGoToIndex(node.left());
                    }
                    else
                    {
                        if (needsRebalance(m_nodes[node.left()].size(), m_nodes[node.right()].size() - 1))
                            needs_rebalance = true;
                        else
                            iterator.addAndGoToIndex(node.right());
                    }
                    if (needs_rebalance || node.size() - 1 == m_bucket_size)
                    {
                        Buffer<uint04> indices(node.size());
                        _getAll(node_index, indices);
                        node.setSize(node.size() - 1);
                        indices.removeElement(index);
                        reclaimChildren(node_index);
                        _balanceLeafNode(node_index, elements, indices, 0, indices.size());
                        recalculate_bounds = true;
                        break;
                    }
                    else
                    {
                        node.setSize(node.size() - 1);
                    }
                }
            }
            if (recalculate_bounds)
            {
                while (iterator.valid())
                {
                    if (!recalculateBounds(iterator.get(), elements))
                        return;
                    iterator.pop();
                }
            }
        }
 
        bool needsRebalance(uint04 left_size, uint04 right_size)
        {
            const uint04 total_size = left_size + right_size;
            return (left_size < total_size / 4 || right_size < total_size / 4);
        }
 
        bool useBulkResize(uint04 change_size)
        {
            return (change_size > size() / 4);
        }
 
        template<class t_node_type, class t_buffer_type>
        void _addValues(uint04 top_node, Buffer<uint04> indices, const t_buffer_type& elements, bool is_precise, ProgressInfo* progress = nullptr)
        {
            _getAll(top_node, indices);
            reclaimChildren(top_node);
 
            uint04 max_size = 3 * indices.size();
            m_indices.ensureCapacity(max_size + (m_indices.size() - getNumberOfFreeIndices()));
            m_nodes.ensureCapacity(2 * max_size / m_bucket_size + (m_nodes.size() - getNumberOfNodes()));
 
            //m_nodes[top_node].setBounds(_getBounds(indices, elements, 0, indices.size()));
            bool did_balance = _balanceLeafNode(top_node, elements, indices, 0, indices.size(), is_precise, progress);
        }
 
 
        template<class t_node_type, class t_buffer_type>
        void _addValue(uint04 node_index, uint04 index, const t_buffer_type& elements)
        {
            /*Bounds<t_dims, t_type> bounds(elements[index]);
            for (;;)
            {
 
                KDNode<t_dims, t_type>& node = m_nodes[node_index];
                node.bounds().addToBoundingBox(bounds);
                if (node.isLeaf())
                {
                    if (node.size() == m_bucket_size)
                    {
                        Buffer<uint04> indices(m_bucket_size + 1);
                        _getAll(node_index, indices);
                        indices.add(index);
                        _balanceLeafNode(node_index, elements, indices, 0, m_bucket_size + 1, false);
                    }
                    else
                    {
                        m_indices[node.begin() + node.size()] = index;
                        node.setSize(node.size() + 1);
                    }
                    return;
                }
                else
                {
                    Bounds<t_dims, t_type> left = m_nodes[node.left()].bounds();
                    Bounds<t_dims, t_type> right = m_nodes[node.right()].bounds();
 
                    left.addToBoundingBox(bounds);
                    right.addToBoundingBox(bounds);
                    bool needs_rebalance = false;
                    if (left.volume() - m_nodes[node.left()].bounds().volume() < right.volume() - m_nodes[node.right()].bounds().volume())
                    {
                        if (needsRebalance(m_nodes[node.left()].size() + 1, m_nodes[node.right()].size()))
                            needs_rebalance = true;
                        else
                            node_index = node.left();
                    }
                    else
                    {
                        if (needsRebalance(m_nodes[node.left()].size(), m_nodes[node.right()].size() + 1))
                            needs_rebalance = true;
                        else
                            node_index = node.right();
                    }
                    if (needs_rebalance)
                    {
                        Buffer<uint04> indices(node.size() + 1);
                        _getAll(node_index, indices);
                        indices.add(index);
                        reclaimChildren(node_index);
                        _balanceLeafNode(node_index, elements, indices, 0, indices.size(), false);
                        return;
                    }
                    else
                    {
                        node.setSize(node.size() + 1);
                    }
                }
            }*/
        }
 
        template<class t_node_type, class t_buffer_type>
        bool recalculateBounds(uint04 node_index, const t_buffer_type& elements)
        {
            Bounds<t_dims, t_type> bounds(Constant<Bounds<t_dims, t_type>>::Min);
            KDNode<t_dims, t_type>& node = m_nodes[node_index];
            if (node.isLeaf())
            {
                uint04 end = node.end();
                for (uint04 i = node.begin(); i < end; i++)
                {
                    bounds.addToBounds(elements[m_indices[i]]);
                }
            }
            else
            {
                bounds = Bounds<t_dims, t_type>(m_nodes[node.left()].bounds(), m_nodes[node.right()].bounds());
            }
            if (classify(node.bounds(), bounds) == inside)
                return false;
            node.setBounds(bounds);
            return true;
        }
 
        void _makeWriteable()
        {
            if (!m_is_read_only)
                return;
            Buffer<uint04> indices(size());
 
            TreeIterator iterator(root_node);
            while (iterator.valid())
            {
                KDNode<m_bucket_size, t_dims, t_type>& node = m_nodes[iterator.get()];
                iterator.pop();
                if (node.isLeaf())
                {
                    node.setBegin(iterator.size());
                    indices.addAll(m_indices.begin(node.begin()), node.size());
                }
                else
                    iterator.addAndGoToIndex(node.left(), node.right());
            }
            m_indices = indices;
        }
    };
 
 
    template<uint01 t_dims, class t_type>
    class KDTree : public Tree<t_dims, t_type, KDTreeBase<t_dims, t_type>>
    {
    public:
        explicit KDTree<t_dims, t_type>(uint04 bucket_size)
            : Tree(bucket_size)
        {}
        KDTree<t_dims, t_type>(const KDTreeBase<t_dims, t_type>& tree)
            : Tree(tree)
        {}
        KDTree<t_dims, t_type>(KDTreeBase<t_dims, t_type>&& tree)
            : Tree(std::move(tree))
        {}
    protected:
        KDTree<t_dims, t_type>(const Buffer<KDNode<t_dims, t_type>>& nodes, const Buffer<uint04>& indices, bool is_read_only)
            : TreeBase(nodes, indices, is_read_only)
        {}
    public:
        inline KDTree& operator=(const KDTree& value)
        {
            m_indices = value.m_indices;//setting equal to ourselves
            m_nodes = value.m_nodes;
            m_available_node_positions = value.m_available_node_positions;
            m_available_indexed_positions = value.m_available_indexed_positions;
            return (*this);
        }
        inline KDTree& operator=(KDTree&& value)
        {
            std::swap(m_indices, value.m_indices);//setting equal to ourselves
            std::swap(m_nodes, value.m_nodes);
            std::swap(m_available_node_positions, value.m_available_node_positions);
            std::swap(m_available_indexed_positions, value.m_available_indexed_positions);
            return (*this);
        }
 
        virtual const char* getTreeType() const override
        {
            return "KD Tree";
        }
    };
}