RTree.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: RTree
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/TreeIterator.h>
#include <NDEVR/Node.h>
#include <NDEVR/Tree.h>
#include <NDEVR/TreeSorter.h>
#include <NDEVR/BaseValues.h>
#include <NDEVR/Buffer.h>
#include <NDEVR/Bounds.h>
#include <NDEVR/Median.h>
#include <NDEVR/OpenMP.h>
#include <NDEVR/ProgressInfo.h>
#include <NDEVR/Vector.h>
#include <NDEVR/VectorFunctions.h>
#include <NDEVR/Intersection.h>
#include <NDEVR/Distance.h>
#include <NDEVR/BinaryHeap.h>
#include <NDEVR/BinaryFile.h>
#include <functional>
namespace NDEVR
{
    template<uint01 t_dims, class t_type>
    class RNode : public TreeNode
    {
    public:
        RNode()
            : TreeNode(Constant<uint04>::Invalid)
            , m_cache_bounds(Constant<Bounds<t_dims, t_type>>::Invalid)
        {}
        explicit RNode(uint04 index_start)
            : TreeNode(index_start)
            , m_cache_bounds(Constant<Bounds<t_dims, t_type>>::Min)
        {}
        void clear(uint04 index_start)
        {
            setBegin(index_start);
            m_cache_bounds = Constant<Bounds<t_dims, t_type>>::Invalid;
            m_element_size = 0;
        }
        void clear(uint04 index_start, const Bounds<t_dims, t_type>& bounds)
        {
            setBegin(index_start);
            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 Bounds<t_dims, t_type>& bounds() const
        {
            return m_cache_bounds;
        }
        Bounds<t_dims, t_type>& bounds()
        {
            return m_cache_bounds;
        }
        void setBounds(const Bounds<t_dims, t_type>& bounds)
        {
            m_cache_bounds = bounds;
        }
 
    protected:
        Bounds<t_dims, t_type> m_cache_bounds;
    };
    
 
 
