00001 00005 #pragma once 00006 00007 #include <Eigen/Core> 00008 00009 // unique_ptr 00010 #include <memory> 00011 #include <vector> 00012 00013 #include "../cost_map.hpp" 00014 00015 namespace cost_map { 00016 00020 class SpiralIterator 00021 { 00022 public: 00023 00030 SpiralIterator(const cost_map::CostMap& gridMap, const Eigen::Vector2d& center, const double radius); 00031 00037 SpiralIterator& operator =(const SpiralIterator& other); 00038 00043 bool operator !=(const SpiralIterator& other) const; 00044 00049 const Eigen::Array2i& operator *() const; 00050 00055 SpiralIterator& operator ++(); 00056 00061 bool isPastEnd() const; 00062 00067 double getCurrentRadius() const; 00068 00069 private: 00070 00075 bool isInside(const Index index) const; 00076 00081 void generateRing(); 00082 00083 int signum(const int val) { 00084 return (0 < val) - (val < 0); 00085 } 00086 00088 Position center_; 00089 Index indexCenter_; 00090 00091 00093 double radius_; 00094 00096 double radiusSquare_; 00097 00099 unsigned int nRings_; 00100 unsigned int distance_; 00101 std::vector<Index> pointsRing_; 00102 00104 Length mapLength_; 00105 Position mapPosition_; 00106 double resolution_; 00107 Size bufferSize_; 00108 }; 00109 00110 } /* namespace */