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


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