    template<uint01 t_dims, class t_type>
    class RTreeBase : public TreeBase<RNode<t_dims, t_type>, t_type, 2>
    {
    public:
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_nodes;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::root_node;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_indices;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_bucket_size;
        explicit RTreeBase<t_dims, t_type>(uint04 bucket_size)
            : TreeBase<RNode<t_dims, t_type>, t_type, 2>(bucket_size)
        {}
        RTreeBase<t_dims, t_type>(const RTreeBase<t_dims, t_type>& tree)
            : TreeBase<RNode<t_dims, t_type>, t_type, 2>(tree)
        {}
        RTreeBase<t_dims, t_type>(RTreeBase<t_dims, t_type>&& tree)
            : TreeBase<RNode<t_dims, t_type>, t_type, 2>(std::move(tree))
        {}
    protected:
        RTreeBase<t_dims, t_type>(const Buffer<RNode<t_dims, t_type>>& nodes, const Buffer<uint04>& indices, bool is_read_only)
            : TreeBase<RNode<t_dims, t_type>, t_type, 2>(nodes, indices, is_read_only)
        {}
    public:
        template<class t_buffer_type>
        bool validate(const t_buffer_type& elements)
        {
            TreeIterator iterator(root_node);
 
            Buffer<uint04> values;
            values.setSize(elements.size());
            if (elements.size() == 0)
                return true;
            uint04 i = 0;
            while (iterator.valid())
            {
                if (i++ > m_nodes.size())
                {
                    lib_assert(false, "invalid tree");
                    return false;
                }
                const uint04 node_index = iterator.get();
                RNode<t_dims, t_type>& node = m_nodes[node_index];
                iterator.pop();
 
                values.clear();
                TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(node_index, values);
                if (node.size() != values.size())
                {
                    lib_assert(false, "invalid tree");
                    return false;
                }
                if (!IsInvalid(node.bounds()))
                {
                    if (node.size() != 0 && !node.bounds().validate())
                    {
                        lib_assert(false, "invalid tree");
                        return false;
                    }
                }
                if (node.isLeaf())
                {
                    const uint04 end = node.begin() + node.size();
                    for (uint04 idx = node.begin(); idx < end; idx++)
                    {
                        const auto& element = elements[m_indices[idx]];
                        if (!IsInvalid(element) && classify(node.bounds(), element.template as<t_dims, t_type>()) != inside)
                        {
                            lib_assert(false, "invalid tree");
                            return false;
                        }
                    }
                }
                else
                {
                    if (node.size() != m_nodes[node.left()].size() + m_nodes[node.right()].size())
                    {
                        lib_assert(false, "invalid tree");
                        return false;
                    }
                    if(!IsInvalid(node.bounds()) && !IsInvalid(m_nodes[node.left()].bounds()))
                        if (classify(node.bounds(), m_nodes[node.left()].bounds()) != inside)
                        {
                            lib_assert(false, "invalid tree");
                            return false;
                        }
                    if (!IsInvalid(node.bounds()) && !IsInvalid(m_nodes[node.right()].bounds()))
                        if (classify(node.bounds(), m_nodes[node.right()].bounds()) != inside)
                        {
                            lib_assert(false, "invalid tree");
                            return false;
                        }
 
                    iterator.addAndGoToIndex(node.left(), node.right());
                }
            }
            return true;
        }
        void traverse(const std::function<void(uint04 node_index, const Buffer<RNode<t_dims, t_type>, uint04, ObjectAllocator<true>>& node, const Buffer<uint04>& indices)>& callback) const
        {
            TreeIterator iterator(root_node);
            while (iterator.valid())
            {
                const uint04 node_index = iterator.get();
                const RNode<t_dims, t_type>& node = m_nodes[node_index];
                iterator.pop();
                callback(node_index, m_nodes, m_indices);
                if (!node.isLeaf())
                {
                    iterator.addAndGoToIndex(node.left(), node.right());
                }
            }
        }
    protected:
        
        template<class t_buffer_type>
        uint04 _calculateAndSplit(uint04 node_index, TreeBoundarySorter<t_dims, t_type>& sorter, const t_buffer_type& elements, uint04 start, uint04 end, bool is_precise)
        {
            UNUSED(is_precise);
            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 (m_nodes[node_index].bounds().span()[dim] != 0)
                {
                    Vector<2, Bounds<t_dims, t_type>> potential_bounds = sorter.getMedianBounds(dim, start, end, elements);
                    d_size[dim] = potential_bounds[0].surfaceArea() + potential_bounds[1].surfaceArea();
                }
 
            }
            const uint01 split_dim = d_size.template dimensionalIndex<MIN>();//maximize our smallest split dimension
 
            sorter.getMedian(split_dim, start, end);
            const uint04 median_location = ((end - start) / 2) + start;
 
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::splitLeafNode(node_index);
            auto left = sorter.getBounds(start, median_location, elements);
            auto right = sorter.getBounds(median_location, end, elements);
#ifdef _DEBUG
            if (!IsInvalid(m_nodes[node_index].bounds()) && !IsInvalid(left))
                if (classify(m_nodes[node_index].bounds(), left) != inside)
                    lib_assert(false, "bad bounds creation");
            if (!IsInvalid(m_nodes[node_index].bounds()) && !IsInvalid(right))
                if (classify(m_nodes[node_index].bounds(), right) != inside)
                    lib_assert(false, "bad bounds creation");
#endif
            m_nodes[m_nodes[node_index].left()].setBounds(left);
            m_nodes[m_nodes[node_index].right()].setBounds(right);
 
