PolygonIterator.cpp
Go to the documentation of this file.
00001 /*
00002  * PolygonIterator.hpp
00003  *
00004  *  Created on: Sep 19, 2014
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_core/iterators/PolygonIterator.hpp"
00010 #include "grid_map_core/GridMapMath.hpp"
00011 
00012 using namespace std;
00013 
00014 namespace grid_map {
00015 
00016 PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
00017     : polygon_(polygon)
00018 {
00019   mapLength_ = gridMap.getLength();
00020   mapPosition_ = gridMap.getPosition();
00021   resolution_ = gridMap.getResolution();
00022   bufferSize_ = gridMap.getSize();
00023   bufferStartIndex_ = gridMap.getStartIndex();
00024   Index submapStartIndex;
00025   Size submapBufferSize;
00026   findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
00027   internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
00028   if(!isInside()) ++(*this);
00029 }
00030 
00031 PolygonIterator& PolygonIterator::operator =(const PolygonIterator& other)
00032 {
00033   polygon_ = other.polygon_;
00034   internalIterator_ = other.internalIterator_;
00035   mapLength_ = other.mapLength_;
00036   mapPosition_ = other.mapPosition_;
00037   resolution_ = other.resolution_;
00038   bufferSize_ = other.bufferSize_;
00039   bufferStartIndex_ = other.bufferStartIndex_;
00040   return *this;
00041 }
00042 
00043 bool PolygonIterator::operator !=(const PolygonIterator& other) const
00044 {
00045   return (internalIterator_ != other.internalIterator_);
00046 }
00047 
00048 const Index& PolygonIterator::operator *() const
00049 {
00050   return *(*internalIterator_);
00051 }
00052 
00053 PolygonIterator& PolygonIterator::operator ++()
00054 {
00055   ++(*internalIterator_);
00056   if (internalIterator_->isPastEnd()) return *this;
00057 
00058   for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
00059     if (isInside()) break;
00060   }
00061 
00062   return *this;
00063 }
00064 
00065 bool PolygonIterator::isPastEnd() const
00066 {
00067   return internalIterator_->isPastEnd();
00068 }
00069 
00070 bool PolygonIterator::isInside() const
00071 {
00072   Position position;
00073   getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00074   return polygon_.isInside(position);
00075 }
00076 
00077 void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const
00078 {
00079   Position topLeft = polygon_.getVertices()[0];
00080   Position bottomRight = topLeft;
00081   for (const auto& vertex : polygon_.getVertices()) {
00082     topLeft = topLeft.array().max(vertex.array());
00083     bottomRight = bottomRight.array().min(vertex.array());
00084   }
00085   boundPositionToRange(topLeft, mapLength_, mapPosition_);
00086   boundPositionToRange(bottomRight, mapLength_, mapPosition_);
00087   getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00088   Index endIndex;
00089   getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00090   bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
00091 }
00092 
00093 } /* namespace grid_map */
00094 


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:16