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");
117 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
122 std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
133 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
136 for (std::size_t i = 0 ; i <
data_.size() ; ++i)
138 nbh.push_back(
data_[i]);
142 virtual std::size_t
size(
void)
const
147 virtual void list(std::vector<_T> &data)
const
165 bool operator()(
const _T &a,
const _T &b)
const
167 return df_(a, e_) < df_(b, e_);
boost::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const
Return the nearest neighbors within distance radius in sorted order.
virtual void clear(void)
Clear the datastructure.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const
Return the k nearest neighbors in sorted order.
virtual void add(const std::vector< _T > &data)
Add a vector of points.
virtual void list(std::vector< _T > &data) const
Get all the elements in the datastructure.
A nearest neighbors datastructure that uses linear search.
DistanceFunction distFun_
The used distance function.
Abstract representation of a container that can perform nearest neighbors queries.
The exception type for ompl.
virtual std::size_t size(void) const
Get the number of elements in the datastructure.
virtual void add(const _T &data)
Add an element to the datastructure.
std::vector< _T > data_
The data elements stored in this structure.
virtual _T nearest(const _T &data) const
Get the nearest neighbor of a point.