            return median_location;
        }
        template<bool t_has_nans, bool t_uses_boundary, 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)
        {
            TreeBoundarySorter<t_dims, t_type> sorter;
            if constexpr (t_uses_boundary)
            {
                sorter.template createBoundarySortList<t_has_nans>(indices, elements, progress);
            }
            else
            {
                sorter.template createCenterSortList<t_has_nans>(indices, elements, progress);
            }
            m_nodes[top_index].setBounds(sorter.getBounds(0, (top_end - top_start), elements));
            uint04 start_stack[256];
            uint04 end_stack[256];
            start_stack[0] = top_start;
            end_stack[0] = top_end;
            TreeIterator iterator(top_index);
            while (iterator.valid())
            {
                uint01 level = iterator.depth();
                uint04 start = start_stack[level];
                uint04 end = end_stack[level];
                uint04 node_index = iterator.get();
                iterator.pop();
                m_nodes[node_index].setSize(end - start);
 
                if ((end - start) > m_bucket_size)
                {
                    const uint04 new_left = _calculateAndSplit(node_index, sorter, elements, start, end, is_precise);
 
                    iterator.addAndGoToIndex(m_nodes[node_index].right());
                    start_stack[iterator.depth()] = new_left;
                    end_stack[iterator.depth()] = end;
 
                    iterator.addAndGoToIndex(m_nodes[node_index].left());
                    start_stack[iterator.depth()] = start;
                    end_stack[iterator.depth()] = new_left;
                }
                else
                {
                    sorter.getList(0, start, end, m_indices, m_nodes[node_index].begin());
                }
            }
            return true;
        }
        template<class t_location_type>
        bool predictBranch(uint04& index, const RNode<t_dims, t_type>& node, const t_location_type& selector, MaxHeap<t_type, uint04>& node_heap, t_type min_distance_squared) const
        {
            t_type left_distance = distanceSquared(selector, m_nodes[node.left()].bounds());
            if(left_distance <= t_type(0))
                left_distance = -(cast<t_type>(1) / distanceSquared(selector, m_nodes[node.left()].bounds().center()));
            t_type right_distance = distanceSquared(selector, m_nodes[node.right()].bounds());
            if (right_distance <= t_type(0))
                right_distance = -(cast<t_type>(1) / distanceSquared(selector, m_nodes[node.right()].bounds().center()));
            if (left_distance < right_distance)
            {
                if (left_distance >= min_distance_squared)
                {
                    if (node_heap.size() == 0 || node_heap.extremeComp() >= min_distance_squared)
                        return false;
                    index = node_heap.popExtreme();
                }
                else if(node_heap.size() > 0 && node_heap.extremeComp() < left_distance)
                {
                    index = node_heap.popExtreme();
                    node_heap.insert(left_distance, node.left());
                    if (right_distance < min_distance_squared)
                        node_heap.insert(right_distance, node.right());
                }
                else
                {
                    index = node.left();
                    if (right_distance < min_distance_squared)
                        node_heap.insert(right_distance, node.right());
                }
            }
            else
            {
                if (right_distance >= min_distance_squared)
                {
                    if (node_heap.size() == 0 || node_heap.extremeComp() >= min_distance_squared)
                        return false;
                    index = node_heap.popExtreme();
                }
                else if (node_heap.size() > 0 && node_heap.extremeComp() < right_distance)
                {
                    index = node_heap.popExtreme();
                    node_heap.insert(right_distance, node.right());
                    if (left_distance < min_distance_squared)
                        node_heap.insert(left_distance, node.right());
                }
                else
                {
                    index = node.right();
                    if (left_distance < min_distance_squared)
                        node_heap.insert(left_distance, node.left());
                }
            }
            return true;
        }
        template<bool t_presorted, class t_location_type, class t_buffer_type>
        void _getClosestElement(uint04 top_index, const t_location_type& selector, const t_buffer_type& elements, t_type& min_distance_squared, const t_type& epsilon, uint04& min_index) const
        {
            t_type root_distance = distanceSquared(selector, m_nodes[top_index].bounds());
            lib_assert(min_distance_squared >= 0.0f, "Cannot search for 0.0 distance. Use epsilon");
            if (root_distance >= min_distance_squared)
                return;
            MaxHeap<t_type, uint04> node_heap(cast<uint04>(sqrt(cast<t_type>(m_nodes.size()))));
            uint04 index = top_index;
            for(;;)
            {
                const RNode<t_dims, t_type>& node = m_nodes[index];
                if (node.isLeaf())
                {
                    const uint04 end = node.end();
                    const uint04 start_index = node.begin();
                    for(uint04 i = start_index; i < end; ++i)
                    {
                        uint04 check_index;
                        if constexpr(t_presorted)
                            check_index = i;
                        else
                            check_index = m_indices[i];
                        t_type distance = distanceSquared(selector, elements[check_index].template as<t_dims, t_type>());
                        if (distance < min_distance_squared)
                        {
                            min_distance_squared = distance;
                            min_index = check_index;
                            if (min_distance_squared <= epsilon)
                                return;
                        }
                    }
                    if (node_heap.size() == 0 || node_heap.extremeComp() >= min_distance_squared)
                        return;
                    index = node_heap.popExtreme();
                }
                else
                {
                    if (!predictBranch(index, node, selector, node_heap, min_distance_squared))
                        return;
                }
            }
        }
 
