Merge pull request #394 from MFraters/add_kd-tree

Add kd tree as first step to variable plate depth

include/world_builder/kd_tree.h

@@ -0,0 +1,156 @@
+/*
+  Copyright (C) 2022 by the authors of the World Builder code.
+
+  This file is part of the World Builder.
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU Lesser General Public License as published
+   by the Free Software Foundation, either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU Lesser General Public License for more details.
+
+   You should have received a copy of the GNU Lesser General Public License
+   along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef KD_TREE_H
+#define KD_TREE_H
+
+#include <vector>
+#include <cstddef>
+#include <iostream>
+#include <limits>
+
+#include "world_builder/assert.h"
+#include "world_builder/point.h"
+
+namespace WorldBuilder
+{
+  namespace KDTree
+  {
+    /**
+     * A struct to store an index for a point and the
+     * distance to that point.
+     */
+    struct IndexDistance
+    {
+      size_t index;
+      double distance;
+    };
+
+
+    /**
+     * A struct to store an index for a point and the
+     * distance to that point.
+     */
+    struct IndexDistances
+    {
+      size_t min_index;
+      double min_distance;
+      std::vector<IndexDistance> vector;
+    };
+
+    /**
+     * Each item contains an index and an x and y coordinate
+     */
+    struct Node
+    {
+      size_t index;
+      double x;
+      double y;
+
+      Node(size_t index_, double x_, double y_)
+        :
+        index(index_),
+        x(x_),
+        y(y_)
+      {}
+
+
+      /**
+       * access index (const)
+       */
+      inline
+      const double &operator[](const bool y_axis) const
+      {
+        WBAssert(abs((y_axis ? y : x) - *(&x+y_axis)) < std::numeric_limits<double>::epsilon(),
+                 "Internal error: y_axis=" << y_axis << ", x=" << x << ", y=" << y <<", *(&x+y_axis)=" << *(&x+y_axis) << ", ((bool)y_axis ? x : y) - *(&x+y_axis)=" << abs(((bool)y_axis ? x : y) - *(&x+y_axis)));
+        return *(&x+y_axis);
+      }
+    };
+
+    class KDTree
+    {
+      public:
+        /**
+         * Constructor. Requires a list of Nodes.
+         */
+        KDTree(const std::vector<Node> point_list);
+
+        /**
+         * Create a tree based on the current information in the
+         * node vector.
+         */
+        void create_tree(const size_t left,
+                         const size_t right,
+                         const bool x_axis);
+
+        /**
+         * Retun a reference to the vector containing the nodes.
+         */
+        std::vector<Node> &get_nodes();
+
+        /**
+         * Returns the index of the closest point and the distance
+         * of that point to the check point.
+         */
+        IndexDistance find_closest_point(const Point<2> &check_point) const;
+
+
+
+        /**
+         * Returns the index of the closest point and the distance
+         * of that point to the check point. Stores the points searched
+         * through in a unsorted vector.
+         * Note: I can only guarentee that the point with the least distance
+         * is the closest point. The point in the list with the second/third/etc.
+         * smallest distance may or may not actually be the global point
+         * with the second/third/etc. smallest distance. It will most likely be a
+         * very good guess though.
+         */
+        IndexDistances find_closest_points(const Point<2> &check_point) const;
+
+
+      private:
+        /**
+         * Returns the index of the closest point and the distance
+         * of that point to the check point. This function is used
+         * by find_closest_point to find the correct point.
+         */
+        void find_closest_point_recursive(const Point<2> &check_point,
+                                          const size_t left,
+                                          const size_t right,
+                                          const bool y_axis,
+                                          IndexDistance &index_distance) const;
+
+
+        /**
+         * Returns the index of the closest point and the distance
+         * of that point to the check point. This function is used
+         * by find_closest_point to find the correct point.
+         */
+        void find_closest_points_recursive(const Point<2> &check_point,
+                                           const size_t left,
+                                           const size_t right,
+                                           const bool y_axis,
+                                           IndexDistances &index_distances) const;
+
+        std::vector<Node> nodes;
+    };
+  }
+}
+#endif

source/kd_tree.cc

