| 562 | } |
| 563 | |
| 564 | void HGrid::getGridTour(const vector<int>& ids, const Eigen::Vector3d& pos, |
| 565 | vector<Eigen::Vector3d>& tour, vector<Eigen::Vector3d>& tour2) { |
| 566 | const int grid_num1 = grid1_->grid_data_.size(); |
| 567 | |
| 568 | // Get the centers of the visited grids |
| 569 | vector<Eigen::Vector3d> centers = { pos }; |
| 570 | for (auto id : ids) { |
| 571 | if (id < grid_num1) { |
| 572 | centers.push_back(grid1_->grid_data_[id].center_); |
| 573 | } else { |
| 574 | int tmp = id - grid_num1; |
| 575 | centers.push_back(grid2_->grid_data_[tmp].center_); |
| 576 | } |
| 577 | } |
| 578 | tour = centers; |
| 579 | |
| 580 | // Find the exact path visiting the grids |
| 581 | tour2 = { pos }; |
| 582 | for (int i = 0; i < centers.size() - 1; ++i) { |
| 583 | path_finder_->reset(); |
| 584 | if (path_finder_->search(centers[i], centers[i + 1]) == Astar::REACH_END) { |
| 585 | auto path = path_finder_->getPath(); |
| 586 | tour2.insert(tour2.end(), path.begin() + 1, path.end()); |
| 587 | } else { |
| 588 | tour2.push_back(centers[i + 1]); |
| 589 | } |
| 590 | } |
| 591 | } |
| 592 | |
| 593 | void HGrid::getFrontiersInGrid(const vector<int>& grid_ids, vector<int>& ftr_ids) { |
| 594 | ftr_ids.clear(); |