Merge pull request #4 from naviqore/feature/NAV-88-KD-Tree-locations

Feature/nav 88 kd tree locations

CMakeLists.txt

@@ -46,6 +46,7 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE ${CMAKE_SOURCE_DIR}/output/lib/releas
 add_subdirectory(interfaces)
 add_subdirectory(logging)
 add_subdirectory(geometry)
+add_subdirectory(geometry/test)
 add_subdirectory(schedule)
 add_subdirectory(schedule/test)
 add_subdirectory(schedule/benchmarks)

geometry/CMakeLists.txt

@@ -1,36 +1,14 @@
 project(geometry)
 
-add_library(${PROJECT_NAME} SHARED)
-
-target_sources(${PROJECT_NAME} PRIVATE
-        src/CoordinateComponent.h
-        src/K2DTree.cpp
-        src/K2DTree.h
-        src/Coordinate.h
-)
-
-set(CMAKE_CXX_VISIBILITY_PRESET hidden)
-set(CMAKE_VISIBILITY_INLINES_HIDDEN 1)
-
-include(GenerateExportHeader)
-generate_export_header(${PROJECT_NAME}
-        BASE_NAME ${PROJECT_NAME}
-        EXPORT_MACRO_NAME GEOMETRY_API
-        EXPORT_FILE_NAME ${PROJECT_NAME}_export.h
-        STATIC_DEFINE ${PROJECT_NAME}_BUILT_AS_STATIC
-)
-
-target_include_directories(${PROJECT_NAME} PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
-)
-
-set(LIBRARY_INSTALL_DIR lib)
-set(INCLUDE_INSTALL_DIR include)
-
-install(TARGETS ${PROJECT_NAME} DESTINATION ${LIBRARY_INSTALL_DIR})
-install(FILES
-        src/CoordinateComponent.h
-        src/Coordinate.h
-        ${PROJECT_BINARY_DIR}/${PROJECT_NAME}_export.h DESTINATION ${INCLUDE_INSTALL_DIR}
-)
+add_library(${PROJECT_NAME} INTERFACE)
+
+target_sources(${PROJECT_NAME} INTERFACE
+        include/spatial/CoordinateComponent.h
+        include/spatial/index/K2DTree.h
+        include/spatial/Coordinate.h
+        include/utils/utils.h
+        include/utils/usings.h)
+
+target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include/spatial/index)
+target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include/spatial)
+target_include_directories(${PROJECT_NAME} INTERFACE include/utils)

geometry/src/Coordinate.h → geometry/include/spatial/Coordinate.h

@@ -5,13 +5,17 @@
 #ifndef COORDINATE_H
 #define COORDINATE_H
 
