libnabo 1.0.1
index_heap.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
00004 You can contact the author at <stephane at magnenat dot net>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the <organization> nor the
00016       names of its contributors may be used to endorse or promote products
00017       derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef __INDEX_HEAP_H
00033 #define __INDEX_HEAP_H
00034 
00035 #include "nabo.h"
00036 #include <vector>
00037 #include <algorithm>
00038 #include <limits>
00039 
00045 namespace Nabo
00046 {
00048 
00050         template<typename IT, typename VT>
00051         struct IndexHeapSTL
00052         {
00054                 typedef IT Index;
00056                 typedef VT Value;
00057                 
00059                 struct Entry
00060                 {
00061                         IT index; 
00062                         VT value; 
00063                         
00065                         Entry(const IT index, const VT value): index(index), value(value) {}
00067                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00068                 };
00070                 typedef std::vector<Entry> Entries;
00072                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00074                 typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
00075                 
00077                 Entries data;
00079                 const VT& headValueRef;
00081                 const typename Entries::iterator insertIt;
00082                 
00084 
00085                 IndexHeapSTL(const size_t size):
00086                         data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00087                         headValueRef(data.begin()->value),
00088                         insertIt(data.end() - 1)
00089                 {
00090                         std::make_heap(data.begin(), data.end());
00091                 }
00092                 
00094                 inline void reset()
00095                 {
00096                         std::fill(data.begin(), data.end(), Entry(0, std::numeric_limits<VT>::infinity()));
00097                         std::make_heap(data.begin(), data.end());
00098                 }
00099                 
00101 
00102                 inline const VT& headValue() const { return headValueRef; }
00103                 
00105 
00107                 inline void replaceHead(const Index index, const Value value)
00108                 {
00109                         std::pop_heap(data.begin(), data.end());
00110                         insertIt->index = index;
00111                         insertIt->value = value;
00112                         push_heap(data.begin(), data.end());
00113                 }
00114                 
00116                 inline void sort()
00117                 {
00118                         sort_heap (data.begin(), data.end());
00119                 }
00120                 
00122 
00124                 template<typename DI, typename DV>
00125                 inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
00126                 {
00127                         // note: we must implement this hack because of problem with reference to temporary
00128                         // C++0x will solve this with rvalue
00129                         // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
00130                         // for more informations
00131                         for (size_t i = 0; i < data.size(); ++i)
00132                         {
00133                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
00134                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
00135                         }
00136                 }
00137                 
00138 #if 0
00139 
00140 
00141                 inline IndexVector getIndexes() const
00142                 {
00143                         IndexVector indexes(data.size());
00144                         for (size_t i = 0; i < data.size(); ++i)
00145                                 indexes.coeffRef(i) = data[i].index;
00146                         return indexes;
00147                 }
00148 #endif
00149         };
00150         
00151 #if 0
00152 
00153 
00155         template<typename IT, typename VT>
00156         struct IndexHeapBruteForceVector
00157         {
00159                 typedef IT Index;
00161                 typedef VT Value;
00162                 
00164                 struct Entry
00165                 {
00166                         IT index; 
00167                         VT value;  
00168                         
00170                         Entry(const IT index, const VT value): index(index), value(value) {} 
00172                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00173                 };
00175                 typedef std::vector<Entry> Entries;
00177                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00178                 
00180                 Entries data;
00182                 const VT& headValueRef;
00184                 const size_t sizeMinusOne;
00185                 
00187 
00188                 IndexHeapBruteForceVector(const size_t size):
00189                         data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00190                         headValueRef((data.end() - 1)->value),
00191                         sizeMinusOne(data.size() - 1)
00192                 {
00193                 }
00194                 
00196                 inline void reset()
00197                 {
00198                         for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
00199                                 it->value = std::numeric_limits<VT>::infinity();
00200                 }
00201                 
00203 
00204                 inline const VT& headValue() const { return data[0].value; }
00205                 
00207 
00209                 inline void replaceHead(const Index index, const Value value)
00210                 {
00211                         register size_t i = 0;
00212                         for (; i < sizeMinusOne; ++i)
00213                         {
00214                                 if (data[i + 1].value > value)
00215                                         data[i] = data[i + 1];
00216                                 else
00217                                         break;
00218                         }
00219                         data[i].value = value;
00220                         data[i].index = index;
00221                 }
00222                 
00224                 inline void sort()
00225                 {
00226                         // no need to sort as data are already sorted
00227                 }
00228                 
00230 
00231                 inline IndexVector getIndexes() const
00232                 {
00233                         IndexVector indexes(data.size());
00234                         for (size_t i = 0; i < data.size(); ++i)
00235                                 indexes.coeffRef(i) = data[sizeMinusOne-i].index;
00236                         return indexes;
00237                 }
00238         };
00239 #endif
00240         
00242 
00244         template<typename IT, typename VT>
00245         struct IndexHeapBruteForceVector
00246         {
00248                 typedef IT Index;
00250                 typedef VT Value;
00251                 
00253                 struct Entry
00254                 {
00255                         IT index; 
00256                         VT value;  
00257                         
00259                         Entry(const IT index, const VT value): index(index), value(value) {} 
00261                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00262                 };
00264                 typedef std::vector<Entry> Entries;
00266                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00268                 typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
00269                 
00271                 Entries data;
00273                 const VT& headValueRef;
00275                 const size_t sizeMinusOne;
00276                 
00278 
00279                 IndexHeapBruteForceVector(const size_t size):
00280                 data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00281                 headValueRef((data.end() - 1)->value),
00282                 sizeMinusOne(data.size() - 1)
00283                 {
00284                 }
00285                 
00287                 inline void reset()
00288                 {
00289                         for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
00290                                 it->value = std::numeric_limits<VT>::infinity();
00291                 }
00292                 
00294 
00295                 inline const VT& headValue() const { return headValueRef; }
00296                 
00298 
00300                 inline void replaceHead(const Index index, const Value value)
00301                 {
00302                         register size_t i;
00303                         for (i = sizeMinusOne; i > 0; --i)
00304                         {
00305                                 if (data[i-1].value > value)
00306                                         data[i] = data[i-1];
00307                                 else
00308                                         break;
00309                         }
00310                         data[i].value = value;
00311                         data[i].index = index;
00312                 }
00313                 
00315                 inline void sort()
00316                 {
00317                         // no need to sort as data are already sorted
00318                 }
00319                 
00321 
00323                 template<typename DI, typename DV>
00324                 inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
00325                 {
00326                         // note: we must implement this hack because of problem with reference to temporary
00327                         // C++0x will solve this with rvalue
00328                         // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
00329                         // for more informations
00330                         for (size_t i = 0; i < data.size(); ++i)
00331                         {
00332                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
00333                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
00334                         }
00335                 }
00336 #if 0
00337 
00338 
00339                 inline IndexVector getIndexes() const
00340                 {
00341                         IndexVector indexes(data.size());
00342                         for (size_t i = 0; i < data.size(); ++i)
00343                                 indexes.coeffRef(i) = data[i].index;
00344                         return indexes;
00345                 }
00346 #endif
00347         };
00348 }
00349 
00350 #endif // __INDEX_HEAP_H