SpiralIterator.cpp
Go to the documentation of this file.
1 /*
2  * SpiralIterator.hpp
3  *
4  * Created on: Jul 7, 2015
5  * Author: Martin Wermelinger
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 #include <cmath>
13 
14 using namespace std;
15 
16 namespace grid_map {
17 
18 SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center,
19  const double radius)
20  : center_(center),
21  radius_(radius),
22  distance_(0)
23 {
25  mapLength_ = gridMap.getLength();
26  mapPosition_ = gridMap.getPosition();
27  resolution_ = gridMap.getResolution();
28  bufferSize_ = gridMap.getSize();
29  gridMap.getIndex(center_, indexCenter_);
30  nRings_ = std::ceil(radius_ / resolution_);
32  pointsRing_.push_back(indexCenter_);
33  else
34  while(pointsRing_.empty() && !isPastEnd())
35  generateRing();
36 }
37 
39 {
40  center_ = other.center_;
41  indexCenter_ = other.indexCenter_;
42  radius_ = other.radius_;
44  nRings_ = other.nRings_;
45  distance_ = other.distance_;
46  pointsRing_ = other.pointsRing_;
47  mapLength_ = other.mapLength_;
48  mapPosition_ = other.mapPosition_;
49  resolution_ = other.resolution_;
50  bufferSize_ = other.bufferSize_;
51  return *this;
52 }
53 
55 {
56  return (pointsRing_.back() != pointsRing_.back()).any();
57 }
58 
59 const Eigen::Array2i& SpiralIterator::operator *() const
60 {
61  return pointsRing_.back();
62 }
63 
65 {
66  pointsRing_.pop_back();
67  if (pointsRing_.empty() && !isPastEnd()) generateRing();
68  return *this;
69 }
70 
72 {
73  return (distance_ == nRings_ && pointsRing_.empty());
74 }
75 
76 bool SpiralIterator::isInside(const Index index) const
77 {
78  Eigen::Vector2d position;
80  double squareNorm = (position - center_).array().square().sum();
81  return (squareNorm <= radiusSquare_);
82 }
83 
85 {
86  distance_++;
87  Index point(distance_, 0);
88  Index pointInMap;
89  Index normal;
90  do {
91  pointInMap.x() = point.x() + indexCenter_.x();
92  pointInMap.y() = point.y() + indexCenter_.y();
93  if (checkIfIndexInRange(pointInMap, bufferSize_)) {
94  if (distance_ == nRings_ || distance_ == nRings_ - 1) {
95  if (isInside(pointInMap))
96  pointsRing_.push_back(pointInMap);
97  } else {
98  pointsRing_.push_back(pointInMap);
99  }
100  }
101  normal.x() = -signum(point.y());
102  normal.y() = signum(point.x());
103  if (normal.x() != 0
104  && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_)
105  point.x() += normal.x();
106  else if (normal.y() != 0
107  && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_)
108  point.y() += normal.y();
109  else {
110  point.x() += normal.x();
111  point.y() += normal.y();
112  }
113  } while (point.x() != distance_ || point.y() != 0);
114 }
115 
117 {
118  Index radius = *(*this) - indexCenter_;
119  return radius.matrix().norm() * resolution_;
120 }
121 
122 } /* namespace grid_map */
123 
const Length & getLength() const
Definition: GridMap.cpp:640
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:233
bool operator!=(const SpiralIterator &other) const
Length mapLength_
Map information needed to get position from iterator.
SpiralIterator & operator=(const SpiralIterator &other)
int signum(const int val)
const Eigen::Array2i & operator*() const
std::vector< Index > pointsRing_
unsigned int nRings_
Number of rings into the circle is divided.
double radius_
Radius of the circle.
double getResolution() const
Definition: GridMap.cpp:648
double getCurrentRadius() const
bool isInside(const Index index) const
double radiusSquare_
Square of the radius for efficiency.
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
SpiralIterator & operator++()
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:229
const Size & getSize() const
Definition: GridMap.cpp:652
Position center_
Position of the circle center;.


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