PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
: polygon_(polygon)
{
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
Index submapStartIndex;
Size submapBufferSize;
findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
if(!isInside()) ++(*this);
}
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
: center_(center)
{
semiAxisSquare_ = (0.5 * length).square();
double sinRotation = sin(rotation);
double cosRotation = cos(rotation);
transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
Index submapStartIndex;
Index submapBufferSize;
findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
if(!isInside()) ++(*this);
}
