Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/iterators/SpiralIterator.hpp"
00010 #include "grid_map_core/GridMapMath.hpp"
00011
00012 #include <cmath>
00013
00014 using namespace std;
00015
00016 namespace grid_map {
00017
00018 SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center,
00019 const double radius)
00020 : center_(center),
00021 radius_(radius),
00022 distance_(0)
00023 {
00024 radiusSquare_ = radius_ * radius_;
00025 mapLength_ = gridMap.getLength();
00026 mapPosition_ = gridMap.getPosition();
00027 resolution_ = gridMap.getResolution();
00028 bufferSize_ = gridMap.getSize();
00029 gridMap.getIndex(center_, indexCenter_);
00030 nRings_ = std::ceil(radius_ / resolution_);
00031 if (checkIfIndexInRange(indexCenter_, bufferSize_))
00032 pointsRing_.push_back(indexCenter_);
00033 else
00034 while(pointsRing_.empty() && !isPastEnd())
00035 generateRing();
00036 }
00037
00038 SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other)
00039 {
00040 center_ = other.center_;
00041 indexCenter_ = other.indexCenter_;
00042 radius_ = other.radius_;
00043 radiusSquare_ = other.radiusSquare_;
00044 nRings_ = other.nRings_;
00045 distance_ = other.distance_;
00046 pointsRing_ = other.pointsRing_;
00047 mapLength_ = other.mapLength_;
00048 mapPosition_ = other.mapPosition_;
00049 resolution_ = other.resolution_;
00050 bufferSize_ = other.bufferSize_;
00051 return *this;
00052 }
00053
00054 bool SpiralIterator::operator !=(const SpiralIterator& other) const
00055 {
00056 return (pointsRing_.back() != pointsRing_.back()).any();
00057 }
00058
00059 const Eigen::Array2i& SpiralIterator::operator *() const
00060 {
00061 return pointsRing_.back();
00062 }
00063
00064 SpiralIterator& SpiralIterator::operator ++()
00065 {
00066 pointsRing_.pop_back();
00067 if (pointsRing_.empty() && !isPastEnd()) generateRing();
00068 return *this;
00069 }
00070
00071 bool SpiralIterator::isPastEnd() const
00072 {
00073 return (distance_ == nRings_ && pointsRing_.empty());
00074 }
00075
00076 bool SpiralIterator::isInside(const Index index) const
00077 {
00078 Eigen::Vector2d position;
00079 getPositionFromIndex(position, index, mapLength_, mapPosition_, resolution_, bufferSize_);
00080 double squareNorm = (position - center_).array().square().sum();
00081 return (squareNorm <= radiusSquare_);
00082 }
00083
00084 void SpiralIterator::generateRing()
00085 {
00086 distance_++;
00087 Index point(distance_, 0);
00088 Index pointInMap;
00089 Index normal;
00090 do {
00091 pointInMap.x() = point.x() + indexCenter_.x();
00092 pointInMap.y() = point.y() + indexCenter_.y();
00093 if (checkIfIndexInRange(pointInMap, bufferSize_)) {
00094 if (distance_ == nRings_ || distance_ == nRings_ - 1) {
00095 if (isInside(pointInMap))
00096 pointsRing_.push_back(pointInMap);
00097 } else {
00098 pointsRing_.push_back(pointInMap);
00099 }
00100 }
00101 normal.x() = -signum(point.y());
00102 normal.y() = signum(point.x());
00103 if (normal.x() != 0
00104 && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_)
00105 point.x() += normal.x();
00106 else if (normal.y() != 0
00107 && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_)
00108 point.y() += normal.y();
00109 else {
00110 point.x() += normal.x();
00111 point.y() += normal.y();
00112 }
00113 } while (point.x() != distance_ || point.y() != 0);
00114 }
00115
00116 double SpiralIterator::getCurrentRadius() const
00117 {
00118 Index radius = *(*this) - indexCenter_;
00119 return radius.matrix().norm() * resolution_;
00120 }
00121
00122 }
00123