18 SpiralIterator::SpiralIterator(
const grid_map::GridMap& gridMap,
const Eigen::Vector2d& center,
78 Eigen::Vector2d position;
80 double squareNorm = (position -
center_).array().square().sum();
101 normal.x() = -
signum(point.y());
102 normal.y() =
signum(point.x());
104 &&
static_cast<unsigned int>(
Vector(point.x() + normal.x(), point.y()).norm()) ==
distance_)
105 point.x() += normal.x();
106 else if (normal.y() != 0
107 &&
static_cast<unsigned int>(
Vector(point.x(), point.y() + normal.y()).norm()) ==
distance_)
108 point.y() += normal.y();
110 point.x() += normal.x();
111 point.y() += normal.y();
113 }
while (static_cast<unsigned int>(point.x()) !=
distance_ || point.y() != 0);
const Length & getLength() const
bool getPosition(const Index &index, Position &position) const
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
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
const Size & getSize() const
Position center_
Position of the circle center;.