Skip to content

Commit e88838f

Browse files
artem-ogreartem-ogre
authored
#200 Fix KDTree (#203)
* #200 Suppress warnings * #200 Fix KDTree nearest algorithm Add regression test * #200 Make more efficient distance calculations * #200 Fix KDTree bug and add small stress test
1 parent 81d3109 commit e88838f

File tree

5 files changed

+233
-47
lines changed

5 files changed

+233
-47
lines changed

‎.clang-format‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,8 @@ UseTab: Never
3333
# Clang 5 new features:
3434
# =====================
3535
BreakBeforeBraces: Custom
36-
BraceWrapping:
36+
BraceWrapping:
37+
AfterCaseLabel: true
3738
AfterClass: true
3839
AfterControlStatement: true
3940
AfterEnum : true

‎CDT/include/KDTree.h‎

Lines changed: 100 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99

1010
#include "CDTUtils.h"
1111

12-
#include <cassert>
1312
#include <limits>
1413

1514
namespace CDT
@@ -192,8 +191,12 @@ class KDTree
192191
value_type out;
193192
int iTask = -1;
194193
coord_type minDistSq = std::numeric_limits<coord_type>::max();
195-
m_tasksStack[++iTask] =
196-
NearestTask(m_root, m_min, m_max, m_rootDir, minDistSq);
194+
m_tasksStack[++iTask] = NearestTask(
195+
m_root,
196+
m_min,
197+
m_max,
198+
m_rootDir,
199+
distanceSquaredToBox(point, m_min, m_max));
197200
while(iTask != -1)
198201
{
199202
const NearestTask t = m_tasksStack[iTask--];
@@ -217,35 +220,65 @@ class KDTree
217220
else
218221
{
219222
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;
223+
NodeSplitDirection::Enum newDir(NodeSplitDirection::X);
224+
point_type newMin = t.min, newMax = t.max;
225+
coord_type dSqFarther = std::numeric_limits<coord_type>::max();
226+
switch(t.dir)
227+
{
228+
case NodeSplitDirection::X:
229+
{
230+
mid = (t.min.x + t.max.x) / coord_type(2);
231+
newDir = NodeSplitDirection::Y;
232+
newMin.x = mid;
233+
newMax.x = mid;
234+
const coord_type dx = point.x - mid;
235+
const coord_type dy = std::max(
236+
std::max(t.min.y - point.y, coord_type(0)),
237+
point.y - t.max.y);
238+
dSqFarther = dx * dx + dy * dy;
239+
break;
240+
}
241+
case NodeSplitDirection::Y:
242+
{
243+
mid = (t.min.y + t.max.y) / coord_type(2);
244+
newDir = NodeSplitDirection::X;
245+
newMin.y = mid;
246+
newMax.y = mid;
247+
const coord_type dx = std::max(
248+
std::max(t.min.x - point.x, coord_type(0)),
249+
point.x - t.max.x);
250+
const coord_type dy = point.y - mid;
251+
dSqFarther = dx * dx + dy * dy;
252+
break;
253+
}
254+
}
228255

229-
const std::size_t iChild = whichChild(point, mid, t.dir);
230256
if(iTask + 2 >= static_cast<int>(m_tasksStack.size()))
231257
{
232258
m_tasksStack.resize(
233259
m_tasksStack.size() + StackDepthIncrement);
234260
}
235-
// node containing point should end up on top of the stack
236-
if(iChild == 0)
261+
262+
// put the closest node on top of the stack
263+
if(isAfterSplit(point, mid, t.dir))
237264
{
265+
if(dSqFarther <= minDistSq)
266+
{
267+
m_tasksStack[++iTask] = NearestTask(
268+
n.children[0], t.min, newMax, newDir, dSqFarther);
269+
}
238270
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);
271+
n.children[1], newMin, t.max, newDir, t.distSq);
242272
}
243273
else
244274
{
275+
if(dSqFarther <= minDistSq)
276+
{
277+
m_tasksStack[++iTask] = NearestTask(
278+
n.children[1], newMin, t.max, newDir, dSqFarther);
279+
}
245280
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);
281+
n.children[0], t.min, newMax, newDir, t.distSq);
249282
}
250283
}
251284
}
@@ -261,15 +294,22 @@ class KDTree
261294
return newNodeIndex;
262295
}
263296

