37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_LINEAR_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_LINEAR_
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include "ompl/util/Exception.h"
73 virtual void add(
const _T &data)
75 data_.push_back(data);
78 virtual void add(
const std::vector<_T> &data)
81 data_.insert(
data_.end(), data.begin(), data.end());
84 virtual bool remove(
const _T &data)
87 for (
int i =
data_.size() - 1 ; i >= 0 ; --i)
98 const std::size_t sz =
data_.size();
101 for (std::size_t i = 0 ; i < sz ; ++i)
104 if (pos == sz || dmin > distance)
113 throw Exception(
"No elements found in nearest neighbors data structure");
116 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
121 std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
131 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
134 for (std::size_t i = 0 ; i <
data_.size() ; ++i)
136 nbh.push_back(
data_[i]);
140 virtual std::size_t
size(
void)
const
145 virtual void list(std::vector<_T> &data)
const
163 bool operator()(
const _T &a,
const _T &b)
const
165 return df_(a, e_) < df_(b, e_);