| 547 | } |
| 548 | |
| 549 | bool GridMap::extendToInclude(const GridMap& other) { |
| 550 | // Get dimension of maps. |
| 551 | Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0); |
| 552 | Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0); |
| 553 | Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0); |
| 554 | Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0, |
| 555 | other.getPosition().y() - other.getLength().y() / 2.0); |
| 556 | // Check if map needs to be resized. |
| 557 | bool resizeMap = false; |
| 558 | Position extendedMapPosition = position_; |
| 559 | Length extendedMapLength = length_; |
| 560 | if (topLeftCornerOther.x() > topLeftCorner.x()) { |
| 561 | extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0; |
| 562 | extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x(); |
| 563 | resizeMap = true; |
| 564 | } |
| 565 | if (topLeftCornerOther.y() > topLeftCorner.y()) { |
| 566 | extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0; |
| 567 | extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y(); |
| 568 | resizeMap = true; |
| 569 | } |
| 570 | if (bottomRightCornerOther.x() < bottomRightCorner.x()) { |
| 571 | extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0; |
| 572 | extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x(); |
| 573 | resizeMap = true; |
| 574 | } |
| 575 | if (bottomRightCornerOther.y() < bottomRightCorner.y()) { |
| 576 | extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0; |
| 577 | extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y(); |
| 578 | resizeMap = true; |
| 579 | } |
| 580 | // Resize map and copy data to new map. |
| 581 | if (resizeMap) { |
| 582 | GridMap mapCopy = *this; |
| 583 | setGeometry(extendedMapLength, resolution_, extendedMapPosition); |
| 584 | // Align new map with old one. |
| 585 | Vector shift = position_ - mapCopy.getPosition(); |
| 586 | shift.x() = std::fmod(shift.x(), resolution_); |
| 587 | shift.y() = std::fmod(shift.y(), resolution_); |
| 588 | if (std::abs(shift.x()) < resolution_ / 2.0) { |
| 589 | position_.x() -= shift.x(); |
| 590 | } else { |
| 591 | position_.x() += resolution_ - shift.x(); |
| 592 | } |
| 593 | if (size_.x() % 2 != mapCopy.getSize().x() % 2) { |
| 594 | position_.x() += -std::copysign(resolution_ / 2.0, shift.x()); |
| 595 | } |
| 596 | if (std::abs(shift.y()) < resolution_ / 2.0) { |
| 597 | position_.y() -= shift.y(); |
| 598 | } else { |
| 599 | position_.y() += resolution_ - shift.y(); |
| 600 | } |
| 601 | if (size_.y() % 2 != mapCopy.getSize().y() % 2) { |
| 602 | position_.y() += -std::copysign(resolution_ / 2.0, shift.y()); |
| 603 | } |
| 604 | // Copy data. |
| 605 | for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) { |
| 606 | if (isValid(*iterator)) continue; |