Go to the documentation of this file.00001
00004
00005
00006
00007
00008 #include <cost_map_core/iterators/line_iterator.hpp>
00009 #include <grid_map_core/GridMapMath.hpp>
00010
00011
00012 using namespace std;
00013
00014 namespace cost_map {
00015
00016 LineIterator::LineIterator(const cost_map::CostMap& cost_map, const Position& start, const Position& end)
00017 {
00018 Index startIndex, endIndex;
00019 if (getIndexLimitedToMapRange(cost_map, start, end, startIndex) &&
00020 getIndexLimitedToMapRange(cost_map, end, start, endIndex))
00021 {
00022 initialize(cost_map, startIndex, endIndex);
00023 }
00024 }
00025
00026 LineIterator::LineIterator(const cost_map::CostMap& cost_map, const Index& start, const Index& end)
00027 {
00028 initialize(cost_map, start, end);
00029 }
00030
00031 LineIterator& LineIterator::operator =(const LineIterator& other)
00032 {
00033 index_ = other.index_;
00034 start_ = other.start_;
00035 end_ = other.end_;
00036 iCell_ = other.iCell_;
00037 nCells_ = other.nCells_;
00038 increment1_ = other.increment1_;
00039 increment2_ = other.increment2_;
00040 denominator_ = other.denominator_;
00041 numerator_ = other.numerator_;
00042 numeratorAdd_ = other.numeratorAdd_;
00043 mapLength_ = other.mapLength_;
00044 mapPosition_ = other.mapPosition_;
00045 resolution_ = other.resolution_;
00046 bufferSize_ = other.bufferSize_;
00047 bufferStartIndex_ = other.bufferStartIndex_;
00048 return *this;
00049 }
00050
00051 bool LineIterator::operator !=(const LineIterator& other) const
00052 {
00053 return (index_ != other.index_).any();
00054 }
00055
00056 const Index& LineIterator::operator *() const
00057 {
00058 return index_;
00059 }
00060
00061 LineIterator& LineIterator::operator ++()
00062 {
00063 numerator_ += numeratorAdd_;
00064 if (numerator_ >= denominator_) {
00065 numerator_ -= denominator_;
00066 index_ += increment1_;
00067 }
00068 index_ += increment2_;
00069 ++iCell_;
00070 return *this;
00071 }
00072
00073 bool LineIterator::isPastEnd() const
00074 {
00075 return iCell_ >= nCells_;
00076 }
00077
00078 bool LineIterator::initialize(const cost_map::CostMap& cost_map, const Index& start, const Index& end)
00079 {
00080 start_ = start;
00081 end_ = end;
00082 mapLength_ = cost_map.getLength();
00083 mapPosition_ = cost_map.getPosition();
00084 resolution_ = cost_map.getResolution();
00085 bufferSize_ = cost_map.getSize();
00086 bufferStartIndex_ = cost_map.getStartIndex();
00087 Index submapStartIndex;
00088 Size submapBufferSize;
00089 initializeIterationParameters();
00090 return true;
00091 }
00092
00093 bool LineIterator::getIndexLimitedToMapRange(const cost_map::CostMap& cost_map,
00094 const Position& start, const Position& end,
00095 Index& index)
00096 {
00097 Position newStart = start;
00098 Vector direction = (end - start).normalized();
00099 while (!cost_map.getIndex(newStart, index)) {
00100 newStart += (cost_map.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
00101 if ((end - newStart).norm() < cost_map.getResolution() - std::numeric_limits<double>::epsilon())
00102 return false;
00103 }
00104 return true;
00105 }
00106
00107 void LineIterator::initializeIterationParameters()
00108 {
00109 iCell_ = 0;
00110 index_ = start_;
00111
00112 Size delta = (end_ - start_).abs();
00113
00114 if (end_.x() >= start_.x()) {
00115
00116 increment1_.x() = 1;
00117 increment2_.x() = 1;
00118 } else {
00119
00120 increment1_.x() = -1;
00121 increment2_.x() = -1;
00122 }
00123
00124 if (end_.y() >= start_.y()) {
00125
00126 increment1_.y() = 1;
00127 increment2_.y() = 1;
00128 } else {
00129
00130 increment1_.y() = -1;
00131 increment2_.y() = -1;
00132 }
00133
00134 if (delta.x() >= delta.y()) {
00135
00136 increment1_.x() = 0;
00137 increment2_.y() = 0;
00138 denominator_ = delta.x();
00139 numerator_ = delta.x() / 2;
00140 numeratorAdd_ = delta.y();
00141 nCells_ = delta.x() + 1;
00142 } else {
00143
00144 increment2_.x() = 0;
00145 increment1_.y() = 0;
00146 denominator_ = delta.y();
00147 numerator_ = delta.y() / 2;
00148 numeratorAdd_ = delta.x();
00149 nCells_ = delta.y() + 1;
00150 }
00151 }
00152
00153 }