diff --git a/Ponca/src/SpatialPartitioning/KdTree/Query/kdTreeQuery.h b/Ponca/src/SpatialPartitioning/KdTree/Query/kdTreeQuery.h index 953643c44..f51b29912 100644 --- a/Ponca/src/SpatialPartitioning/KdTree/Query/kdTreeQuery.h +++ b/Ponca/src/SpatialPartitioning/KdTree/Query/kdTreeQuery.h @@ -61,8 +61,8 @@ class KdTreeQuery if(node.is_leaf()) { m_stack.pop(); - IndexType start = node.leaf.start; - IndexType end = node.leaf.start + node.leaf.size; + IndexType start = node.leaf_start(); + IndexType end = node.leaf_start() + node.leaf_size(); prepareLeafTraversal(start, end); for(IndexType i=start; i class KdTreeBase { public: - using DataPoint = typename Traits::DataPoint; ///< DataPoint given by user via Traits - using Scalar = typename DataPoint::Scalar; ///< Scalar given by user via DataPoint - using VectorType = typename DataPoint::VectorType; ///< VectorType given by user via DataPoint - using AabbType = typename Traits::AabbType; ///< Bounding box type given by user via DataPoint + using DataPoint = typename Traits::DataPoint; ///< DataPoint given by user via Traits + using IndexType = typename Traits::IndexType; ///< Type used to index points into the PointContainer + using LeafSizeType = typename Traits::LeafSizeType; ///< Type used to store the size of leaf nodes - using IndexType = typename Traits::IndexType; using PointContainer = typename Traits::PointContainer; ///< Container for DataPoint used inside the KdTree using IndexContainer = typename Traits::IndexContainer; ///< Container for indices used inside the KdTree + + using NodeIndexType = typename Traits::NodeIndexType; ///< Type used to index nodes into the NodeContainer + using NodeType = typename Traits::NodeType; ///< Type of nodes used inside the KdTree using NodeContainer = typename Traits::NodeContainer; ///< Container for nodes used inside the KdTree - using NodeType = typename NodeContainer::value_type; - using NodeCountType = typename NodeContainer::size_type; - using LeafSizeType = typename NodeType::LeafSizeType; + using Scalar = typename DataPoint::Scalar; ///< Scalar given by user via DataPoint + using VectorType = typename DataPoint::VectorType; ///< VectorType given by user via DataPoint + using AabbType = typename NodeType::AabbType; ///< Bounding box type given by user via NodeType enum { /*! - * The maximum number of points that can be stored in the kd-tree, considering how many - * bits the inner nodes use to store their children indices. + * The maximum number of points that can be stored in the kd-tree, + * considering the bit width of the index type. */ - MAX_POINT_COUNT = 2 << NodeType::InnerType::INDEX_BITS, + MAX_POINT_COUNT = std::size_t(2) << sizeof(IndexType)*8, }; static_assert(std::is_same::value, @@ -80,7 +81,9 @@ class KdTreeBase // Queries use a value of -1 for invalid indices static_assert(std::is_signed::value, "Index type must be signed"); + static_assert(std::is_same::value, "Index type mismatch"); + static_assert(std::is_same::value, "Node type mismatch"); static_assert(Traits::MAX_DEPTH > 0, "Max depth must be strictly positive"); @@ -188,7 +191,7 @@ class KdTreeBase // Accessors --------------------------------------------------------------- public: - inline NodeCountType node_count() const + inline NodeIndexType node_count() const { return m_nodes.size(); } @@ -203,7 +206,7 @@ class KdTreeBase return (IndexType)m_points.size(); } - inline NodeCountType leaf_count() const + inline NodeIndexType leaf_count() const { return m_leaf_count; } @@ -255,7 +258,7 @@ class KdTreeBase // Internal ---------------------------------------------------------------- protected: - inline void build_rec(NodeCountType node_id, IndexType start, IndexType end, int level); + inline void build_rec(NodeIndexType node_id, IndexType start, IndexType end, int level); inline IndexType partition(IndexType start, IndexType end, int dim, Scalar value); // Query ------------------------------------------------------------------- @@ -298,7 +301,7 @@ public : IndexContainer m_indices; LeafSizeType m_min_cell_size; ///< Minimal number of points per leaf - NodeCountType m_leaf_count; ///< Number of leaves in the Kdtree (computed during construction) + NodeIndexType m_leaf_count; ///< Number of leaves in the Kdtree (computed during construction) }; #include "./kdTree.hpp" diff --git a/Ponca/src/SpatialPartitioning/KdTree/kdTree.hpp b/Ponca/src/SpatialPartitioning/KdTree/kdTree.hpp index 94e806c8c..5912c7ee3 100644 --- a/Ponca/src/SpatialPartitioning/KdTree/kdTree.hpp +++ b/Ponca/src/SpatialPartitioning/KdTree/kdTree.hpp @@ -95,12 +95,12 @@ bool KdTreeBase::valid() const b[idx] = true; } - for(NodeCountType n=0;n::valid() const } else { - if(node.inner.dim < 0 || 2 < node.inner.dim) + if(node.inner_dim() < 0 || 2 < node.inner_dim()) { PONCA_DEBUG_ERROR; return false; } - if(node_count() <= node.inner.first_child_id || node_count() <= node.inner.first_child_id+1) + if(node_count() <= node.inner_first_child_id() || node_count() <= node.inner_first_child_id()+1) { PONCA_DEBUG_ERROR; return false; @@ -136,24 +136,24 @@ std::string KdTreeBase::to_string() const str << " " << i << ": " << m_indices.operator[](i) << "\n"; } str << "nodes (" << node_count() << ") :\n"; - for(NodeCountType n=0; n< node_count(); ++n) + for(NodeIndexType n=0; n< node_count(); ++n) { const NodeType& node = m_nodes.operator[](n); if(node.is_leaf()) { - auto end = node.leaf.start + node.leaf.size; - str << " leaf: start=" << node.leaf.start << " end=" << end << " (size=" << node.leaf.size << ")\n"; + auto end = node.leaf_start() + node.leaf_size(); + str << " leaf: start=" << node.leaf_start() << " end=" << end << " (size=" << node.leaf_size() << ")\n"; } else { - str << " node: dim=" << node.inner.dim << " split=" << node.inner.split_value << " child=" << node.inner.first_child_id << "\n"; + str << " node: dim=" << node.inner_dim() << " split=" << node.inner_split_value() << " child=" << node.inner_first_child_id() << "\n"; } } return str.str(); } template -void KdTreeBase::build_rec(NodeCountType node_id, IndexType start, IndexType end, int level) +void KdTreeBase::build_rec(NodeIndexType node_id, IndexType start, IndexType end, int level) { NodeType& node = m_nodes[node_id]; AabbType aabb; @@ -161,26 +161,22 @@ void KdTreeBase::build_rec(NodeCountType node_id, IndexType start, Index aabb.extend(m_points[m_indices[i]].pos()); node.set_is_leaf(end-start <= m_min_cell_size || level >= Traits::MAX_DEPTH); + node.configure_range(start, end-start, aabb); if (node.is_leaf()) { - node.leaf.start = start; - node.leaf.size = static_cast(end-start); ++m_leaf_count; } else { - int dim = 0; - (Scalar(0.5) * (aabb.max() - aabb.min())).maxCoeff(&dim); - node.inner.dim = dim; - node.inner.split_value = aabb.center()[dim]; - - IndexType mid_id = this->partition(start, end, dim, node.inner.split_value); - node.inner.first_child_id = m_nodes.size(); + int split_dim = 0; + (Scalar(0.5) * aabb.diagonal()).maxCoeff(&split_dim); + node.configure_inner(aabb.center()[split_dim], m_nodes.size(), split_dim); m_nodes.emplace_back(); m_nodes.emplace_back(); - build_rec(node.inner.first_child_id, start, mid_id, level+1); - build_rec(node.inner.first_child_id+1, mid_id, end, level+1); + IndexType mid_id = this->partition(start, end, split_dim, node.inner_split_value()); + build_rec(node.inner_first_child_id(), start, mid_id, level+1); + build_rec(node.inner_first_child_id()+1, mid_id, end, level+1); } } diff --git a/Ponca/src/SpatialPartitioning/KdTree/kdTreeTraits.h b/Ponca/src/SpatialPartitioning/KdTree/kdTreeTraits.h index 5f4977eb8..6c6cb38a1 100644 --- a/Ponca/src/SpatialPartitioning/KdTree/kdTreeTraits.h +++ b/Ponca/src/SpatialPartitioning/KdTree/kdTreeTraits.h @@ -9,14 +9,26 @@ #include +#include + +#ifdef __has_builtin +#if __has_builtin(__builtin_clz) +#define PONCA_HAS_BUILTIN_CLZ 1 +#endif +#endif + +#ifndef PONCA_HAS_BUILTIN_CLZ +#define PONCA_HAS_BUILTIN_CLZ 0 +#endif + namespace Ponca { #ifndef PARSED_WITH_DOXYGEN namespace internal { constexpr int clz(unsigned int value) { -#if defined(__has_builtin) && __has_builtin(__builtin_clz) - return __builtin_clz(value); +#if PONCA_HAS_BUILTIN_CLZ + return __builtin_clz(value); #else if (value == 0) { @@ -31,123 +43,122 @@ namespace internal return count; #endif } - - /*! - * \brief Calculates bitfield sizes of a default kd-tree inner node at compile-time. - * - * \tparam Index The type used to index data points. - * \tparam DIM The dimension of the data points. - */ - template - struct KdTreeDefaultInnerNodeBitfieldInfo - { - using UIndex = typename std::make_unsigned::type; - - static_assert(DIM >= 0, "Dim must be positive"); - - enum - { - /*! - * \brief The minimum bit width required to store the point 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), - UINDEX_BITS = sizeof(UIndex)*8, - }; - - // 1 bit is reserved for the leaf flag - static_assert(DIM_BITS < UINDEX_BITS - 1, - "Dim does not fit in the index bitfield of a default inner node"); - - enum - { - /*! - * \brief The number of remaining bits that can be used to store indices. - */ - CHILD_ID_BITS = UINDEX_BITS - (DIM_BITS + 1), - }; - }; } #endif -template +template struct KdTreeDefaultInnerNode { private: - using BitfieldInfo = internal::KdTreeDefaultInnerNodeBitfieldInfo; - using UIndex = typename BitfieldInfo::UIndex; + // We're using unsigned indices since we're using bitfields. + using UIndex = typename std::make_unsigned::type; -public: enum { - /*! - * \brief The number of bits used to store the point dimension. - * - * Points being in higher dimensions will result in less possible points being stored in the kd-tree. - */ - DIM_BITS = BitfieldInfo::DIM_BITS, + // 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), + }; +public: + enum + { /*! - * \brief The number of bits used to store point indices. + * \brief The bit width used to store the first child index. */ - INDEX_BITS = BitfieldInfo::CHILD_ID_BITS, + INDEX_BITS = sizeof(UIndex)*8 - DIM_BITS, }; Scalar split_value; UIndex first_child_id : INDEX_BITS; - UIndex dim : DIM_BITS; - UIndex leaf : 1; + UIndex split_dim : DIM_BITS; }; template struct KdTreeDefaultLeafNode { Index start; - Size size; + Size size; }; /*! * \brief The node type used by default by the kd-tree. */ -template -struct KdTreeDefaultNode +template +class KdTreeDefaultNode { - /*! - * \brief The type of the `inner` member. - * - * Must provide a `split_value` member of type `Scalar`, a `first_child_id` member and a `dim` member. - * - * Must also provide `DIM_BITS` and `INDEX_BITS` compile-time constants specifying the number of - * bits used to store `dim` and `first_child_id` respecitvely. These constants are used to - * determine the maximum number of points the kd-tree can store. - */ - using InnerType = KdTreeDefaultInnerNode; - using LeafSizeType = unsigned short; +private: + using Scalar = typename DataPoint::Scalar; +public: /*! - * \brief The type of the `leaf` member. + * \brief The type used to store node bounding boxes. * - * Must provide a `start` and a `size` member. `size` must be of type `LeafSizeType` and - * `start` must be large enough to store point indices. + * Must provide `diagonal()`, and `center()` functions, all returning a + * `DataPoint::VectorType`. */ - using LeafType = KdTreeDefaultLeafNode; + using AabbType = Eigen::AlignedBox; + + KdTreeDefaultNode() = default; + + /*!*/ + 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) + { + m_leaf.start = start; + m_leaf.size = (LeafSize)size; + } + } + + /*!*/ + void configure_inner(Scalar split_value, Index first_child_id, Index split_dim) + { + if (!m_is_leaf) + { + m_inner.split_value = split_value; + m_inner.first_child_id = first_child_id; + m_inner.split_dim = split_dim; + } + } + + /*!*/ + Index leaf_start() const { return m_leaf.start; } + + /*!*/ + LeafSize leaf_size() const { return m_leaf.size; } + + /*!*/ + Scalar inner_split_value() const { return m_inner.split_value; } + + /*!*/ + int inner_split_dim() const { return (int)m_inner.split_dim; } + + /*!*/ + Index inner_first_child_id() const { return (Index)m_inner.first_child_id; } +private: + bool m_is_leaf; union { - InnerType inner; - LeafType leaf; + KdTreeDefaultLeafNode m_leaf; + KdTreeDefaultInnerNode m_inner; }; - - bool is_leaf() const { return inner.leaf; } - void set_is_leaf(bool new_is_leaf) { inner.leaf = new_is_leaf; } }; /*! @@ -156,22 +167,6 @@ struct KdTreeDefaultNode template struct KdTreeDefaultTraits { - /*! - * \brief The type used to store point data. - * - * Must provide `Scalar` and `VectorType` typedefs. - * - * `VectorType` must provide a `squaredNorm()` function returning a `Scalar`, as well as a - * `maxCoeff(int*)` function returning the dimension index of its largest scalar in its output - * parameter (e.g. 0 for *x*, 1 for *y*, etc.). - */ - using DataPoint = _DataPoint; - -private: - using Scalar = typename DataPoint::Scalar; - using VectorType = typename DataPoint::VectorType; - -public: enum { /*! @@ -181,24 +176,25 @@ struct KdTreeDefaultTraits }; /*! - * \brief The type used to calculate node bounding boxes. + * \brief The type used to store point data. + * + * Must provide `Scalar` and `VectorType` typedefs. * - * Must provide `min()`, `max()`, and `center()` functions, all returning a `VectorType`. + * `VectorType` must provide a `squaredNorm()` function returning a `Scalar`, as well as a + * `maxCoeff(int*)` function returning the dimension index of its largest scalar in its output + * parameter (e.g. 0 for *x*, 1 for *y*, etc.). */ - using AabbType = Eigen::AlignedBox; + using DataPoint = _DataPoint; + using IndexType = int; + using LeafSizeType = unsigned short; // Containers - using IndexType = int; using PointContainer = std::vector; using IndexContainer = std::vector; - /*! - * \brief The container used to store nodes. - * - * \note The node type is deduced from the node container type via `NodeContainer::value_type`. - * - * \see KdTreeDefaultNode for the node interface documentation. - */ - using NodeContainer = std::vector>; + // Nodes + using NodeIndexType = std::size_t; + using NodeType = KdTreeDefaultNode; + using NodeContainer = std::vector; }; } // namespace Ponca