CDT  1.4.1
C++ library for constrained Delaunay triangulation
Loading...
Searching...
No Matches
KDTree.h
1
6
7#ifndef KDTREE_KDTREE_H
8#define KDTREE_KDTREE_H
9
10#include "CDTUtils.h"
11
12#include <cassert>
13#include <limits>
14
15namespace CDT
16{
17namespace KDTree
18{
19
21{
22 enum Enum
23 {
24 X,
25 Y,
26 };
27};
28
40template <
41 typename TCoordType,
42 size_t NumVerticesInLeaf,
43 size_t InitialStackDepth,
44 size_t StackDepthIncrement>
45class KDTree
46{
47public:
48 typedef TCoordType coord_type;
50 typedef CDT::VertInd point_index;
51 typedef std::pair<point_type, point_index> value_type;
52 typedef std::vector<point_index> point_data_vec;
53 typedef point_data_vec::const_iterator pd_cit;
54 typedef CDT::VertInd node_index;
55 typedef CDT::array<node_index, 2> children_type;
56
58 struct Node
59 {
60 children_type children;
61 point_data_vec data;
64 {
65 setChildren(node_index(0), node_index(0));
66 data.reserve(NumVerticesInLeaf);
67 }
69 void setChildren(const node_index c1, const node_index c2)
70 {
71 children[0] = c1;
72 children[1] = c2;
73 }
75 bool isLeaf() const
76 {
77 return children[0] == children[1];
78 }
79 };
80
83 : m_rootDir(NodeSplitDirection::X)
84 , m_min(point_type::make(
85 -std::numeric_limits<coord_type>::max(),
86 -std::numeric_limits<coord_type>::max()))
87 , m_max(point_type::make(
88 std::numeric_limits<coord_type>::max(),
89 std::numeric_limits<coord_type>::max()))
90 , m_size(0)
91 , m_isRootBoxInitialized(false)
92 , m_tasksStack(InitialStackDepth, NearestTask())
93 {
94 m_root = addNewNode();
95 }
96
98 KDTree(const point_type& min, const point_type& max)
99 : m_rootDir(NodeSplitDirection::X)
100 , m_min(min)
101 , m_max(max)
102 , m_size(0)
103 , m_isRootBoxInitialized(true)
104 , m_tasksStack(InitialStackDepth, NearestTask())
105 {
106 m_root = addNewNode();
107 }
108
109 CDT::VertInd size() const
110 {
111 return m_size;
112 }
113
118 void
119 insert(const point_index& iPoint, const std::vector<point_type>& points)
120 {
121 ++m_size;
122 // if point is outside root, extend tree by adding new roots
123 const point_type& pos = points[iPoint];
124 while(!isInsideBox(pos, m_min, m_max))
125 {
126 extendTree(pos);
127 }
128 // now insert the point into the tree
129 node_index node = m_root;
130 point_type min = m_min;
131 point_type max = m_max;
132 NodeSplitDirection::Enum dir = m_rootDir;
133
134 // below: initialized only to suppress warnings
135 NodeSplitDirection::Enum newDir(NodeSplitDirection::X);
136 coord_type mid(0);
137 point_type newMin, newMax;
138 while(true)
139 {
140 if(m_nodes[node].isLeaf())
141 {
142 // add point if capacity is not reached
143 point_data_vec& pd = m_nodes[node].data;
144 if(pd.size() < NumVerticesInLeaf)
145 {
146 pd.push_back(iPoint);
147 return;
148 }
149 // initialize bbox first time the root capacity is reached
150 if(!m_isRootBoxInitialized)
151 {
152 initializeRootBox(points);
153 min = m_min;
154 max = m_max;
155 }
156 // split a full leaf node
157 calcSplitInfo(min, max, dir, mid, newDir, newMin, newMax);
158 const node_index c1 = addNewNode(), c2 = addNewNode();
159 Node& n = m_nodes[node];
160 n.setChildren(c1, c2);
161 point_data_vec& c1data = m_nodes[c1].data;
162 point_data_vec& c2data = m_nodes[c2].data;
163 // move node's points to children
164 for(pd_cit it = n.data.begin(); it != n.data.end(); ++it)
165 {
166 whichChild(points[*it], mid, dir) == 0
167 ? c1data.push_back(*it)
168 : c2data.push_back(*it);
169 }
170 n.data = point_data_vec();
171 }
172 else
173 {
174 calcSplitInfo(min, max, dir, mid, newDir, newMin, newMax);
175 }
176 // add the point to a child
177 const std::size_t iChild = whichChild(points[iPoint], mid, dir);
178 iChild == 0 ? max = newMax : min = newMin;
179 node = m_nodes[node].children[iChild];
180 dir = newDir;
181 }
182 }
183
188 value_type nearest(
189 const point_type& point,
190 const std::vector<point_type>& points) const
191 {
192 value_type out;
193 int iTask = -1;
194 coord_type minDistSq = std::numeric_limits<coord_type>::max();
195 m_tasksStack[++iTask] =
196 NearestTask(m_root, m_min, m_max, m_rootDir, minDistSq);
197 while(iTask != -1)
198 {
199 const NearestTask t = m_tasksStack[iTask--];
200 if(t.distSq > minDistSq)
201 continue;
202 const Node& n = m_nodes[t.node];
203 if(n.isLeaf())
204 {
205 for(pd_cit it = n.data.begin(); it != n.data.end(); ++it)
206 {
207 const point_type& p = points[*it];
208 const coord_type distSq = CDT::distanceSquared(point, p);
209 if(distSq < minDistSq)
210 {
211 minDistSq = distSq;
212 out.first = p;
213 out.second = *it;
214 }
215 }
216 }
217 else
218 {
219 coord_type mid(0);
220 NodeSplitDirection::Enum newDir;
221 point_type newMin, newMax;
222 calcSplitInfo(t.min, t.max, t.dir, mid, newDir, newMin, newMax);
223
224 const coord_type distToMid = t.dir == NodeSplitDirection::X
225 ? (point.x - mid)
226 : (point.y - mid);
227 const coord_type toMidSq = distToMid * distToMid;
228
229 const std::size_t iChild = whichChild(point, mid, t.dir);
230 if(iTask + 2 >= static_cast<int>(m_tasksStack.size()))
231 {
232 m_tasksStack.resize(
233 m_tasksStack.size() + StackDepthIncrement);
234 }
235 // node containing point should end up on top of the stack
236 if(iChild == 0)
237 {
238 m_tasksStack[++iTask] = NearestTask(
239 n.children[1], newMin, t.max, newDir, toMidSq);
240 m_tasksStack[++iTask] = NearestTask(
241 n.children[0], t.min, newMax, newDir, toMidSq);
242 }
243 else
244 {
245 m_tasksStack[++iTask] = NearestTask(
246 n.children[0], t.min, newMax, newDir, toMidSq);
247 m_tasksStack[++iTask] = NearestTask(
248 n.children[1], newMin, t.max, newDir, toMidSq);
249 }
250 }
251 }
252 return out;
253 }
254
255private:
257 node_index addNewNode()
258 {
259 const node_index newNodeIndex = static_cast<node_index>(m_nodes.size());
260 m_nodes.push_back(Node());
261 return newNodeIndex;
262 }
263
266 std::size_t whichChild(
267 const point_type& point,
268 const coord_type& split,
269 const NodeSplitDirection::Enum dir) const
270 {
271 return static_cast<size_t>(
272 dir == NodeSplitDirection::X ? point.x > split : point.y > split);
273 }
274
276 static void calcSplitInfo(
277 const point_type& min,
278 const point_type& max,
279 const NodeSplitDirection::Enum dir,
280 coord_type& midOut,
281 NodeSplitDirection::Enum& newDirOut,
282 point_type& newMinOut,
283 point_type& newMaxOut)
284 {
285 newMaxOut = max;
286 newMinOut = min;
287 switch(dir)
288 {
289 case NodeSplitDirection::X:
290 midOut = (min.x + max.x) / coord_type(2);
291 newDirOut = NodeSplitDirection::Y;
292 newMinOut.x = midOut;
293 newMaxOut.x = midOut;
294 return;
295 case NodeSplitDirection::Y:
296 midOut = (min.y + max.y) / coord_type(2);
297 newDirOut = NodeSplitDirection::X;
298 newMinOut.y = midOut;
299 newMaxOut.y = midOut;
300 return;
301 }
302 }
303
305 static bool isInsideBox(
306 const point_type& p,
307 const point_type& min,
308 const point_type& max)
309 {
310 return p.x >= min.x && p.x <= max.x && p.y >= min.y && p.y <= max.y;
311 }
312
315 void extendTree(const point_type& point)
316 {
317 const node_index newRoot = addNewNode();
318 const node_index newLeaf = addNewNode();
319 switch(m_rootDir)
320 {
321 case NodeSplitDirection::X:
322 m_rootDir = NodeSplitDirection::Y;
323 point.y < m_min.y ? m_nodes[newRoot].setChildren(newLeaf, m_root)
324 : m_nodes[newRoot].setChildren(m_root, newLeaf);
325 if(point.y < m_min.y)
326 m_min.y -= m_max.y - m_min.y;
327 else if(point.y > m_max.y)
328 m_max.y += m_max.y - m_min.y;
329 break;
330 case NodeSplitDirection::Y:
331 m_rootDir = NodeSplitDirection::X;
332 point.x < m_min.x ? m_nodes[newRoot].setChildren(newLeaf, m_root)
333 : m_nodes[newRoot].setChildren(m_root, newLeaf);
334 if(point.x < m_min.x)
335 m_min.x -= m_max.x - m_min.x;
336 else if(point.x > m_max.x)
337 m_max.x += m_max.x - m_min.x;
338 break;
339 }
340 m_root = newRoot;
341 }
342
344 void initializeRootBox(const std::vector<point_type>& points)
345 {
346 const point_data_vec& data = m_nodes[m_root].data;
347 m_min = points[data.front()];
348 m_max = m_min;
349 for(pd_cit it = data.begin(); it != data.end(); ++it)
350 {
351 const point_type& p = points[*it];
352 m_min = point_type::make(
353 std::min(m_min.x, p.x), std::min(m_min.y, p.y));
354 m_max = point_type::make(
355 std::max(m_max.x, p.x), std::max(m_max.y, p.y));
356 }
357 // Make sure bounding box does not have a zero size by adding padding:
358 // zero-size bounding box cannot be extended properly
359 const TCoordType padding(1);
360 if(m_min.x == m_max.x)
361 {
362 m_min.x -= padding;
363 m_max.x += padding;
364 }
365 if(m_min.y == m_max.y)
366 {
367 m_min.y -= padding;
368 m_max.y += padding;
369 }
370 m_isRootBoxInitialized = true;
371 }
372
373private:
374 node_index m_root;
375 std::vector<Node> m_nodes;
376 NodeSplitDirection::Enum m_rootDir;
377 point_type m_min;
378 point_type m_max;
379 CDT::VertInd m_size;
380
381 bool m_isRootBoxInitialized;
382
383 // used for nearest query
384 struct NearestTask
385 {
386 node_index node;
387 point_type min, max;
388 NodeSplitDirection::Enum dir;
389 coord_type distSq;
390 NearestTask()
391 {}
392 NearestTask(
393 const node_index node_,
394 const point_type& min_,
395 const point_type& max_,
396 const NodeSplitDirection::Enum dir_,
397 const coord_type distSq_)
398 : node(node_)
399 , min(min_)
400 , max(max_)
401 , dir(dir_)
402 , distSq(distSq_)
403 {}
404 };
405 // allocated in class (not in the 'nearest' method) for better performance
406 mutable std::vector<NearestTask> m_tasksStack;
407};
408
409} // namespace KDTree
410} // namespace CDT
411
412#endif // header guard
Utilities and helpers.
Simple tree structure with alternating half splitting nodes.
Definition KDTree.h:46
KDTree()
Default constructor.
Definition KDTree.h:82
void insert(const point_index &iPoint, const std::vector< point_type > &points)
Insert a point into kd-tree.
Definition KDTree.h:119
value_type nearest(const point_type &point, const std::vector< point_type > &points) const
Query kd-tree for a nearest neighbor point.
Definition KDTree.h:188
KDTree(const point_type &min, const point_type &max)
Constructor with bounding box known in advance.
Definition KDTree.h:98
Namespace containing triangulation functionality.
IndexSizeType VertInd
Vertex index.
Definition CDTUtils.h:142
CDT_EXPORT T distanceSquared(const V2d< T > &a, const V2d< T > &b)
Squared distance between two 2D points.
Definition CDTUtils.hpp:308
Stores kd-tree node data.
Definition KDTree.h:59
point_data_vec data
points' data if leaf
Definition KDTree.h:61
bool isLeaf() const
Check if node is a leaf (has no valid children)
Definition KDTree.h:75
children_type children
two children if not leaf; {0,0} if leaf
Definition KDTree.h:60
void setChildren(const node_index c1, const node_index c2)
Children setter for convenience.
Definition KDTree.h:69
Node()
Create empty leaf.
Definition KDTree.h:63
2D vector
Definition CDTUtils.h:96
T y
Y-coordinate.
Definition CDTUtils.h:98
static V2d make(coord_type x, coord_type y)
Definition CDTUtils.hpp:23
T x
X-coordinate.
Definition CDTUtils.h:97