#200 Fix KDTree nearest algorithm

Add regression test

Author: artem-ogre

CDT/include/KDTree.h

@@ -192,11 +192,10 @@ class KDTree
         value_type out;
         int iTask = -1;
         coord_type minDistSq = std::numeric_limits<coord_type>::max();
-        m_tasksStack[++iTask] =
-            NearestTask(m_root, m_min, m_max, m_rootDir, minDistSq);
+        m_tasksStack[++iTask] = NearestTask(m_root, m_min, m_max, m_rootDir);
         while(iTask != -1)
         {
-            const NearestTask t = m_tasksStack[iTask--];
+            const NearestTask& t = m_tasksStack[iTask--];
             if(t.distSq > minDistSq)
                 continue;
             const Node& n = m_nodes[t.node];
@@ -221,31 +220,31 @@ class KDTree
                 point_type newMin, newMax;
                 calcSplitInfo(t.min, t.max, t.dir, mid, newDir, newMin, newMax);
 
-                const coord_type distToMid = t.dir == NodeSplitDirection::X
-                                                 ? (point.x - mid)
-                                                 : (point.y - mid);
-                const coord_type toMidSq = distToMid * distToMid;
-
-                const std::size_t iChild = whichChild(point, mid, t.dir);
                 if(iTask + 2 >= static_cast<int>(m_tasksStack.size()))
                 {
                     m_tasksStack.resize(
                         m_tasksStack.size() + StackDepthIncrement);
                 }
-                // node containing point should end up on top of the stack
-                if(iChild == 0)
-                {
-                    m_tasksStack[++iTask] = NearestTask(
-                        n.children[1], newMin, t.max, newDir, toMidSq);
-                    m_tasksStack[++iTask] = NearestTask(
-                        n.children[0], t.min, newMax, newDir, toMidSq);
-                }
-                else
+
+                NearestTask closerTask(n.children[0], t.min, newMax, newDir);
+                NearestTask fartherTask(n.children[1], newMin, t.max, newDir);
+                if(isAfterSplit(point, mid, t.dir))
+                    std::swap(closerTask, fartherTask);
+
+                closerTask.calculateDistanceSquared(point);
+                if(closerTask.distSq <= minDistSq)
                 {
-                    m_tasksStack[++iTask] = NearestTask(
-                        n.children[0], t.min, newMax, newDir, toMidSq);
-                    m_tasksStack[++iTask] = NearestTask(
-                        n.children[1], newMin, t.max, newDir, toMidSq);
+                    fartherTask.calculateDistanceSquared(point);
+                    if(fartherTask.distSq <= minDistSq)
+                    {
+                        // put closest node on top of the stack
+                        m_tasksStack[++iTask] = fartherTask;
+                        m_tasksStack[++iTask] = closerTask;
+                    }
+                    else
+                    {
+                        m_tasksStack[++iTask] = closerTask;
+                    }
                 }
             }
         }
@@ -261,15 +260,22 @@ class KDTree
         return newNodeIndex;
     }
 
+    static bool isAfterSplit(
+        const point_type& point,
+        const coord_type& split,
+        const NodeSplitDirection::Enum dir)
+    {
+        return dir == NodeSplitDirection::X ? point.x > split : point.y > split;
+    }
+
     /// Test which child point belongs to after the split
     /// @returns 0 if first child, 1 if second child
-    std::size_t whichChild(
+    static std::size_t whichChild(
         const point_type& point,
         const coord_type& split,
-        const NodeSplitDirection::Enum dir) const
+        const NodeSplitDirection::Enum dir)
     {
-        return static_cast<size_t>(
-            dir == NodeSplitDirection::X ? point.x > split : point.y > split);
+        return isAfterSplit(point, split, dir);
     }
 
     /// Calculate split location, direction, and children boxes
@@ -389,17 +395,24 @@ class KDTree
             : dir(NodeSplitDirection::X)
         {}
         NearestTask(
-            const node_index node_,
-            const point_type& min_,
-            const point_type& max_,
-            const NodeSplitDirection::Enum dir_,
-            const coord_type distSq_)
-            : node(node_)
-            , min(min_)
-            , max(max_)
-            , dir(dir_)
-            , distSq(distSq_)
+            const node_index node,
+            const point_type& min,
+            const point_type& max,
+            const NodeSplitDirection::Enum dir)
+            : node(node)
+            , min(min)
+            , max(max)
+            , dir(dir)
+            , distSq(std::numeric_limits<coord_type>::max())
         {}
+        void calculateDistanceSquared(const point_type& p)
+        {
+            const coord_type dx =
+                std::max(std::max(min.x - p.x, coord_type(0)), p.x - max.x);
+            const coord_type dy =
+                std::max(std::max(min.y - p.y, coord_type(0)), p.y - max.y);
+            distSq = dx * dx + dy * dy;
+        }
     };
     // allocated in class (not in the 'nearest' method) for better performance
     mutable std::vector<NearestTask> m_tasksStack;

CDT/src/CDT.cpp

@@ -65,6 +65,16 @@ template CDT_EXPORT void initializeWithRegularGrid<double>(
     std::size_t,
     Triangulation<double>&);
 
+template
+CDT_EXPORT float distance(const V2d<float>& a, const V2d<float>& b);
+template
+CDT_EXPORT double distance(const V2d<double>& a, const V2d<double>& b);
+
+template
+CDT_EXPORT float distanceSquared(const V2d<float>& a, const V2d<float>& b);
+template
+CDT_EXPORT double distanceSquared(const V2d<double>& a, const V2d<double>& b);
+
 } // namespace CDT
 
 #endif

CDT/tests/cdt.test.cpp

@@ -1113,4 +1113,29 @@ TEST_CASE("Callbacks test: test aborting the calculation")
         }
     }
 }
-#endif
+
+#endif
+
+TEST_CASE("KDtree regression (#200)")
+{
+    const auto target = V2d<double>(153.63, -30.09);
+    const auto points = std::vector<V2d<double> >{
+        V2d<double>(168.67, -122.39), // approx distance to target: 93.52
+        V2d<double>(-25.12, 109.06),  // approx distance to target: 226.53
+        V2d<double>(178.36, 40.75),   // approx distance to target: 75.03
+        V2d<double>(161.07, 86.2),    // approx distance to target: 116.53
+    };
+
+    auto tree = KDTree::KDTree<double, 1, 32, 32>(
+        V2d<double>(-180, -180), V2d<double>(180, 180));
+    double min_distance = std::numeric_limits<double>::max();
+    for(VertInd i(0); i < points.size(); ++i)
+    {
+        tree.insert(i, points);
+        min_distance = std::min(min_distance, distance(target, points[i]));
+    }
+
+    const auto [matchVec, matchIdx] = tree.nearest(target, points);
+    REQUIRE(min_distance == distance(target, points[matchIdx]));
+}
+