| 445 | } |
| 446 | |
| 447 | bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions) { |
| 448 | Index indexShift; |
| 449 | Position positionShift = position - position_; |
| 450 | getIndexShiftFromPositionShift(indexShift, positionShift, resolution_); |
| 451 | Position alignedPositionShift; |
| 452 | getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_); |
| 453 | |
| 454 | // Delete fields that fall out of map (and become empty cells). |
| 455 | for (int i = 0; i < indexShift.size(); i++) { |
| 456 | if (indexShift(i) != 0) { |
| 457 | if (abs(indexShift(i)) >= getSize()(i)) { |
| 458 | // Entire map is dropped. |
| 459 | clearAll(); |
| 460 | newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined)); |
| 461 | } else { |
| 462 | // Drop cells out of map. |
| 463 | int sign = (indexShift(i) > 0 ? 1 : -1); |
| 464 | int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0); |
| 465 | int endIndex = startIndex - sign + indexShift(i); |
| 466 | int nCells = abs(indexShift(i)); |
| 467 | int index = (sign > 0 ? startIndex : endIndex); |
| 468 | wrapIndexToRange(index, getSize()(i)); |
| 469 | |
| 470 | if (index + nCells <= getSize()(i)) { |
| 471 | // One region to drop. |
| 472 | if (i == 0) { |
| 473 | clearRows(index, nCells); |
| 474 | newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); |
| 475 | } else if (i == 1) { |
| 476 | clearCols(index, nCells); |
| 477 | newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined)); |
| 478 | } |
| 479 | } else { |
| 480 | // Two regions to drop. |
| 481 | int firstIndex = index; |
| 482 | int firstNCells = getSize()(i) - firstIndex; |
| 483 | if (i == 0) { |
| 484 | clearRows(firstIndex, firstNCells); |
| 485 | newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); |
| 486 | } else if (i == 1) { |
| 487 | clearCols(firstIndex, firstNCells); |
| 488 | newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined)); |
| 489 | } |
| 490 | |
| 491 | int secondIndex = 0; |
| 492 | int secondNCells = nCells - firstNCells; |
| 493 | if (i == 0) { |
| 494 | clearRows(secondIndex, secondNCells); |
| 495 | newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); |
| 496 | } else if (i == 1) { |
| 497 | clearCols(secondIndex, secondNCells); |
| 498 | newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined)); |
| 499 | } |
| 500 | } |
| 501 | } |
| 502 | } |
| 503 | } |
| 504 | |