Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/iterators/LineIterator.hpp"
00010 #include "grid_map_core/GridMapMath.hpp"
00011
00012 using namespace std;
00013
00014 namespace grid_map {
00015
00016 LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start,
00017 const Position& end)
00018 {
00019 Index startIndex, endIndex;
00020 if (getIndexLimitedToMapRange(gridMap, start, end, startIndex)
00021 && getIndexLimitedToMapRange(gridMap, end, start, endIndex))
00022 initialize(gridMap, startIndex, endIndex);
00023 }
00024
00025 LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
00026 {
00027 initialize(gridMap, start, end);
00028 }
00029
00030 LineIterator& LineIterator::operator =(const LineIterator& other)
00031 {
00032 index_ = other.index_;
00033 start_ = other.start_;
00034 end_ = other.end_;
00035 iCell_ = other.iCell_;
00036 nCells_ = other.nCells_;
00037 increment1_ = other.increment1_;
00038 increment2_ = other.increment2_;
00039 denominator_ = other.denominator_;
00040 numerator_ = other.numerator_;
00041 numeratorAdd_ = other.numeratorAdd_;
00042 mapLength_ = other.mapLength_;
00043 mapPosition_ = other.mapPosition_;
00044 resolution_ = other.resolution_;
00045 bufferSize_ = other.bufferSize_;
00046 bufferStartIndex_ = other.bufferStartIndex_;
00047 return *this;
00048 }
00049
00050 bool LineIterator::operator !=(const LineIterator& other) const
00051 {
00052 return (index_ != other.index_).any();
00053 }
00054
00055 const Index& LineIterator::operator *() const
00056 {
00057 return index_;
00058 }
00059
00060 LineIterator& LineIterator::operator ++()
00061 {
00062 numerator_ += numeratorAdd_;
00063 if (numerator_ >= denominator_) {
00064 numerator_ -= denominator_;
00065 const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_;
00066 index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
00067 }
00068 const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_;
00069 index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
00070 ++iCell_;
00071 return *this;
00072 }
00073
00074 bool LineIterator::isPastEnd() const
00075 {
00076 return iCell_ >= nCells_;
00077 }
00078
00079 bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
00080 {
00081 start_ = start;
00082 end_ = end;
00083 mapLength_ = gridMap.getLength();
00084 mapPosition_ = gridMap.getPosition();
00085 resolution_ = gridMap.getResolution();
00086 bufferSize_ = gridMap.getSize();
00087 bufferStartIndex_ = gridMap.getStartIndex();
00088 initializeIterationParameters();
00089 return true;
00090 }
00091
00092 bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap,
00093 const Position& start, const Position& end,
00094 Index& index)
00095 {
00096 Position newStart = start;
00097 Vector direction = (end - start).normalized();
00098 while (!gridMap.getIndex(newStart, index)) {
00099 newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
00100 if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon())
00101 return false;
00102 }
00103 return true;
00104 }
00105
00106 void LineIterator::initializeIterationParameters()
00107 {
00108 iCell_ = 0;
00109 index_ = start_;
00110
00111 const Index unwrappedStart = getIndexFromBufferIndex(start_, bufferSize_, bufferStartIndex_);
00112 const Index unwrappedEnd = getIndexFromBufferIndex(end_, bufferSize_, bufferStartIndex_);
00113 const Size delta = (unwrappedEnd - unwrappedStart).abs();
00114
00115 if (unwrappedEnd.x() >= unwrappedStart.x()) {
00116
00117 increment1_.x() = 1;
00118 increment2_.x() = 1;
00119 } else {
00120
00121 increment1_.x() = -1;
00122 increment2_.x() = -1;
00123 }
00124
00125 if (unwrappedEnd.y() >= unwrappedStart.y()) {
00126
00127 increment1_.y() = 1;
00128 increment2_.y() = 1;
00129 } else {
00130
00131 increment1_.y() = -1;
00132 increment2_.y() = -1;
00133 }
00134
00135 if (delta.x() >= delta.y()) {
00136
00137 increment1_.x() = 0;
00138 increment2_.y() = 0;
00139 denominator_ = delta.x();
00140 numerator_ = delta.x() / 2;
00141 numeratorAdd_ = delta.y();
00142 nCells_ = delta.x() + 1;
00143 } else {
00144
00145 increment2_.x() = 0;
00146 increment1_.y() = 0;
00147 denominator_ = delta.y();
00148 numerator_ = delta.y() / 2;
00149 numeratorAdd_ = delta.x();
00150 nCells_ = delta.y() + 1;
00151 }
00152 }
00153
00154 }