18 Index startIndex, endIndex;
19 if (getIndexLimitedToMapRange(cost_map, start, end, startIndex) &&
20 getIndexLimitedToMapRange(cost_map, end, start, endIndex))
22 initialize(cost_map, startIndex, endIndex);
28 initialize(cost_map, start, end);
53 return (index_ != other.
index_).any();
56 const Index& LineIterator::operator *()
const 63 numerator_ += numeratorAdd_;
64 if (numerator_ >= denominator_) {
65 numerator_ -= denominator_;
66 index_ += increment1_;
68 index_ += increment2_;
73 bool LineIterator::isPastEnd()
const 75 return iCell_ >= nCells_;
85 bufferSize_ = cost_map.
getSize();
87 Index submapStartIndex;
88 Size submapBufferSize;
89 initializeIterationParameters();
98 Vector direction = (end - start).normalized();
99 while (!cost_map.
getIndex(newStart, index)) {
100 newStart += (cost_map.
getResolution() - std::numeric_limits<double>::epsilon()) * direction;
101 if ((end - newStart).norm() < cost_map.
getResolution() - std::numeric_limits<double>::epsilon())
107 void LineIterator::initializeIterationParameters()
112 Size delta = (end_ - start_).abs();
114 if (end_.x() >= start_.x()) {
120 increment1_.x() = -1;
121 increment2_.x() = -1;
124 if (end_.y() >= start_.y()) {
130 increment1_.y() = -1;
131 increment2_.y() = -1;
134 if (delta.x() >= delta.y()) {
138 denominator_ = delta.x();
139 numerator_ = delta.x() / 2;
140 numeratorAdd_ = delta.y();
141 nCells_ = delta.x() + 1;
146 denominator_ = delta.y();
147 numerator_ = delta.y() / 2;
148 numeratorAdd_ = delta.x();
149 nCells_ = delta.y() + 1;
const Length & getLength() const
Length mapLength_
Map information needed to get position from iterator.
const Size & getSize() const
double getResolution() const
Index start_
Starting index of the line.
Index index_
Current index.
Index end_
Ending index of the line.
Size increment1_
Helper variables for Bresenham Line Drawing algorithm.
bool getPosition(const Index &index, Position &position) const
grid_map::Position Position
unsigned int nCells_
Number of cells in the line.
unsigned int iCell_
Current cell number.
const Index & getStartIndex() const
bool getIndex(const Position &position, Index &index) const