00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_SQRT_APPROX_
00038 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_SQRT_APPROX_
00039
00040 #include "ompl/datastructures/NearestNeighborsLinear.h"
00041 #include <algorithm>
00042 #include <cmath>
00043
00044 namespace ompl
00045 {
00056 template<typename _T>
00057 class NearestNeighborsSqrtApprox : public NearestNeighborsLinear<_T>
00058 {
00059 public:
00060 NearestNeighborsSqrtApprox(void) : NearestNeighborsLinear<_T>(), checks_(0), offset_(0)
00061 {
00062 }
00063
00064 virtual ~NearestNeighborsSqrtApprox(void)
00065 {
00066 }
00067
00068 virtual void clear(void)
00069 {
00070 NearestNeighborsLinear<_T>::clear();
00071 checks_ = 0;
00072 offset_ = 0;
00073 }
00074
00075 virtual void add(const _T &data)
00076 {
00077 NearestNeighborsLinear<_T>::add(data);
00078 updateCheckCount();
00079 }
00080
00081 virtual void add(const std::vector<_T> &data)
00082 {
00083 NearestNeighborsLinear<_T>::add(data);
00084 updateCheckCount();
00085 }
00086
00087 virtual bool remove(const _T &data)
00088 {
00089 bool result = NearestNeighborsLinear<_T>::remove(data);
00090 if (result)
00091 updateCheckCount();
00092 return result;
00093 }
00094
00095 virtual _T nearest(const _T &data) const
00096 {
00097 const std::size_t n = NearestNeighborsLinear<_T>::data_.size();
00098 std::size_t pos = n;
00099
00100 if (checks_ > 0 && n > 0)
00101 {
00102 double dmin = 0.0;
00103 for (std::size_t j = 0 ; j < checks_ ; ++j)
00104 {
00105 std::size_t i = (j * checks_ + offset_) % n;
00106
00107 double distance = NearestNeighbors<_T>::distFun_(NearestNeighborsLinear<_T>::data_[i], data);
00108 if (pos == n || dmin > distance)
00109 {
00110 pos = i;
00111 dmin = distance;
00112 }
00113 }
00114 offset_ = (offset_ + 1) % checks_;
00115 }
00116 if (pos != n)
00117 return NearestNeighborsLinear<_T>::data_[pos];
00118
00119 throw Exception("No elements found");
00120 }
00121
00122 protected:
00123
00125 inline void updateCheckCount(void)
00126 {
00127 checks_ = 1 + (std::size_t)floor(sqrt((double)NearestNeighborsLinear<_T>::data_.size()));
00128 }
00129
00131 std::size_t checks_;
00132
00134 mutable std::size_t offset_;
00135
00136 };
00137
00138 }
00139
00140 #endif