+#include <type_traits>
+#include <concepts>
+
 namespace geometry {
 
   template<typename T>
     requires std::convertible_to<T, double>
   class Coordinate
   {
     T value;
+
   public:
     explicit Coordinate(const T value)
       : value(value) {}

geometry/src/CoordinateComponent.h → geometry/include/spatial/CoordinateComponent.h

@@ -10,19 +10,19 @@
 
 
 template<typename T>
-concept CoordinateComponentConcept = requires(T a) {
+concept CoordinateItemConcept = requires(T a) {
   { a.getValue() } -> std::convertible_to<double>;
 };
 
 template<typename T>
-concept PointConcept = requires(T a) {
+concept CoordinateComponentConcept = requires(T a) {
   { a.getFirstCoordinate() } -> std::convertible_to<double>;
   { a.getSecondCoordinate() } -> std::convertible_to<double>;
 };
 
 namespace geometry {
 
-  template<CoordinateComponentConcept T>
+  template<CoordinateItemConcept T>
   class CoordinateComponent
   {
     T firstCoordinate;
@@ -68,12 +68,12 @@ namespace geometry {
 
     CoordinateComponent operator*(const double scalar) {
       return CoordinateComponent(getFirstCoordinate() * scalar,
-                   getSecondCoordinate() * scalar);
+                                 getSecondCoordinate() * scalar);
     }
 
     CoordinateComponent operator/(const double scalar) {
       return CoordinateComponent(getFirstCoordinate() / scalar,
-                   getSecondCoordinate() / scalar);
+                                 getSecondCoordinate() / scalar);
     }
 
     double distanceTo(const CoordinateComponent& other) {

geometry/include/spatial/index/K2DTree.h

@@ -0,0 +1,161 @@
+//
+// Created by MichaelBrunner on 07/06/2024.
+//
+
+#ifndef K2DTREE_H
+#define K2DTREE_H
+
+#include <vector>
+#include <algorithm>
+#include <memory>
+#include <concepts>
+#include <functional>
+#include <CoordinateComponent.h>
+#include <utils.h>
+
+#include <stdexcept>
+
+namespace geometry::kd_tree {
+  // https://medium.com/smucs/a-look-into-k-dimensional-trees-290ec69dffe9#:~:text=What%20is%20a%20K%2DDimensional,in%20the%20multi%20dimensional%20space.
+  template<CoordinateComponentConcept T>
+  class K2DTree
+  {
+    struct Node
+    {
+      T point;
+      std::shared_ptr<Node> left{nullptr};
+      std::shared_ptr<Node> right{nullptr};
+
+      explicit Node(const T& point)
+        : point(point) {}
+    };
+
+    std::shared_ptr<Node> root;
+    const int kDimensions = 2; // 2D tree
+    std::function<int(int, int)> axisFunc = [](const int depth, const int kDimension) { return depth % kDimension; };
+
+    std::shared_ptr<Node> buildTree(std::vector<T>& points, const int depth = 0) {
+      if (points.empty())
+      {
+        // base case
+        return nullptr;
+      }
+      int axis = axisFunc(depth, kDimensions);
+
+      std::ranges::sort(points, [axis](CoordinateComponentConcept auto& a, CoordinateComponentConcept auto& b) {
+        switch (axis)
+        {
+          case 0: // x-axis
+            return a.getFirstCoordinate() < b.getFirstCoordinate();
+          case 1: //  y-axis
+            return a.getSecondCoordinate() < b.getSecondCoordinate();
+          default:
+            return false;
+        }
+      });
+
+      const auto median = points.size() / 2;
+      auto node = std::make_shared<Node>(points[median]);
+
+      std::vector<T> left_points(points.begin(), points.begin() + median);
+      std::vector<T> right_points(points.begin() + median + 1, points.end());
+
+      node->left = buildTree(left_points, depth + 1);
+      node->right = buildTree(right_points, depth + 1);
+
+      return node;
+    }
+
+  public:
+    explicit K2DTree(std::vector<T> points) {
+      root = buildTree(points);
+    }
+
+    K2DTree(std::initializer_list<T> points) {
+      std::vector<T> pointsVector(points);
+      root = buildTree(pointsVector);
+    }
+
+    void nearest(const std::shared_ptr<Node>& node, const T& point, const int depth, std::shared_ptr<Node>& best, double& bestDist) {
+      if (nullptr == node)
+      {
+        return;
+      }
+
+      if (const double distance = utils::haversineDistance(node->point, point);
+          distance < bestDist)
+      {
+        bestDist = distance;
+        best = node;
+      }
+
+      int axis = axisFunc(depth, kDimensions);
+      const double difference = utils::getCoordinateComponent(point, axis) - utils::getCoordinateComponent(node->point, axis);
+
+      std::shared_ptr<Node> near, far;
+      if (difference <= 0)
+      {
+        near = node->left;
+        far = node->right;
+      }
+      else
+      {
+        near = node->right;
+        far = node->left;
+      }
+
+      nearest(near, point, depth + 1, best, bestDist);
+
+      //
+      if (difference * difference < bestDist)
+      {
+        nearest(far, point, depth + 1, best, bestDist);
+      }
+    }
+
+    T nearest(const T& point) {
+      std::shared_ptr<Node> best = nullptr;
+      double bestDist = std::numeric_limits<double>::infinity(); // start with INFINITY
+      nearest(root, point, 0, best, bestDist);
+      if (nullptr == best)
+      {
+        throw std::runtime_error("No nearest point found");
+      }
+      return best->point;
+    }
+
+    void rangeSearch(const std::shared_ptr<Node>& node, const T& point, const double range, const int depth, std::vector<T>& results) {
+      if (nullptr == node)
+      {
+        return;
+      }
+
+      if (const double distance = utils::haversineDistance(node->point, point);
+          distance <= range)
+      {
+        results.push_back(node->point);
+      }
+
+      int axis = axisFunc(depth, kDimensions);
+      const double difference = utils::getCoordinateComponent(point, axis) - utils::getCoordinateComponent(node->point, axis);
+
+      if (difference <= range)
+      {
+        rangeSearch(node->left, point, range, depth + 1, results);
+      }
+      if (difference >= -range)
+      {
+        rangeSearch(node->right, point, range, depth + 1, results);
+      }
+    }
+
+    std::vector<T> rangeSearch(const T& point, const double range) {
+      std::vector<T> results;
+      rangeSearch(root, point, range, 0, results);
+      return results;
+    }
+  };
+
+} // namespace geometry
+
+#endif //K2DTREE_H

geometry/include/utils/usings.h

@@ -0,0 +1,19 @@
+//
+// Created by MichaelBrunner on 20/06/2024.
+//
+
+#ifndef USINGS_H
+#define USINGS_H
+
+#include <K2DTree.h>
+#include <CoordinateComponent.h>
+#include <Coordinate.h>
+
+namespace geometry::kd_tree {
+  using spatialCoordinate = Coordinate<double>;
+  using coordinateComponent = CoordinateComponent<spatialCoordinate>;
+  using spatialK2dTree = K2DTree<coordinateComponent>;
+}
+
+
+#endif //USINGS_H

geometry/include/utils/utils.h

@@ -0,0 +1,76 @@
+//
+// Created by MichaelBrunner on 20/06/2024.
+//
+
+#pragma once
+
+#include <cmath>
+#include <stdexcept>
+#include <numbers>
+#include <../../include/spatial/CoordinateComponent.h>
+
+constexpr long double operator"" _km(const long double value) {
+  return value;
+}
+
+
+namespace geometry::utils {
+
+  double distance(const CoordinateComponentConcept auto& a, const CoordinateComponentConcept auto& b) {
+    const double dx = a.getFirstCoordinate() - b.getFirstCoordinate();
+    const double dy = a.getSecondCoordinate() - b.getSecondCoordinate();
+    return std::sqrt(dx * dx + dy * dy);
+  }
+
+  template<CoordinateComponentConcept T>
+  double getCoordinateComponent(const T& point, const int axis) {
+    if (axis == 0)
+    {
+      return point.getFirstCoordinate();
+    }
+    if (axis == 1)
+    {
+      return point.getSecondCoordinate();
+    }
+    throw std::invalid_argument("Invalid axis value");
+  }
+
+  double haversineDistance(const CoordinateComponentConcept auto& aFirstPoint, const CoordinateComponentConcept auto& aSecondPoint) {
+    constexpr double kEarthRadiusKilometers = 6'371.00_km;
+    auto toRadians = [](const double degrees) { return degrees * std::numbers::pi / 180.0; };
+
+    const double lat1 = toRadians(aFirstPoint.getFirstCoordinate());
+    const double lon1 = toRadians(aFirstPoint.getSecondCoordinate());
+
+    const double lat2 = toRadians(aSecondPoint.getFirstCoordinate());
+    const double lon2 = toRadians(aSecondPoint.getSecondCoordinate());
+
+    const double a = std::sin((lat2 - lat1) / 2) * std::sin((lat2 - lat1) / 2) + std::cos(lat1) * std::cos(lat2) * std::sin((lon2 - lon1) / 2) * std::sin((lon2 - lon1) / 2);
+
+    const double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
+
+    return kEarthRadiusKilometers * c;
+  }
+
+  template<CoordinateItemConcept T>
+  struct CoordinateComponentHash
+  {
+    size_t operator()(const CoordinateComponent<T>& k) const {
+      constexpr std::hash<double> hash;
+      return hash(k.getFirstCoordinate()) ^ hash(k.getSecondCoordinate());
+    }
+  };
+
+
+  template<CoordinateItemConcept T>
+  struct CoordinateComponentEqual
+  {
+    bool operator()(const CoordinateComponent<T>& p1, const CoordinateComponent<T>& p2) const {
+      return std::make_tuple(p1.getFirstCoordinate(), p1.getSecondCoordinate())
+             == std::make_tuple(p2.getFirstCoordinate(), p2.getSecondCoordinate());
+    }
+  };
+
+
+
+}