PolygonIterator.cpp
Go to the documentation of this file.
1 /*
2  * PolygonIterator.hpp
3  *
4  * Created on: Sep 19, 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 PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
17  : polygon_(polygon)
18 {
19  mapLength_ = gridMap.getLength();
20  mapPosition_ = gridMap.getPosition();
21  resolution_ = gridMap.getResolution();
22  bufferSize_ = gridMap.getSize();
23  bufferStartIndex_ = gridMap.getStartIndex();
24  Index submapStartIndex;
25  Size submapBufferSize;
26  findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
27  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
28  if(!isInside()) ++(*this);
29 }
30 
32 {
33  polygon_ = other.polygon_;
35  mapLength_ = other.mapLength_;
36  mapPosition_ = other.mapPosition_;
37  resolution_ = other.resolution_;
38  bufferSize_ = other.bufferSize_;
40  return *this;
41 }
42 
44 {
45  return (internalIterator_ != other.internalIterator_);
46 }
47 
49 {
50  return *(*internalIterator_);
51 }
52 
54 {
55  ++(*internalIterator_);
56  if (internalIterator_->isPastEnd()) return *this;
57 
58  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
59  if (isInside()) break;
60  }
61 
62  return *this;
63 }
64 
66 {
67  return internalIterator_->isPastEnd();
68 }
69 
71 {
72  Position position;
74  return polygon_.isInside(position);
75 }
76 
77 void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const
78 {
79  Position topLeft = polygon_.getVertices()[0];
80  Position bottomRight = topLeft;
81  for (const auto& vertex : polygon_.getVertices()) {
82  topLeft = topLeft.array().max(vertex.array());
83  bottomRight = bottomRight.array().min(vertex.array());
84  }
88  Index endIndex;
90  bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
91 }
92 
93 } /* namespace grid_map */
94 
const Length & getLength() const
Definition: GridMap.cpp:640
grid_map::Polygon polygon_
Polygon to iterate on.
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
PolygonIterator & operator++()
const Index & getStartIndex() const
Definition: GridMap.cpp:660
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:233
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator.
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
double getResolution() const
Definition: GridMap.cpp:648
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
PolygonIterator & operator=(const PolygonIterator &other)
Size getSubmapSizeFromCornerIndeces(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
bool isInside(const Position &point) const
Definition: Polygon.cpp:32
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
const Index & operator*() const
Length mapLength_
Map information needed to get position from iterator.
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
void findSubmapParameters(const grid_map::Polygon &polygon, Index &startIndex, Size &bufferSize) const
const std::vector< Position > & getVertices() const
Definition: Polygon.cpp:66
bool operator!=(const PolygonIterator &other) const
const Size & getSize() const
Definition: GridMap.cpp:652


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Sun Oct 25 2020 03:14:42