@@ -0,0 +1,207 @@
+/*
+  Copyright (C) 2022 by the authors of the World Builder code.
+
+  This file is part of the World Builder.
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU Lesser General Public License as published
+   by the Free Software Foundation, either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU Lesser General Public License for more details.
+
+   You should have received a copy of the GNU Lesser General Public License
+   along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+#include "world_builder/kd_tree.h"
+#include <algorithm>
+
+namespace WorldBuilder
+{
+  namespace KDTree
+  {
+    KDTree::KDTree(std::vector<Node> point_list)
+      : nodes(point_list)
+    {}
+
+
+    void
+    KDTree::create_tree(const size_t left,
+                        const size_t right,
+                        const bool y_axis)
+    {
+      size_t mid=(left+right)>>1;
+      std::nth_element(nodes.begin()+left,nodes.begin()+mid,nodes.begin()+right+1,
+                       [y_axis](Node& i, Node& j) -> bool
+      {
+        return i[y_axis] < j[y_axis];
+      });
+
+      if (left<mid)
+        create_tree(left,mid-1,!y_axis);
+      if (right>mid)
+        create_tree(mid+1,right,!y_axis);
+    }
+
+
+    std::vector<Node> &
+    KDTree::get_nodes()
+    {
+      return nodes;
+    }
+
+
+    IndexDistance
+    KDTree::find_closest_point(const Point<2> &check_point) const
+    {
+      size_t start_node_index = 0;
+      IndexDistance index_distance = {start_node_index,
+                                      std::numeric_limits<double>::max()
+                                     };
+
+      find_closest_point_recursive(check_point,0,nodes.size()-1,false,index_distance);
+
+      return index_distance;
+    }
+
+
+    void
+    KDTree::find_closest_point_recursive(const Point<2> &check_point,
+                                         const size_t left,
+                                         const size_t right,
+                                         const bool y_axis,
+                                         IndexDistance &index_distance) const
+    {
+      // Calculate the index of this node
+      const size_t mid=(left+right)>>1;
+      if (check_point[y_axis] < nodes[mid][y_axis])
+        {
+          // Traverse left child
+          if (left<mid)
+            {
+              find_closest_point_recursive(check_point,left,mid-1,!y_axis,index_distance);
+            }
+
+          // Compare node's point to current closest point
+          double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0])
+                                 +(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1]));
+          if (index_distance.distance > distance)
+            {
+              index_distance.index = mid;
+              index_distance.distance = distance;
+            }
+
+          // Traverse right child
+          if (right>mid)
+            {
+              if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distance.distance)
+                {
+                  find_closest_point_recursive(check_point,mid+1,right,!y_axis,index_distance);
+                }
+            }
+        }
+      else
+        {
+          // Traverse right child
+          if (right>mid)
+            find_closest_point_recursive(check_point,mid+1,right,!y_axis,index_distance);
+
+          // Compare node's point to current closest point
+          double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0])
+                                 +(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1]));
+          if (index_distance.distance > distance)
+            {
+              index_distance.index = mid;
+              index_distance.distance = distance;
+            }
+
+          // Traverse left child
+          if (left<mid)
+            if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distance.distance)
+              find_closest_point_recursive(check_point,left,mid-1,!y_axis,index_distance);
+        }
+    }
+
+
+
+    IndexDistances
+    KDTree::find_closest_points(const Point<2> &check_point) const
+    {
+      size_t start_node_index = 0;
+      IndexDistances index_distances = {start_node_index,
+                                        std::numeric_limits<double>::max(),
+                                        {}
+                                       };
+
+      find_closest_points_recursive(check_point,0,nodes.size()-1,false,index_distances);
+
+      return index_distances;
+    }
+
+
+    void
+    KDTree::find_closest_points_recursive(const Point<2> &check_point,
+                                          const size_t left,
+                                          const size_t right,
+                                          const bool y_axis,
+                                          IndexDistances &index_distances) const
+    {
+      // Calculate the index of this node
+      const size_t mid=(left+right)>>1;
+      if (check_point[y_axis] < nodes[mid][y_axis])
+        {
+          // Traverse left child
+          if (left<mid)
+            {
+              find_closest_points_recursive(check_point,left,mid-1,!y_axis,index_distances);
+            }
+
+          // Compare node's point to current closest point
+          double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0])
+                                 +(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1]));
+          if (index_distances.min_distance > distance)
+            {
+              index_distances.min_index = mid;
+              index_distances.min_distance = distance;
+            }
+
+          index_distances.vector.push_back({mid, distance});
+
+          // Traverse right child
+          if (right>mid)
+            {
+              if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distances.min_distance)
+                {
+                  find_closest_points_recursive(check_point,mid+1,right,!y_axis,index_distances);
+                }
+            }
+        }
+      else
+        {
+          // Traverse right child
+          if (right>mid)
+            find_closest_points_recursive(check_point,mid+1,right,!y_axis,index_distances);
+
+          // Compare node's point to current closest point
+          double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0])
+                                 +(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1]));
+          if (index_distances.min_distance > distance)
+            {
+              index_distances.min_index = mid;
+              index_distances.min_distance = distance;
+            }
+
+          index_distances.vector.push_back({mid, distance});
+
+          // Traverse left child
+          if (left<mid)
+            if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distances.min_distance)
+              find_closest_points_recursive(check_point,left,mid-1,!y_axis,index_distances);
+        }
+    }
+  }
+}