SpiralIterator.cpp
Go to the documentation of this file.
00001 /*
00002  * SpiralIterator.hpp
00003  *
00004  *  Created on: Jul 7, 2015
00005  *      Author: Martin Wermelinger
00006  *   Institute: ETH Zurich, ANYbotics
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 } /* namespace grid_map */
00123 


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13