libnabo 1.0.1
|
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