13 #include <Eigen/Geometry> 21 double sinRotation = std::sin(rotation);
22 double cosRotation = std::cos(rotation);
29 Index submapStartIndex;
30 Index submapBufferSize;
32 internalIterator_ = std::make_shared<SubmapIterator>(gridMap, submapStartIndex, submapBufferSize);
45 return *(*internalIterator_);
50 ++(*internalIterator_);
83 Index& startIndex,
Size& bufferSize)
const 85 const Eigen::Rotation2Dd rotationMatrix(rotation);
86 Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
87 Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
88 const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
89 Position topLeft = center.array() + boundingBoxHalfLength;
90 Position bottomRight = center.array() - boundingBoxHalfLength;
Length mapLength_
Map information needed to get position from iterator.
const Index & operator*() const
void findSubmapParameters(const Position ¢er, const Length &length, double rotation, Index &startIndex, Size &bufferSize) const
const Index & getStartIndex() const
Position center_
Position of the circle center;.
const Size & getSize() const
EllipseIterator & operator++()
EllipseIterator(const GridMap &gridMap, const Position ¢er, const Length &length, double rotation=0.0)
double getResolution() const
Eigen::Matrix2d transformMatrix_
Sine and cosine values of the rotation angle as transformation matrix.
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Size getSubmapSizeFromCornerIndices(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator. // TODO Think of using unique_ptr instead.
const Size & getSubmapSize() const
bool operator!=(const EllipseIterator &other) const
Eigen::Array2d semiAxisSquare_
Square length of the semi axis.
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
bool getPosition(const Index &index, Position &position) const
const Length & getLength() const