kdTreeTraits.h

/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
 
#pragma once
 
#include "../../Common/Macro.h"
 
#include <cstddef>
 
#include <Eigen/Geometry>
 
namespace Ponca {
#ifndef PARSED_WITH_DOXYGEN
namespace internal
{
    constexpr int clz(unsigned int value)
    {
#if PONCA_HAS_BUILTIN_CLZ
        return __builtin_clz(value);
#else
        if (value == 0)
        {
            return 0;
        }
 
        unsigned int msb_mask = 1 << (sizeof(unsigned int)*8 - 1);
        int count = 0;
        for (; (value & msb_mask) == 0; value <<= 1, ++count)
        {
        }
        return count;
#endif
    }
}
#endif
 
template <typename NodeIndex, typename Scalar, int DIM>
struct KdTreeDefaultInnerNode
{
private:
    enum
    {
        // The minimum bit width required to store the split dimension.
        // 
        // Equal to the index of DIM's most significant bit (starting at 1), e.g.:
        // With DIM = 4,
        //             -------------
        // DIM =    0b | 1 | 0 | 0 |
        //             -------------
        // Bit index =  #3  #2  #1
        // 
        // The MSB has an index of 3, so we store the dimension on 3 bits.
        DIM_BITS = sizeof(unsigned int)*8 - internal::clz((unsigned int)DIM),
    };
 
    // The node stores bitfields as unsigned indices.
    using UIndex = typename std::make_unsigned<NodeIndex>::type;
 
public:
    enum
    {
        INDEX_BITS = sizeof(UIndex)*8 - DIM_BITS,
    };
 
    Scalar split_value{0};
    UIndex first_child_id : INDEX_BITS;
    UIndex split_dim : DIM_BITS;
};
 
template <typename Index, typename Size>
struct KdTreeDefaultLeafNode
{
    Index start{0};
    Size size{0};
};
 
template <typename Index, typename NodeIndex, typename DataPoint,
          typename LeafSize = Index,
          typename _InnerNodeType = KdTreeDefaultInnerNode<NodeIndex, typename DataPoint::Scalar, DataPoint::Dim>,
          typename _LeafNodeType = KdTreeDefaultLeafNode<Index, LeafSize>>
class KdTreeCustomizableNode
{
private:
    using Scalar    = typename DataPoint::Scalar;
    using InnerType = _InnerNodeType;
    using LeafType  = _LeafNodeType;
 
public:
    enum : std::size_t
    {
        MAX_COUNT = std::size_t(2) << InnerType::INDEX_BITS,
    };
 
    using AabbType = Eigen::AlignedBox<Scalar, DataPoint::Dim>;
 
    KdTreeCustomizableNode() = default;
 
    constexpr KdTreeCustomizableNode(KdTreeCustomizableNode&& n)
        : m_is_leaf(n.m_is_leaf)
    {
        if (m_is_leaf)
        {
            data.m_leaf = n.data.m_leaf;
        }
        else
        {
            data.m_inner = n.data.m_inner;
        }
    }
 
    constexpr KdTreeCustomizableNode& operator=(KdTreeCustomizableNode&& n)
    {
        if (&n != this)
        {
            m_is_leaf = n.m_is_leaf;
            if (m_is_leaf)
            {
                data.m_leaf = n.data.m_leaf;
            }
            else
            {
                data.m_inner = n.data.m_inner;
            }
        }
 
        return *this;
    }
 
    constexpr KdTreeCustomizableNode(const KdTreeCustomizableNode& n)
        : m_is_leaf(n.m_is_leaf)
    {
        if (n.m_is_leaf)
        {
            data.m_leaf = n.data.m_leaf;
        }
        else
        {
            data.m_inner = n.data.m_inner;
        }
    }
 
    constexpr KdTreeCustomizableNode& operator=(const KdTreeCustomizableNode& n)
    {
        if (&n != this)
        {
            m_is_leaf = n.m_is_leaf;
            if (n.m_is_leaf)
            {
                data.m_leaf = n.data.m_leaf;
            }
            else
            {
                data.m_inner = n.data.m_inner;
            }
        }
 
        return *this;
    }
 
    ~KdTreeCustomizableNode() {}
    
    [[nodiscard]] bool is_leaf() const { return m_is_leaf; }
    void set_is_leaf(bool is_leaf) { m_is_leaf = is_leaf; }
 
    void configure_range(Index start, Index size, const AabbType &aabb)
    {
        if (m_is_leaf)
        {
            data.m_leaf.start = start;
            data.m_leaf.size = (LeafSize)size;
        }
    }
 
    void configure_inner(Scalar split_value, Index first_child_id, Index split_dim)
    {
        if (!m_is_leaf)
        {
            data.m_inner.split_value = split_value;
            data.m_inner.first_child_id = first_child_id;
            data.m_inner.split_dim = split_dim;
        }
    }
 
    [[nodiscard]] Index leaf_start() const { return data.m_leaf.start; }
 
    [[nodiscard]] LeafSize leaf_size() const { return data.m_leaf.size; }
 
    [[nodiscard]] Scalar inner_split_value() const { return data.m_inner.split_value; }
    
    [[nodiscard]] int inner_split_dim() const { return (int)data.m_inner.split_dim; }
    
    [[nodiscard]] Index inner_first_child_id() const { return (Index)data.m_inner.first_child_id; }
 
protected:
    [[nodiscard]] inline LeafType& getAsLeaf() { return data.m_leaf; }
    [[nodiscard]] inline InnerType& getAsInner() { return data.m_inner; }
    [[nodiscard]] inline const LeafType& getAsLeaf() const { return data.m_leaf; }
    [[nodiscard]] inline const InnerType& getAsInner() const { return data.m_inner; }
 
private:
    bool m_is_leaf{true};
    union Data
    {
        // We need an explicit constructor here, see https://stackoverflow.com/a/70428826
        constexpr Data() : m_leaf() {}
 
        ~Data() {}
        LeafType m_leaf;
        InnerType m_inner;
    };
    Data data;
};
 
template <typename Index, typename NodeIndex, typename DataPoint,
        typename LeafSize = Index>
struct KdTreeDefaultNode : public KdTreeCustomizableNode<Index, NodeIndex, DataPoint, LeafSize,
        KdTreeDefaultInnerNode<NodeIndex, typename DataPoint::Scalar, DataPoint::Dim>,
        KdTreeDefaultLeafNode<Index, LeafSize>> {
    using Base =  KdTreeCustomizableNode<Index, NodeIndex, DataPoint, LeafSize,
            KdTreeDefaultInnerNode<NodeIndex, typename DataPoint::Scalar, DataPoint::Dim>,
            KdTreeDefaultLeafNode<Index, LeafSize>>;
};
 
template <typename _DataPoint,
        template <typename /*Index*/,
                  typename /*NodeIndex*/,
                  typename /*DataPoint*/,
                  typename /*LeafSize*/> typename _NodeType = KdTreeDefaultNode>
struct KdTreeDefaultTraits
{
    enum
    {
        MAX_DEPTH = 32,
    };
 
    using DataPoint    = _DataPoint;
    using IndexType    = int;
    using LeafSizeType = unsigned short;
 
    // Containers
    using PointContainer = std::vector<DataPoint>;
    using IndexContainer = std::vector<IndexType>;
 
    // Nodes
    using NodeIndexType = std::size_t;
    using NodeType      = _NodeType<IndexType, NodeIndexType, DataPoint, LeafSizeType>;
    using NodeContainer = std::vector<NodeType>;
};
} // namespace Ponca