ompl/datastructures/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() : NearestNeighbors<_T>()
00061         {
00062         }
00063 
00064         virtual ~NearestNeighborsLinear()
00065         {
00066         }
00067 
00068         virtual void clear()
00069         {
00070             data_.clear();
00071         }
00072 
00073         virtual bool reportsSortedResults() const
00074         {
00075             return true;
00076         }
00077 
00078         virtual void add(const _T &data)
00079         {
00080             data_.push_back(data);
00081         }
00082 
00083         virtual void add(const std::vector<_T> &data)
00084         {
00085             data_.reserve(data_.size() + data.size());
00086             data_.insert(data_.end(), data.begin(), data.end());
00087         }
00088 
00089         virtual bool remove(const _T &data)
00090         {
00091             if (!data_.empty())
00092                 for (int i = data_.size() - 1 ; i >= 0 ; --i)
00093                     if (data_[i] == data)
00094                     {
00095                         data_.erase(data_.begin() + i);
00096                         return true;
00097                     }
00098             return false;
00099         }
00100 
00101         virtual _T nearest(const _T &data) const
00102         {
00103             const std::size_t sz = data_.size();
00104             std::size_t pos = sz;
00105             double dmin = 0.0;
00106             for (std::size_t i = 0 ; i < sz ; ++i)
00107             {
00108                 double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
00109                 if (pos == sz || dmin > distance)
00110                 {
00111                     pos = i;
00112                     dmin = distance;
00113                 }
00114             }
00115             if (pos != sz)
00116                 return data_[pos];
00117 
00118             throw Exception("No elements found in nearest neighbors data structure");
00119         }
00120 
00122         virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
00123         {
00124             nbh = data_;
00125             if (nbh.size() > k)
00126             {
00127                 std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
00128                                   ElemSort(data, NearestNeighbors<_T>::distFun_));
00129                 nbh.resize(k);
00130             }
00131             else
00132             {
00133                 std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
00134             }
00135         }
00136 
00138         virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
00139         {
00140             nbh.clear();
00141             for (std::size_t i = 0 ; i < data_.size() ; ++i)
00142                 if (NearestNeighbors<_T>::distFun_(data_[i], data) <= radius)
00143                     nbh.push_back(data_[i]);
00144             std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
00145         }
00146 
00147         virtual std::size_t size() const
00148         {
00149             return data_.size();
00150         }
00151 
00152         virtual void list(std::vector<_T> &data) const
00153         {
00154             data = data_;
00155         }
00156 
00157     protected:
00158 
00160         std::vector<_T>   data_;
00161 
00162     private:
00163 
00164         struct ElemSort
00165         {
00166             ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
00167             {
00168             }
00169 
00170             bool operator()(const _T &a, const _T &b) const
00171             {
00172                 return df_(a, e_) < df_(b, e_);
00173             }
00174 
00175             const _T                                              &e_;
00176             const typename NearestNeighbors<_T>::DistanceFunction &df_;
00177         };
00178 
00179     };
00180 
00181 
00182 }
00183 
00184 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines