File KDTree.h

File List > include > structure > KDTree.h

Go to the documentation of this file

#pragma once

#include <vector>

#include "../mesh/Vector3D.h"

namespace Argos {

class KDTree {
private:
  struct Node {
    size_t index; 
    int axis;     
    Node *left;   
    Node *right;  
  };

  const std::vector<Argos::Vector3D<double>> &m_points;
  std::vector<size_t> m_indices;
  Node *m_root;

  Node *build(size_t start, size_t end, int depth);
  void nearest(Node *node, const Argos::Vector3D<double> &query,
               double &bestDist) const;

  double getCoord(size_t idx, int axis) const;

  void free(Node *node);

public:
  KDTree(const std::vector<Argos::Vector3D<double>> &points);

  ~KDTree();

  double nearestDistance(const Argos::Vector3D<double> &query) const;
};

} // namespace Argos