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


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