CircleIterator.cpp
Go to the documentation of this file.
1 /*
2  * Circleterator.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 CircleIterator::CircleIterator(const GridMap& gridMap, const Position& center, const double radius)
17  : center_(center),
18  radius_(radius)
19 {
20  radiusSquare_ = pow(radius_, 2);
21  mapLength_ = gridMap.getLength();
22  mapPosition_ = gridMap.getPosition();
23  resolution_ = gridMap.getResolution();
24  bufferSize_ = gridMap.getSize();
25  bufferStartIndex_ = gridMap.getStartIndex();
26  Index submapStartIndex;
27  Index submapBufferSize;
28  findSubmapParameters(center, radius, submapStartIndex, submapBufferSize);
29  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
30  if(!isInside()) ++(*this);
31 }
32 
34 {
35  center_ = other.center_;
36  radius_ = other.radius_;
39  mapLength_ = other.mapLength_;
40  mapPosition_ = other.mapPosition_;
41  resolution_ = other.resolution_;
42  bufferSize_ = other.bufferSize_;
44  return *this;
45 }
46 
48 {
49  return (internalIterator_ != other.internalIterator_);
50 }
51 
53 {
54  return *(*internalIterator_);
55 }
56 
58 {
59  ++(*internalIterator_);
60  if (internalIterator_->isPastEnd()) return *this;
61 
62  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
63  if (isInside()) break;
64  }
65 
66  return *this;
67 }
68 
70 {
71  return internalIterator_->isPastEnd();
72 }
73 
75 {
76  Position position;
78  double squareNorm = (position - center_).array().square().sum();
79  return (squareNorm <= radiusSquare_);
80 }
81 
82 void CircleIterator::findSubmapParameters(const Position& center, const double radius,
83  Index& startIndex, Size& bufferSize) const
84 {
85  Position topLeft = center.array() + radius;
86  Position bottomRight = center.array() - radius;
90  Index endIndex;
92  bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
93 }
94 
95 } /* namespace grid_map */
96 
const Length & getLength() const
Definition: GridMap.cpp:644
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
double radiusSquare_
Square of the radius (for efficiency).
bool operator!=(const CircleIterator &other) const
double radius_
Radius of the circle.
const Index & getStartIndex() const
Definition: GridMap.cpp:664
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:237
Position center_
Position of the circle center;.
const Index & operator*() const
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
CircleIterator & operator++()
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:652
void findSubmapParameters(const Position &center, const double radius, Index &startIndex, Size &bufferSize) const
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
Size getSubmapSizeFromCornerIndeces(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
CircleIterator & operator=(const CircleIterator &other)
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
Length mapLength_
Map information needed to get position from iterator.
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator. // TODO Think of using unique_ptr instead.
const Size & getSize() const
Definition: GridMap.cpp:656


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27