Go to the documentation of this file.00001
00002
00003
00004
00005
00006
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 }
00094