297+
static bool isAfterSplit(
298+
const point_type& point,
299+
const coord_type& split,
300+
const NodeSplitDirection::Enum dir)
301+
{
302+
return dir == NodeSplitDirection::X ? point.x > split : point.y > split;
303+
}
304+
264305
/// Test which child point belongs to after the split
265306
/// @returns 0 if first child, 1 if second child
266-
std::size_t whichChild(
307+
static std::size_t whichChild(
267308
const point_type& point,
268309
const coord_type& split,
269-
const NodeSplitDirection::Enum dir) const
310+
const NodeSplitDirection::Enum dir)
270311
{
271-
return static_cast<size_t>(
272-
dir == NodeSplitDirection::X ? point.x > split : point.y > split);
312+
return isAfterSplit(point, split, dir);
273313
}
274314

275315
/// Calculate split location, direction, and children boxes
@@ -320,21 +360,29 @@ class KDTree
320360
{
321361
case NodeSplitDirection::X:
322362
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);
325363
if(point.y < m_min.y)
364+
{
326365
m_min.y -= m_max.y - m_min.y;
327-
else if(point.y > m_max.y)
366+
m_nodes[newRoot].setChildren(newLeaf, m_root);
367+
}
368+
else
369+
{
328370
m_max.y += m_max.y - m_min.y;
371+
m_nodes[newRoot].setChildren(m_root, newLeaf);
372+
}
329373
break;
330374
case NodeSplitDirection::Y:
331375
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);
334376
if(point.x < m_min.x)
377+
{
335378
m_min.x -= m_max.x - m_min.x;
336-
else if(point.x > m_max.x)
379+
m_nodes[newRoot].setChildren(newLeaf, m_root);
380+
}
381+
else
382+
{
337383
m_max.x += m_max.x - m_min.x;
384+
m_nodes[newRoot].setChildren(m_root, newLeaf);
385+
}
338386
break;
339387
}
340388
m_root = newRoot;
@@ -368,6 +416,18 @@ class KDTree
368416
m_isRootBoxInitialized = true;
369417
}
370418

419+
static coord_type distanceSquaredToBox(
420+
const point_type& p,
421+
const point_type& min,
422+
const point_type& max)
423+
{
424+
const coord_type dx =
425+
std::max(std::max(min.x - p.x, coord_type(0)), p.x - max.x);
426+
const coord_type dy =
427+
std::max(std::max(min.y - p.y, coord_type(0)), p.y - max.y);
428+
return dx * dx + dy * dy;
429+
}
430+
371431
private:
372432
node_index m_root;
373433
std::vector<Node> m_nodes;
@@ -389,16 +449,16 @@ class KDTree
389449
: dir(NodeSplitDirection::X)
390450
{}
391451
NearestTask(
392-
const node_index node_,
393-
const point_type& min_,
394-
const point_type& max_,
395-
const NodeSplitDirection::Enum dir_,
396-
const coord_type distSq_)
397-
: node(node_)
398-
, min(min_)
399-
, max(max_)
400-
, dir(dir_)
401-
, distSq(distSq_)
452+
const node_index node,
453+
const point_type& min,
454+
const point_type& max,
455+
const NodeSplitDirection::Enum dir,
456+
const coord_type distSq = std::numeric_limits<coord_type>::max())
457+
: node(node)
458+
, min(min)
459+
, max(max)
460+
, dir(dir)
461+
, distSq(distSq)
402462
{}
403463
};
404464
// allocated in class (not in the 'nearest' method) for better performance

‎CDT/include/Triangulation.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -289,6 +289,11 @@ struct CDT_EXPORT TriangleChangeType
289289
};
290290
};
291291

292+
// parameter names are used for documentation purposes, even if they are un-used
293+
// in the interface's default implementation
294+
#pragma GCC diagnostic push
295+
#pragma GCC diagnostic ignored "-Wunused-parameter"
296+
292297
/**
293298
* Interface for the callback handler that user can derive from and inject into
294299
* the triangulation to monitor certain events or order aborting the calculation
@@ -384,6 +389,10 @@ class CDT_EXPORT ICallbackHandler
384389
};
385390
};
386391

392+
// parameter names are used for documentation purposes, even if they are
393+
// un-used in the interface's default implementation
394+
#pragma GCC diagnostic pop
395+
387396
#endif
388397

389398
/**

‎CDT/src/CDT.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,16 @@ template CDT_EXPORT void initializeWithRegularGrid<double>(
6565
std::size_t,
6666
Triangulation<double>&);
6767

68+
template
69+
CDT_EXPORT float distance(const V2d<float>& a, const V2d<float>& b);
70+
template
71+
CDT_EXPORT double distance(const V2d<double>& a, const V2d<double>& b);
72+
73+
template
74+
CDT_EXPORT float distanceSquared(const V2d<float>& a, const V2d<float>& b);
75+
template
76+
CDT_EXPORT double distanceSquared(const V2d<double>& a, const V2d<double>& b);
77+
6878
} // namespace CDT
6979

7080
#endif

0 commit comments

Comments
 (0)