        template<bool t_presorted, class t_buffer_type, class t_reference_type>
        void _getClosestElements(uint04 top_index, const t_reference_type& selector, const t_buffer_type& elements, t_type& min_distance_squared, const t_type& epsilon, MinHeap<t_type, uint04>& value_heap, uint04 size) const
        {
            MaxHeap<t_type, uint04> node_heap(cast<uint04>(sqrt(cast<t_type>(m_nodes.size()))));
            t_type root_distance = distanceSquared(selector, m_nodes[top_index].bounds());
            if (root_distance >= min_distance_squared)
                return;
            uint04 index = top_index;
            for(;;)
            {
                const RNode<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;
                        if constexpr (t_presorted)
                            check_index = i;
                        else
                            check_index = m_indices[i];
                        t_type distance = distanceSquared(selector, elements[check_index]);
                        if (distance < min_distance_squared)
                        {
                            value_heap.insert(distance, check_index);
                            if (value_heap.size() > size)
                            {
                                value_heap.deleteExtreme();
                                min_distance_squared = value_heap.extremeComp();
                                if (min_distance_squared <= epsilon)
                                    return;
                            }
                        }
                    }
                    if (node_heap.size() == 0 || node_heap.extremeComp() >= min_distance_squared)
                        return;
                    index = node_heap.popExtreme();
                }
                else
                {
                    if (!predictBranch(index, node, selector, node_heap, min_distance_squared))
                        return;
                }
            }
        }
        template<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
        {
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                uint04 index = iterator.get();
                const RNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                switch (classify(area, node.bounds()))
                {
                case IntersectionTypes::inside:
                    TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAllT<true>(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[m_indices[i]] = true;
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                    break;
                }
            }
        }
 
        template<bool t_is_presorted, class t_area_type, class t_buffer_type>
        void _getEnclosedElements(uint04 top_node, t_area_type area, Buffer<uint04>& indices, const t_buffer_type& elements) const
        {
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                uint04 index = iterator.get();
                const RNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                switch (classify(area, node.bounds()))
                {
                case IntersectionTypes::inside:
                    TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<t_is_presorted>(index, indices);
                    break;
                case IntersectionTypes::mixed:
                    if (node.isLeaf())
                    {
                        const uint04 end = node.begin() + node.size();
                        for (uint04 i = node.begin(); i < end; ++i)
                        {
                            uint04 local_index;
                            if constexpr (t_is_presorted)
                                local_index = i;
                            else
                                local_index = m_indices[i];
                            if (classify(area, elements[local_index]) != outside)
                            {
                                indices.add(local_index);
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                    break;
                case IntersectionTypes::outside:
                    break;
                }
            }
        }
 
        template<class t_area_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 RNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                switch (TreeBase<RNode<t_dims, t_type>, t_type, 2>::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 (int i = node.begin(); i < end; i++)
                        {
                            if (TreeBase<RNode<t_dims, t_type>, t_type, 2>::classify(area, elements[m_indices[i]]) != outside)
                            {
                                ++size;
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                    break;
                }
            }
            return size;
        }
 
        template<class t_area_type, class t_buffer_type>
        void _getNearElements(uint04 top_node, t_type max_distance, const t_area_type& area, Buffer<bool>& indices, const t_buffer_type& elements) const
        {
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                const uint04 index = iterator.get();
                const RNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                Bounds<t_dims, t_type> bounds = node.bounds();
                bounds.expand(max_distance);
                switch (classify(bounds, area))
                {
#if __clang__
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wunused-value"//It makes no sense this error is being reported?
#endif
                case IntersectionTypes::inside:
                    TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(index, indices);
                    break;
#if __clang__
    #pragma clang diagnostic pop
#endif
                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 (distanceSqaured(area, elements[m_indices[i]]) < max_distance)
                            {
                                indices[m_indices[i]] = true;
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                    break;
                }
            }
        }
        template<class t_area_type, class t_buffer_type>
        void _getChangedElements(uint04 top_node, const t_area_type& new_area, const t_area_type& old_area, Buffer<bool>& indices, const t_buffer_type& elements, bool allow_enable, bool allow_disable) const
        {
            TreeIterator iterator(top_node);
            while (iterator.valid())
            {
                bool is_mixed = false;
                uint04 index = iterator.get();
                const RNode<t_dims, t_type>& node = m_nodes[index];
                iterator.pop();
                switch (classify(new_area, node.bounds()))
                {
                case IntersectionTypes::inside:
                    switch (classify(old_area, node.bounds()))
                    {
                    case IntersectionTypes::inside:
                        break;
                    case IntersectionTypes::outside:
                        if(allow_enable)
                            TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAllT<true>(index, indices);
                        break;
                    case IntersectionTypes::mixed:
                        is_mixed = true;
                        break;
                    }
                    break;
                case IntersectionTypes::outside:
                    switch (classify(old_area, node.bounds()))
                    {
                    case IntersectionTypes::inside:
                        if(allow_disable)
                            TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAllT<false>(index, indices);
                        break;
                    case IntersectionTypes::outside:
                        break;
                    case IntersectionTypes::mixed:
                        is_mixed = true;
                        break;
                    }
                    break;
                case IntersectionTypes::mixed:
                    is_mixed = true;
                    break;
                }
                if (is_mixed)
                {
                    if (node.isLeaf())
                    {
                        const uint04 end = node.begin() + node.size();
                        for (uint04 i = node.begin(); i < end; i++)
                        {
                            if (allow_disable && indices[m_indices[i]])
                            {
                                if(classify(new_area, elements[m_indices[i]]) == outside)
                                    indices[m_indices[i]] = false;
                            }
                            else if (allow_enable && !indices[m_indices[i]])
                            {
                                if(classify(new_area, elements[m_indices[i]]) != outside)
                                    indices[m_indices[i]] = true;
                            }
                        }
                    }
                    else
                    {
                        iterator.addAndGoToIndex(node.left(), node.right());
                    }
                }
            }
        }
 
        template<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();
                RNode<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>::Invalid;
                    for (uint04 i = node.begin(); i < end; i++)
                    {
                        uint04 check_index = m_indices[i];
                        if (index != check_index)
                            bounds.addToBounds(elements[check_index].template as<t_dims, t_type>());
                        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());
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(node_index, indices);
                        node.setSize(node.size() - 1);
                        indices.removeElement(index);
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::reclaimChildren(node_index);
                        //TreeBase<RNode<t_dims, t_type>, t_type, 2>::_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& old_location, const t_buffer_type& elements, bool rebalance)
        {
            TreeIterator iterator(top_node);
            bool recalculate_bounds = false;
            while (iterator.valid())
            {
                const uint04 node_index = iterator.get();
                RNode<t_dims, t_type>& node = m_nodes[node_index];
                if (node.isLeaf())
                {
                    //If the bounds of the node don't change, no need to do anything
                    if (classify(m_nodes[node.left()].bounds(), elements[index]) == inside)
                        return;
                    uint04 end = node.end();
                    //Recalculate Node Bounds
                    Bounds<t_dims, t_type> bounds(Constant<Bounds<t_dims, t_type>>::Min);
                    uint04 index_location = Constant<uint04>::Invalid;
                    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;
                    }
                    //Check to see if bounding box is smaller
                    switch (classify(node.bounds(), bounds))
                    {
                    case inside:
                        recalculate_bounds = true;//Node boundary made smaller
                        node.setBounds(bounds);
                        break;
                    case mixed:
                    case outside:
                        lib_assert(node.bounds() == bounds, "Unexpected Node Resize behavior: Removing object should not increase bounds");
                        recalculate_bounds = false;
                        break;
                    }
 
                    //reset Node Size
                    node.setSize(node.size() - 1);
                    if (node.size() != 0)
                        m_indices.swapIndices(index_location, node.end());
 
                    iterator.pop();
                    break;//Break to begin resetting bounds
                }
                else
                {
                    if (classify(m_nodes[node.left()].bounds(), old_location) == inside)
                    {
                        iterator.addAndGoToIndex(node.left());
                    }
                    else
                    {
                        lib_assert(classify(m_nodes[node.right()].bounds(), old_location) == inside, "Original Element not in either bounds");
                        iterator.addAndGoToIndex(node.right());
                    }
                }
            }
            bool added_in = false;
            while (iterator.valid() && (!added_in || recalculate_bounds))
            {
                const uint04 node_index = iterator.get();
                if (recalculate_bounds)
                {
                    recalculate_bounds = recalculateBounds(node_index, elements);
                }
                if (!added_in)
                {
                    RNode<t_dims, t_type>& node = m_nodes[node_index];
                    if (classify(node.bounds(), elements[index]) == inside)
                    {
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::_addValue(node_index, index, elements, rebalance);
                        added_in = true;
                    }
                }
                iterator.pop();
            }
            if (!added_in)
            {
                TreeBase<RNode<t_dims, t_type>, t_type, 2>::_addValue(top_node, index, elements, rebalance);
            }
        }
 
        bool needsRebalance(uint04 left_size, uint04 right_size)
        {
            const uint04 total_size = left_size + right_size;
            return (left_size < total_size / 3 || right_size < total_size / 3);
        }
 
        bool useBulkResize(uint04 change_size)
        {
            return (change_size > TreeBase<RNode<t_dims, t_type>, t_type, 2>::size() / 4);
        }
        template<bool t_has_nans, bool t_uses_boundary, class t_buffer_type>
        void _addValues(uint04 top_node, Buffer<uint04> indices, const t_buffer_type& elements, bool is_precise, ProgressInfo* progress = nullptr)
        {
            if (progress) progress->addMessage("RTree: Getting all");
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(top_node, indices);
            if (progress) progress->addMessage("RTree: reclaiming children");
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::clear();
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::reclaimChildren(top_node);
 
            uint04 max_node_size = 1;
            uint04 max_index_size = m_bucket_size;
            uint04 load_size = indices.size();
            uint04 level = 1;
            while (load_size >= m_bucket_size)
            {
                load_size = cast<uint04>(ceil(load_size / 2));
                max_index_size = 2 * max_index_size;
                max_node_size = cast<uint04>(pow(2, level++)) + max_node_size;
            }
            m_indices.ensureCapacity(max_index_size);
            m_nodes.ensureCapacity(max_node_size);
            if (progress) progress->addMessage("RTree: Balancing Node");
            _balanceLeafNode<t_has_nans, t_uses_boundary>(top_node, elements, indices, 0, indices.size(), is_precise, progress);
            if (progress) progress->setProgress(1.0f);
            if (progress) progress->addMessage("RTree: Finished Adding values");
        }
        
 
        template<bool t_has_nans, bool t_uses_boundary, class t_buffer_type>
        void _addValue(uint04 node_index, uint04 index, const t_buffer_type& elements, bool balance)
        {
            Bounds<t_dims, t_type> bounds = m_nodes[node_index].bounds();
            const auto value = elements[index].template as<t_dims, t_type>();
            bounds.addToBounds(value);
            for (;;)
            {
                RNode<t_dims, t_type>& node = m_nodes[node_index];
                node.setBounds(bounds);
                if (node.isLeaf())
                {
                    if (node.size() == m_bucket_size)
                    {
                        Buffer<uint04> indices(m_bucket_size + 1);
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(node_index, indices);
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::reclaimChildren(node_index);
                        indices.add(index);
                        _balanceLeafNode<t_has_nans, t_uses_boundary>(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.addToBounds(value);
                    right.addToBounds(value);
                    bool needs_rebalance = false;
                    t_type surface_diff = 
                          (left.surfaceArea()  - m_nodes[node.left()].bounds().surfaceArea())
                        - (right.surfaceArea() - m_nodes[node.right()].bounds().surfaceArea());
                    if(surface_diff == 0)//Volumes are equal, insert to least full.
                    {
                        if (m_nodes[node.left()].size() < m_nodes[node.right()].size())
                        {
                            node_index = node.left();
                            bounds = left;
                        }
                        else
                        {
                            node_index = node.right();
                            bounds = right;
                        }
                    }
                    else if (surface_diff < 0)//better to add it to left side
                    {
                        if (balance && needsRebalance(
                              m_nodes[node.left()].size() + 1
                            , m_nodes[node.right()].size()))
                            needs_rebalance = true;
                        else
                            node_index = node.left();
                        bounds = left;
                    }
                    else//better to add it to right side
                    {
                        if (balance && needsRebalance(
                              m_nodes[node.left()].size()
                            , m_nodes[node.right()].size() + 1))
                            needs_rebalance = true;
                        else
                            node_index = node.right();
                        bounds = right;
                    }
                    if (needs_rebalance)
                    {
                        Buffer<uint04> indices(node.size() + 1);
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::template _getAll<false>(node_index, indices);
                        indices.add(index);
                        TreeBase<RNode<t_dims, t_type>, t_type, 2>::reclaimChildren(node_index);
                        _balanceLeafNode<t_has_nans, t_uses_boundary>(node_index, elements, indices, 0, indices.size(), false);
                        return;
                    }
                    else
                    {
                        node.setSize(node.size() + 1);
                    }
                }
            }
        }
        //Returns true if bounds changed, false if stayed the same
        template<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);
            RNode<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()
        {
            Buffer<uint04> indices(TreeBase<RNode<t_dims, t_type>, t_type, 2>::size());
 
            TreeIterator iterator(root_node);
            while (iterator.valid())
            {
                RNode<t_dims, t_type>& node = m_nodes[iterator.get()];
                iterator.pop();
                if (node.isLeaf())
                {
                    node.setBegin(iterator.get());
                    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 RTree : public Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>
    {
    public:
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_nodes;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::root_node;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_indices;
        using TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_bucket_size;
        RTree<t_dims, t_type>()
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(16)
        {}
        explicit RTree<t_dims, t_type>(uint04 bucket_size)
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(bucket_size)
        {}
        RTree<t_dims, t_type>(const RTreeBase<t_dims, t_type>& tree)
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(tree)
        {}
        RTree<t_dims, t_type>(RTreeBase<t_dims, t_type>&& tree)
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(std::move(tree))
        {}
        RTree<t_dims, t_type>(const RTree<t_dims, t_type>& value)
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(16)
        {
            m_bucket_size = value.m_bucket_size;
            m_indices = value.m_indices;
            m_nodes = value.m_nodes;
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions = value.m_available_node_positions;
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions = value.m_available_indexed_positions;
        }
        RTree<t_dims, t_type>(RTree<t_dims, t_type>&& value) noexcept
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(64)
        {
            m_bucket_size = value.m_bucket_size;
            std::swap(m_indices, value.m_indices);
            std::swap(m_nodes, value.m_nodes);
            std::swap(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions, value.m_available_node_positions);
            std::swap(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions, value.m_available_indexed_positions);
        }
        Bounds<t_dims, t_type> bounds() const
        {
            const RNode<t_dims, t_type>& node = m_nodes[root_node];
            return node.bounds();
        }
    protected:
        RTree<t_dims, t_type>(const Buffer<RNode<t_dims, t_type>>& nodes, const Buffer<uint04>& indices, bool is_read_only)
            : Tree<t_dims, t_type, RTreeBase<t_dims, t_type>>(nodes, indices, is_read_only)
        {}
    public:
        inline RTree& operator=(const RTree& value)
        {
            m_bucket_size = value.m_bucket_size;
            m_indices = value.m_indices;
            m_nodes = value.m_nodes;
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions = value.m_available_node_positions;
            TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions = value.m_available_indexed_positions;
            return (*this);
        }
        inline RTree& operator=(RTree&& value)
        {
            m_bucket_size = value.m_bucket_size;
            std::swap(m_indices, value.m_indices);
            std::swap(m_nodes, value.m_nodes);
            std::swap(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions, value.m_available_node_positions);
            std::swap(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions, value.m_available_indexed_positions);
            return (*this);
        }
        void mapToFile(BinaryFile& file, Buffer<BinaryCompressionObject>& objects, uint04 compression_index = 0)
        {
            file.write(m_bucket_size);
            bool placeholder = false;
            file.write(placeholder);
            file.writeCompression(objects[compression_index + 0]);
            file.writeCompression(objects[compression_index + 1]);
            file.writeCompression(objects[compression_index + 2]);
            file.writeCompression(objects[compression_index + 3]);
        }
        void compress(Buffer<BinaryCompressionObject>& objects, Buffer<Buffer<uint01>>& compressed_data, uint04 compression_index, uint04 internal_index)
        {
            switch (internal_index)
            {
            case 0:
                Compressor::Compress(objects[compression_index]
                    , compressed_data[compression_index]
                    , m_indices);
                break;
            case 1:
                Compressor::Compress(objects[compression_index]
                    , compressed_data[compression_index]
                    , m_nodes);
                break;
            case 2:
                Compressor::Compress(objects[compression_index]
                    , compressed_data[compression_index]
                    , TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions);
                break;
            case 3:
                Compressor::Compress(objects[compression_index]
                    , compressed_data[compression_index]
                    , TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions);
                break;
            }
            
        }
        void mapToFile(BinaryFile& file, CompressionMode mode)
        {
            file.write(m_bucket_size);
            bool placeholder = false;
            file.write(placeholder);
            file.write(m_indices, mode);
            file.write(m_nodes, mode);
            file.write(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions, mode);
            file.write(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions, mode);
        }
        void mapFromFile(BinaryFile& file)
        {
            m_bucket_size = file.read<uint04>();
            file.read<bool>();//placeholder
            file.read(m_indices);
            file.read(m_nodes);
            file.read(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_node_positions);
            file.read(TreeBase<RNode<t_dims, t_type>, t_type, 2>::m_available_indexed_positions);
        }
        virtual const char* getTreeType() const override
        {
            return "R Tree";
        }
    };
}