00001 /* 00002 * GridMapIterator.cpp 00003 * 00004 * Created on: Sep 22, 2014 00005 * Author: Péter Fankhauser 00006 * Institute: ETH Zurich, ANYbotics 00007 */ 00008 00009 #include "grid_map_core/iterators/GridMapIterator.hpp" 00010 #include "grid_map_core/GridMapMath.hpp" 00011 00012 namespace grid_map { 00013 00014 GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap) 00015 { 00016 size_ = gridMap.getSize(); 00017 startIndex_ = gridMap.getStartIndex(); 00018 linearSize_ = size_.prod(); 00019 linearIndex_ = 0; 00020 isPastEnd_ = false; 00021 } 00022 00023 GridMapIterator::GridMapIterator(const GridMapIterator* other) 00024 { 00025 size_ = other->size_; 00026 startIndex_ = other->startIndex_; 00027 linearSize_ = other->linearSize_; 00028 linearIndex_ = other->linearIndex_; 00029 isPastEnd_ = other->isPastEnd_; 00030 } 00031 00032 GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other) 00033 { 00034 size_ = other.size_; 00035 startIndex_ = other.startIndex_; 00036 linearSize_ = other.linearSize_; 00037 linearIndex_ = other.linearIndex_; 00038 isPastEnd_ = other.isPastEnd_; 00039 return *this; 00040 } 00041 00042 bool GridMapIterator::operator !=(const GridMapIterator& other) const 00043 { 00044 return linearIndex_ != other.linearIndex_; 00045 } 00046 00047 const Index GridMapIterator::operator *() const 00048 { 00049 return getIndexFromLinearIndex(linearIndex_, size_); 00050 } 00051 00052 const size_t& GridMapIterator::getLinearIndex() const 00053 { 00054 return linearIndex_; 00055 } 00056 00057 const Index GridMapIterator::getUnwrappedIndex() const 00058 { 00059 return getIndexFromBufferIndex(*(*this), size_, startIndex_); 00060 } 00061 00062 GridMapIterator& GridMapIterator::operator ++() 00063 { 00064 size_t newIndex = linearIndex_ + 1; 00065 if (newIndex < linearSize_) { 00066 linearIndex_ = newIndex; 00067 } else { 00068 isPastEnd_ = true; 00069 } 00070 return *this; 00071 } 00072 00073 GridMapIterator GridMapIterator::end() const 00074 { 00075 GridMapIterator res(this); 00076 res.linearIndex_ = linearSize_ - 1; 00077 return res; 00078 } 00079 00080 bool GridMapIterator::isPastEnd() const 00081 { 00082 return isPastEnd_; 00083 } 00084 00085 } /* namespace grid_map */