EllipseIterator.cpp
Go to the documentation of this file.
00001 /*
00002  * EllipseIterator.hpp
00003  *
00004  *  Created on: Dec 2, 2015
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_core/iterators/EllipseIterator.hpp"
00010 #include "grid_map_core/GridMapMath.hpp"
00011 
00012 #include <math.h>
00013 #include <Eigen/Geometry>
00014 
00015 using namespace std;
00016 
00017 namespace grid_map {
00018 
00019 EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
00020     : center_(center)
00021 {
00022   semiAxisSquare_ = (0.5 * length).square();
00023   double sinRotation = sin(rotation);
00024   double cosRotation = cos(rotation);
00025   transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
00026   mapLength_ = gridMap.getLength();
00027   mapPosition_ = gridMap.getPosition();
00028   resolution_ = gridMap.getResolution();
00029   bufferSize_ = gridMap.getSize();
00030   bufferStartIndex_ = gridMap.getStartIndex();
00031   Index submapStartIndex;
00032   Index submapBufferSize;
00033   findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
00034   internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
00035   if(!isInside()) ++(*this);
00036 }
00037 
00038 EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other)
00039 {
00040   center_ = other.center_;
00041   semiAxisSquare_ = other.semiAxisSquare_;
00042   transformMatrix_ = other.transformMatrix_;
00043   internalIterator_ = other.internalIterator_;
00044   mapLength_ = other.mapLength_;
00045   mapPosition_ = other.mapPosition_;
00046   resolution_ = other.resolution_;
00047   bufferSize_ = other.bufferSize_;
00048   bufferStartIndex_ = other.bufferStartIndex_;
00049   return *this;
00050 }
00051 
00052 bool EllipseIterator::operator !=(const EllipseIterator& other) const
00053 {
00054   return (internalIterator_ != other.internalIterator_);
00055 }
00056 
00057 const Eigen::Array2i& EllipseIterator::operator *() const
00058 {
00059   return *(*internalIterator_);
00060 }
00061 
00062 EllipseIterator& EllipseIterator::operator ++()
00063 {
00064   ++(*internalIterator_);
00065   if (internalIterator_->isPastEnd()) return *this;
00066 
00067   for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
00068     if (isInside()) break;
00069   }
00070 
00071   return *this;
00072 }
00073 
00074 bool EllipseIterator::isPastEnd() const
00075 {
00076   return internalIterator_->isPastEnd();
00077 }
00078 
00079 const Size& EllipseIterator::getSubmapSize() const
00080 {
00081   return internalIterator_->getSubmapSize();
00082 }
00083 
00084 bool EllipseIterator::isInside() const
00085 {
00086   Position position;
00087   getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00088   double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
00089   return (value <= 1);
00090 }
00091 
00092 void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
00093                                            Index& startIndex, Size& bufferSize) const
00094 {
00095   const Eigen::Rotation2Dd rotationMatrix(rotation);
00096   Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
00097   Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
00098   const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
00099   Position topLeft = center.array() + boundingBoxHalfLength;
00100   Position bottomRight = center.array() - boundingBoxHalfLength;
00101   boundPositionToRange(topLeft, mapLength_, mapPosition_);
00102   boundPositionToRange(bottomRight, mapLength_, mapPosition_);
00103   getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00104   Index endIndex;
00105   getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00106   bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
00107 }
00108 
00109 } /* namespace grid_map */
00110 


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