All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
NearestNeighborsLinear.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_LINEAR_
00038 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_LINEAR_
00039 
00040 #include "ompl/datastructures/NearestNeighbors.h"
00041 #include "ompl/util/Exception.h"
00042 #include <algorithm>
00043 
00044 namespace ompl
00045 {
00046 
00056     template<typename _T>
00057     class NearestNeighborsLinear : public NearestNeighbors<_T>
00058     {
00059     public:
00060         NearestNeighborsLinear(void) : NearestNeighbors<_T>()
00061         {
00062         }
00063 
00064         virtual ~NearestNeighborsLinear(void)
00065         {
00066         }
00067 
00068         virtual void clear(void)
00069         {
00070             data_.clear();
00071         }
00072 
00073         virtual void add(const _T &data)
00074         {
00075             data_.push_back(data);
00076         }
00077 
00078         virtual void add(const std::vector<_T> &data)
00079         {
00080             data_.reserve(data_.size() + data.size());
00081             data_.insert(data_.end(), data.begin(), data.end());
00082         }
00083 
00084         virtual bool remove(const _T &data)
00085         {
00086             if (!data_.empty())
00087                 for (int i = data_.size() - 1 ; i >= 0 ; --i)
00088                     if (data_[i] == data)
00089                     {
00090                         data_.erase(data_.begin() + i);
00091                         return true;
00092                     }
00093             return false;
00094         }
00095 
00096         virtual _T nearest(const _T &data) const
00097         {
00098             const std::size_t sz = data_.size();
00099             std::size_t pos = sz;
00100             double dmin = 0.0;
00101             for (std::size_t i = 0 ; i < sz ; ++i)
00102             {
00103                 double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
00104                 if (pos == sz || dmin > distance)
00105                 {
00106                     pos = i;
00107                     dmin = distance;
00108                 }
00109             }
00110             if (pos != sz)
00111                 return data_[pos];
00112 
00113             throw Exception("No elements found");
00114         }
00115 
00116         virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
00117         {
00118             nbh = data_;
00119             if (nbh.size() > k)
00120             {
00121                 std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
00122                                   ElemSort(data, NearestNeighbors<_T>::distFun_));
00123                 nbh.resize(k);
00124             }
00125             else
00126             {
00127                 std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
00128             }
00129         }
00130 
00131         virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
00132         {
00133             nbh.clear();
00134             for (std::size_t i = 0 ; i < data_.size() ; ++i)
00135                 if (NearestNeighbors<_T>::distFun_(data_[i], data) <= radius)
00136                     nbh.push_back(data_[i]);
00137             std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
00138         }
00139 
00140         virtual std::size_t size(void) const
00141         {
00142             return data_.size();
00143         }
00144 
00145         virtual void list(std::vector<_T> &data) const
00146         {
00147             data = data_;
00148         }
00149 
00150     protected:
00151 
00153         std::vector<_T>   data_;
00154 
00155     private:
00156 
00157         struct ElemSort
00158         {
00159             ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
00160             {
00161             }
00162 
00163             bool operator()(const _T &a, const _T &b) const
00164             {
00165                 return df_(a, e_) < df_(b, e_);
00166             }
00167 
00168             const _T                                              &e_;
00169             const typename NearestNeighbors<_T>::DistanceFunction &df_;
00170         };
00171 
00172     };
00173 
00174 
00175 }
00176 
00177 #endif