More than 3 years have passed since last update.

A*アルゴリズムによる海上の経路探索🚢

Last updated at 2020-11-23Posted at 2019-04-07

⚓最短航路の作成⚓

今回は自動生成したワールドデータに航路を自動生成していきます。

💻A*による経路探索💻

探索アルゴリズムである**"A*アルゴリズム"**を使用して最短経路を求めます。
下図のようにコストが低い場所から優先して探索していきます。

⚙Vec2クラスとNodeクラスの作成⚙

まずは基本クラスである**"Vec2クラス"と"Nodeクラス"**の作成です。

Vec2

 struct AStarVec2 {
  //X座標値を格納する
  std::int_fast32_t x{};
  //Y座標値を格納する
  std::int_fast32_t y{};
  constexpr AStarVec2() noexcept = default;
  constexpr AStarVec2(const std::int_fast32_t x_, const std::int_fast32_t y_) noexcept :x(x_), y(y_) {}
  constexpr bool operator==(const AStarVec2& vec2_) const noexcept {
   return vec2_.x == x && vec2_.y == y;
  }
 };

Node

 struct AStarNode {
  //位置
  AStarVec2 position{};
  //親ノード
  AStarVec2 parent_node{};
  //距離
  std::int_fast32_t distance{};
  //コスト
  std::int_fast32_t cost{};

  constexpr AStarNode() noexcept = default;
  constexpr AStarNode(const AStarVec2& pos_, const AStarVec2& parent_node_, const std::int_fast32_t distance_, const std::int_fast32_t cost_) noexcept
   :position(pos_), parent_node(parent_node_), distance(distance_), cost(cost_) {}
  constexpr bool operator==(const AStarNode& node_) const noexcept {
   return position == node_.position;
  }
  constexpr bool operator==(const AStarVec2& vec2_) const noexcept {
   return position == vec2_;
  }
 };

⭐️A*クラスの実装⭐️

次は肝心のA*クラスの作成です。

AStar基本部分

 class AStar {
 private:
 //始点
  AStarVec2 start_vec2{};
 //終点
  AStarVec2 end_vec2{};
  std::array <AStarVec2, 8> adjacent_cell{ {AStarVec2(-1, -1),AStarVec2(1, -1),AStarVec2(-1, 1),AStarVec2(1, 1),AStarVec2(0, -1),AStarVec2(-1, 0),AStarVec2(0, 1),AStarVec2(1, 0)} };
  std::list<AStarNode> open{};
  std::list<AStarNode> closed{};

 public:
  constexpr AStar() noexcept = default;
  constexpr AStar(const AStarVec2& start_vec2_, const AStarVec2& end_vec2_) noexcept
   :start_vec2(start_vec2_), end_vec2(end_vec2_) {}
};

⚙ヒューリスティック関数⚙

コストを算出する関数を作成します。

private

  constexpr std::int_fast32_t calculateDistance(const AStarVec2 & vec2_) const noexcept {
   const std::int_fast32_t x{ end_vec2.x - vec2_.x };
   const std::int_fast32_t y{ end_vec2.y - vec2_.y };
   return (x >= y) ? x : y;
  }

⚙範囲指定⚙

private

  constexpr bool isRange(const AStarVec2 & vec2_, const std::int_fast32_t x_, const std::int_fast32_t y_) const noexcept {
   return vec2_.x >= 0 && vec2_.y >= 0 && vec2_.x < x_ && vec2_.y < y_;
  }

⚙座標判定⚙

private

  bool existPoint(const AStarVec2 & vec2_, const std::int_fast32_t cost_) noexcept {
   auto i{ std::find(closed.begin(), closed.end(), vec2_) };
   if (i != closed.end()) {
    if ((*i).cost + (*i).distance < cost_) return true;
    else {
     closed.erase(i);
     return false;
    }
   }
   i = std::find(open.begin(), open.end(), vec2_);
   if (i != open.end()) {
    if ((*i).cost + (*i).distance < cost_) return true;
    else {
     open.erase(i);
     return false;
    }
   }
   return false;
  }

このように周りから探索していきます。

⚙周囲のセルを開く⚙

private

  template<typename Matrix_>
  bool cellOpen(const AStarNode & node_, const Matrix_ & matrix_, const std::int_fast32_t x_, const std::int_fast32_t y_) noexcept {
   std::int_fast32_t node_cost;
   std::int_fast32_t distance;
   AStarVec2 neighbour;
   for (std::int_fast32_t x{}; x < 8; ++x) {
    neighbour.x = node_.position.x + adjacent_cell[x].x;
    neighbour.y = node_.position.y + adjacent_cell[x].y;
    if (neighbour == end_vec2) return true;
    if (!isRange(neighbour, x_, y_) || matrix_[neighbour.y][neighbour.x]) continue;
    //コスト計算
    node_cost = 1 + node_.cost;
    distance = calculateDistance(neighbour);
    if (existPoint(neighbour, node_cost + distance)) continue;
    open.emplace_back(AStarNode(neighbour, node_.position, distance, node_cost));
   }
   return false;
  }

⚙探索⚙

public

  template<typename Matrix_>
  bool search(const Matrix_ & matrix_, const std::int_fast32_t x_, const std::int_fast32_t y_) noexcept {
   open.emplace_back(AStarNode(start_vec2, {}, calculateDistance(start_vec2), 0));
   while (!open.empty()) {
    const AStarNode nn{ open.front() };
    open.pop_front();
    closed.emplace_back(nn);
    if (cellOpen(nn, matrix_, x_, y_)) return true;
   }
   return false;
  }

⚙探索結果の保存⚙

public

  template<typename Path_>
  std::int_fast32_t setPath(Path_ & path_) noexcept {
   path_.emplace_front(end_vec2);
   std::int_fast32_t cost{ 1 + closed.back().cost };
   path_.emplace_front(closed.back().position);
   AStarVec2 parent_node{ closed.back().parent_node };
   for (auto&& i{ closed.rbegin() }; i != closed.rend(); ++i) {
    if (!((*i).position == parent_node) || ((*i).position == start_vec2)) continue;
    path_.emplace_front((*i).position);
    parent_node = (*i).parent_node;
   }
   path_.emplace_front(start_vec2);
   return cost;
  }