bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap,
const Position& start, const Position& end,
Index& index)
{
Position newStart = start;
Vector direction = (end - start).normalized();
while (!gridMap.getIndex(newStart, index)) {
newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon())
return false;
}
return true;
}
SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center,
const double radius)
: center_(center),
radius_(radius),
distance_(0)
{
radiusSquare_ = radius_ * radius_;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
gridMap.getIndex(center_, indexCenter_);
nRings_ = std::ceil(radius_ / resolution_);
if (checkIfIndexWithinRange(indexCenter_, bufferSize_)) pointsRing_.push_back(indexCenter_);
else generateRing();
}