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