LineIterator.cpp
Go to the documentation of this file.
1 /*
2  * LineIterator.hpp
3  *
4  * Created on: Nov 13, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 using namespace std;
13 
14 namespace grid_map {
15 
16 LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start,
17  const Position& end)
18 {
19  Index startIndex, endIndex;
20  if (getIndexLimitedToMapRange(gridMap, start, end, startIndex)
21  && getIndexLimitedToMapRange(gridMap, end, start, endIndex)) {
22  initialize(gridMap, startIndex, endIndex);
23  }
24  else {
25  throw std::invalid_argument("Failed to construct LineIterator.");
26  }
27 }
28 
29 LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
30 {
31  initialize(gridMap, start, end);
32 }
33 
34 LineIterator& LineIterator::operator =(const LineIterator& other)
35 {
36  index_ = other.index_;
37  start_ = other.start_;
38  end_ = other.end_;
39  iCell_ = other.iCell_;
40  nCells_ = other.nCells_;
41  increment1_ = other.increment1_;
42  increment2_ = other.increment2_;
43  denominator_ = other.denominator_;
44  numerator_ = other.numerator_;
45  numeratorAdd_ = other.numeratorAdd_;
46  mapLength_ = other.mapLength_;
47  mapPosition_ = other.mapPosition_;
48  resolution_ = other.resolution_;
49  bufferSize_ = other.bufferSize_;
50  bufferStartIndex_ = other.bufferStartIndex_;
51  return *this;
52 }
53 
54 bool LineIterator::operator !=(const LineIterator& other) const
55 {
56  return (index_ != other.index_).any();
57 }
58 
59 const Index& LineIterator::operator *() const
60 {
61  return index_;
62 }
63 
64 LineIterator& LineIterator::operator ++()
65 {
66  numerator_ += numeratorAdd_; // Increase the numerator by the top of the fraction.
67  if (numerator_ >= denominator_) {
68  numerator_ -= denominator_;
69  const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_;
70  index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
71  }
72  const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_;
73  index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
74  ++iCell_;
75  return *this;
76 }
77 
78 bool LineIterator::isPastEnd() const
79 {
80  return iCell_ >= nCells_;
81 }
82 
83 bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
84 {
85  start_ = start;
86  end_ = end;
87  mapLength_ = gridMap.getLength();
88  mapPosition_ = gridMap.getPosition();
89  resolution_ = gridMap.getResolution();
90  bufferSize_ = gridMap.getSize();
91  bufferStartIndex_ = gridMap.getStartIndex();
92  initializeIterationParameters();
93  return true;
94 }
95 
96 bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap,
97  const Position& start, const Position& end,
98  Index& index)
99 {
100  Position newStart = start;
101  Vector direction = (end - start).normalized();
102  while (!gridMap.getIndex(newStart, index)) {
103  newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
104  if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon())
105  return false;
106  }
107  return true;
108 }
109 
110 void LineIterator::initializeIterationParameters()
111 {
112  iCell_ = 0;
113  index_ = start_;
114 
115  const Index unwrappedStart = getIndexFromBufferIndex(start_, bufferSize_, bufferStartIndex_);
116  const Index unwrappedEnd = getIndexFromBufferIndex(end_, bufferSize_, bufferStartIndex_);
117  const Size delta = (unwrappedEnd - unwrappedStart).abs();
118 
119  if (unwrappedEnd.x() >= unwrappedStart.x()) {
120  // x-values increasing.
121  increment1_.x() = 1;
122  increment2_.x() = 1;
123  } else {
124  // x-values decreasing.
125  increment1_.x() = -1;
126  increment2_.x() = -1;
127  }
128 
129  if (unwrappedEnd.y() >= unwrappedStart.y()) {
130  // y-values increasing.
131  increment1_.y() = 1;
132  increment2_.y() = 1;
133  } else {
134  // y-values decreasing.
135  increment1_.y() = -1;
136  increment2_.y() = -1;
137  }
138 
139  if (delta.x() >= delta.y()) {
140  // There is at least one x-value for every y-value.
141  increment1_.x() = 0; // Do not change the x when numerator >= denominator.
142  increment2_.y() = 0; // Do not change the y for every iteration.
143  denominator_ = delta.x();
144  numerator_ = delta.x() / 2;
145  numeratorAdd_ = delta.y();
146  nCells_ = delta.x() + 1; // There are more x-values than y-values.
147  } else {
148  // There is at least one y-value for every x-value
149  increment2_.x() = 0; // Do not change the x for every iteration.
150  increment1_.y() = 0; // Do not change the y when numerator >= denominator.
151  denominator_ = delta.y();
152  numerator_ = delta.y() / 2;
153  numeratorAdd_ = delta.x();
154  nCells_ = delta.y() + 1; // There are more y-values than x-values.
155  }
156 }
157 
158 } /* namespace grid_map */
const Length & getLength() const
Definition: GridMap.cpp:640
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
unsigned int iCell_
Current cell number.
const Index & getStartIndex() const
Definition: GridMap.cpp:660
unsigned int nCells_
Number of cells in the line.
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:233
Index getIndexFromBufferIndex(const Index &bufferIndex, const Size &bufferSize, const Index &bufferStartIndex)
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
Index end_
Ending index of the line.
Length mapLength_
Map information needed to get position from iterator.
double getResolution() const
Definition: GridMap.cpp:648
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
Size increment1_
Helper variables for Bresenham Line Drawing algorithm.
Index start_
Starting index of the line.
Index getBufferIndexFromIndex(const Index &index, const Size &bufferSize, const Index &bufferStartIndex)
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:229
const Size & getSize() const
Definition: GridMap.cpp:652
Index index_
Current index.


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Thu Oct 22 2020 03:19:19