00001 00005 #pragma once 00006 00007 #include <Eigen/Core> 00008 00009 // unique_ptr 00010 #include <memory> 00011 #include "../iterators/submap_iterator.hpp" 00012 #include "../cost_map.hpp" 00013 00014 namespace cost_map { 00015 00020 class EllipseIterator 00021 { 00022 public: 00023 00031 EllipseIterator(const CostMap& gridMap, const Position& center, const Length& length, const double rotation = 0.0); 00032 00038 EllipseIterator& operator =(const EllipseIterator& other); 00039 00044 bool operator !=(const EllipseIterator& other) const; 00045 00050 const Index& operator *() const; 00051 00056 EllipseIterator& operator ++(); 00057 00062 bool isPastEnd() const; 00063 00064 private: 00065 00070 bool isInside() const; 00071 00080 void findSubmapParameters(const Position& center, const Length& length, const double rotation, 00081 Index& startIndex, Size& bufferSize) const; 00082 00084 Position center_; 00085 00087 Eigen::Array2d semiAxisSquare_; 00088 00090 Eigen::Matrix2d transformMatrix_; 00091 00093 std::shared_ptr<SubmapIterator> internalIterator_; 00094 00096 Length mapLength_; 00097 Position mapPosition_; 00098 double resolution_; 00099 Size bufferSize_; 00100 Index bufferStartIndex_; 00101 }; 00102 00103 } /* namespace */