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 #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
00128
00129
00130
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
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
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
00327
00328
00329
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