kd_tree.h

Go to the documentation of this file.

/*
  Copyright (C) 2018-2024 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
  {
    struct IndexDistance
    {
      size_t index;
      double distance;
    };


    struct IndexDistances
    {
      size_t min_index;
      double min_distance;
      std::vector<IndexDistance> vector;
    };

    struct Node
    {
      size_t index;
      double x;
      double y;

      Node(size_t index_, double x_, double y_)
        :
        index(index_),
        x(x_),
        y(y_)
      {}


      inline
      const double &operator[](const bool y_axis) const
      {
        WBAssert(std::fabs((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)=" << fabs((y_axis ? x : y) - *(&x+y_axis)));
        return *(&x+y_axis);
      }
    };

    class KDTree
    {
      public:
        KDTree() = default;
        KDTree(const std::vector<Node> point_list);

        void create_tree(const size_t left,
                         const size_t right,
                         const bool x_axis);

        const std::vector<Node> &get_nodes() const;

        IndexDistance find_closest_point(const Point<2> &check_point) const;



        IndexDistances find_closest_points(const Point<2> &check_point) const;


      private:
        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;


        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