9 #ifndef CUBBYFLOW_KDTREE_IMPL_H 10 #define CUBBYFLOW_KDTREE_IMPL_H 16 template <
typename T,
size_t K>
19 child = std::numeric_limits<size_t>::max();
22 template <
typename T,
size_t K>
27 child = std::numeric_limits<size_t>::max();
31 template <
typename T,
size_t K>
40 template <
typename T,
size_t K>
46 template <
typename T,
size_t K>
52 template <
typename T,
size_t K>
55 m_points.resize(points.
size());
56 std::copy(points.
begin(), points.
end(), m_points.begin());
65 std::vector<size_t> itemIndices(m_points.size());
66 std::iota(std::begin(itemIndices), std::end(itemIndices), 0);
68 Build(0, itemIndices.data(), m_points.size(), 0);
71 template <
typename T,
size_t K>
73 const Point& origin, T radius,
74 const std::function<
void(
size_t,
const Point&)>& callback)
const 76 const T r2 = radius * radius;
79 static const int maxTreeDepth = 8 *
sizeof(size_t);
80 const Node* todo[maxTreeDepth];
84 const Node* node = m_nodes.data();
86 while (node !=
nullptr)
88 if (node->item != std::numeric_limits<size_t>::max() && (node->point - origin).LengthSquared() <= r2)
90 callback(node->item, node->point);
100 node = todo[todoPos];
110 const Node* firstChild = node + 1;
111 const Node* secondChild =
const_cast<Node*
>(&m_nodes[node->child]);
114 const size_t axis = node->flags;
115 const T plane = node->point[axis];
117 if (plane - origin[axis] > radius)
121 else if (origin[axis] - plane > radius)
128 todo[todoPos] = secondChild;
136 template <
typename T,
size_t K>
139 const T r2 = radius * radius;
142 static const int maxTreeDepth = 8 *
sizeof(size_t);
143 const Node* todo[maxTreeDepth];
147 const Node* node = m_nodes.data();
149 while (node !=
nullptr)
151 if (node->item != std::numeric_limits<size_t>::max() && (node->point - origin).LengthSquared() <= r2)
163 node = todo[todoPos];
173 const Node* firstChild = node + 1;
174 const Node* secondChild =
const_cast<Node*
>(&m_nodes[node->child]);
177 const size_t axis = node->flags;
178 const T plane = node->point[axis];
180 if (origin[axis] < plane && plane - origin[axis] > radius)
184 else if (origin[axis] > plane && origin[axis] - plane > radius)
191 todo[todoPos] = secondChild;
201 template <
typename T,
size_t K>
205 static const int maxTreeDepth = 8 *
sizeof(size_t);
206 const Node* todo[maxTreeDepth];
210 const Node* node = m_nodes.data();
212 T minDist2 = (node->point - origin).LengthSquared();
214 while (node !=
nullptr)
216 const T newDist2 = (node->point - origin).LengthSquared();
217 if (newDist2 <= minDist2)
219 nearest = node->item;
230 node = todo[todoPos];
240 const Node* firstChild = node + 1;
241 const Node* secondChild =
static_cast<Node*
>(&m_nodes[node->child]);
244 const size_t axis = node->flags;
245 const T plane = node->point[axis];
246 const T minDist = std::sqrt(minDist2);
248 if (plane - origin[axis] > minDist)
252 else if (origin[axis] - plane > minDist)
259 todo[todoPos] = secondChild;
269 template <
typename T,
size_t K>
272 m_points.resize(numPoints);
273 m_nodes.resize(numNodes);
276 template <
typename T,
size_t K>
279 return m_points.begin();
282 template <
typename T,
size_t K>
285 return m_points.end();
288 template <
typename T,
size_t K>
291 return m_points.begin();
294 template <
typename T,
size_t K>
297 return m_points.end();
300 template <
typename T,
size_t K>
303 return m_nodes.begin();
306 template <
typename T,
size_t K>
309 return m_nodes.end();
312 template <
typename T,
size_t K>
315 return m_nodes.begin();
318 template <
typename T,
size_t K>
321 return m_nodes.end();
324 template <
typename T,
size_t K>
325 size_t KdTree<T, K>::Build(
size_t nodeIndex,
size_t* itemIndices,
size_t nItems,
size_t currentDepth)
328 m_nodes.emplace_back();
333 m_nodes[nodeIndex].InitLeaf(std::numeric_limits<size_t>::max(), {});
334 return currentDepth + 1;
338 m_nodes[nodeIndex].InitLeaf(itemIndices[0], m_points[itemIndices[0]]);
339 return currentDepth + 1;
344 for (
size_t i = 0; i < nItems; ++i)
346 nodeBound.Merge(m_points[itemIndices[i]]);
348 Point d = nodeBound.upperCorner - nodeBound.lowerCorner;
349 size_t axis =
static_cast<size_t>(d.DominantAxis());
352 std::nth_element(itemIndices, itemIndices + nItems / 2, itemIndices + nItems,
353 [&](
size_t a,
size_t b)
355 return m_points[a][axis] < m_points[b][axis];
357 const size_t midPoint = nItems / 2;
360 const size_t d0 =
Build(nodeIndex + 1, itemIndices, midPoint, currentDepth + 1);
361 m_nodes[nodeIndex].InitInternal(axis, itemIndices[midPoint], m_nodes.size(), m_points[itemIndices[midPoint]]);
362 const size_t d1 =
Build(m_nodes[nodeIndex].child, itemIndices + midPoint + 1, nItems - midPoint - 1, currentDepth + 1);
364 return std::max(d0, d1);
void Reserve(size_t numPoints, size_t numNodes)
Reserves memory space for this tree.
Definition: KdTree-Impl.h:270
NodeIterator EndNode()
Returns the mutable end iterator of the node.
Definition: KdTree-Impl.h:307
typename ContainerType::const_iterator ConstIterator
Definition: KdTree.h:56
typename NodeContainerType::iterator NodeIterator
Definition: KdTree.h:59
const T *const begin() const
Returns the begin iterator of the array.
Definition: ArrayAccessor1-Impl.h:207
Iterator begin()
Returns the mutable begin iterator of the item.
Definition: KdTree-Impl.h:277
bool IsLeaf() const
Returns true if leaf.
Definition: KdTree-Impl.h:41
Node()
Default contructor.
Definition: KdTree-Impl.h:17
bool HasNearbyPoint(const Point &origin, T radius) const
Definition: KdTree-Impl.h:137
1-D read-only array accessor class.
Definition: ArrayAccessor1.h:185
NodeIterator BeginNode()
Returns the mutable begin iterator of the node.
Definition: KdTree-Impl.h:301
void InitInternal(size_t axis, size_t it, size_t c, const Point &pt)
Initializes internal node.
Definition: KdTree-Impl.h:32
size_t GetNearestPoint(const Point &origin) const
Returns index of the nearest point.
Definition: KdTree-Impl.h:202
BoundingBox< T, K > BBox
Definition: KdTree.h:23
Definition: pybind11Utils.h:24
size_t size() const
Returns size of the array.
Definition: ArrayAccessor1-Impl.h:219
typename NodeContainerType::const_iterator ConstNodeIterator
Definition: KdTree.h:60
void Build(const ConstArrayAccessor1< Point > &points)
Builds internal acceleration structure for given points list.
Definition: KdTree-Impl.h:53
const T *const end() const
Returns the end iterator of the array.
Definition: ArrayAccessor1-Impl.h:213
size_t child
Right child index. Note that left child index is this node index + 1.
Definition: KdTree.h:33
Iterator end()
Returns the mutable end iterator of the item.
Definition: KdTree-Impl.h:283
KdTree()
Constructs an empty kD-tree instance.
Definition: KdTree-Impl.h:47
typename ContainerType::iterator Iterator
Definition: KdTree.h:55
void ForEachNearbyPoint(const Point &origin, T radius, const std::function< void(size_t, const Point &)> &callback) const
Definition: KdTree-Impl.h:72
void InitLeaf(size_t it, const Point &pt)
Initializes leaf node.
Definition: KdTree-Impl.h:23
Vector< T, K > Point
Definition: KdTree.h:22