void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const { Position topLeft = polygon_.getVertices()[0]; Position bottomRight = topLeft; for (const auto& vertex : polygon_.getVertices()) { topLeft = topLeft.array().max(vertex.array()); bottomRight = bottomRight.array().min(vertex.array()); } limitPositionToRange(topLeft, mapLength_, mapPosition_); limitPositionToRange(bottomRight, mapLength_, mapPosition_); getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); Index endIndex; getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); }
void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation, Index& startIndex, Size& bufferSize) const { const Eigen::Rotation2Dd rotationMatrix(rotation); Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0); Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1)); const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt(); Position topLeft = center.array() + boundingBoxHalfLength; Position bottomRight = center.array() - boundingBoxHalfLength; boundPositionToRange(topLeft, mapLength_, mapPosition_); boundPositionToRange(bottomRight, mapLength_, mapPosition_); getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); Index endIndex; getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); }