HEXA BLOG

プログラム

HEXA BLOGプログラム2013.4.19

経路探索アルゴリズム

なんと ドラゴンが おきあがり
なかまに なりたそうに こちらをみている!
なかまに してあげますか?
>はい
 いいえ
ドラゴンが なかまに くわわった!
・・・
そんなこんながありまして、ヘキサドライブの一員となりましたるんるん
はじめまして、ドラゴンですグッド(上向き矢印)
ヘキサドライブは志の高い人が多く、それに刺激されて僕も負けじとレベルアップを目指す毎日です手(グー)
より良いコンテンツを作っていけるよう、頑張っていきたいと思います。
さて、少し前に大阪メンバーで集まってお花見をしていましたが、同時期に東京メンバーもお花見をしましたかわいい
それの場所取りで皆を待っている時間に「Rasende Roboter」というボードゲームをしたのですが、一言で言うとゴールまでの最短経路を探すゲームで、プログラマ3人でプレイしたために本気の戦いを繰り広げてしまいましたexclamation
とても面白いボードゲームなので、興味を持たれた方は是非プレイしてみて下さい。
経路探索といえば、ゲームでも敵キャラのAIなどで実装されますねひらめき
今回は探索アルゴリズムの中でも割とポピュラーな A* を紹介しようと思います。
A*の概要に関してはこちらを参照して下さい。
A*の特徴としては、単純に全ての経路を調べていくのではなく、よりゴールに近いと推測される方を優先して調べることにより、評価回数を減らすことができる点ですね。
以下の条件下でA*を実装してみました。
・移動は上下左右のみ
・隣接ノード間の移動コストは常に1
・マップの左上端のノードをスタートとし、右下端のノードをゴールとする
マップデータ(MapData.h)

namespace Map
{
// マップデータ(0:壁、1:通路)
static const int DATA[] = {
1,0,1,1,1,1,1,1,0,1,
1,0,1,0,1,0,1,1,1,1,
1,1,1,0,1,0,1,0,0,1,
1,0,0,1,1,1,1,1,0,1,
1,0,1,1,0,0,1,1,0,1,
1,1,0,1,1,1,1,1,0,1,
0,1,0,1,0,1,0,0,0,0,
0,1,0,1,1,0,1,1,1,1,
0,1,0,1,1,1,1,1,0,1,
1,1,0,0,1,0,1,1,0,1,
};
// マップサイズ
static const int WIDTH  = 10;    // 幅
static const int HEIGHT = 10;    // 高さ
// スタート地点
static const int START_X = 0;
static const int START_Y = 0;
// ゴール地点
static const int GOAL_X = WIDTH - 1;
static const int GOAL_Y = HEIGHT - 1;
}

…ちょっとわかりにくいですね。
マップはこんな感じです。
■■■■■■■■■■■■


■■
■■
■■

■■■■■■■
■■
■■
■■
■■■■■■■■■■■■
main.cpp

#include <functional>
#include <iostream>
#include <vector>
#include <queue>
#include <memory>
#include "MapData.h"
//===========================================================================
// ノードクラス
//===========================================================================
class Node
{
public:
//-----------------------------------------------------------
//! @name 初期化
//-----------------------------------------------------------
//@{
Node(int posX, int posY)
: _pParentNode(nullptr)
, _posX(posX)
, _posY(posY)
, _costFromStartNode(0)
, _costToGoalNode(0)
{}
//@}
//-----------------------------------------------------------
//! @name setter/getter
//-----------------------------------------------------------
//@{
Node& SetParentNode(Node* pNode) { _pParentNode = pNode; return *this; }
Node* GetParentNode(void) const { return _pParentNode; }
Node& SetPosX(int posX) { _posX = posX; return *this; }
int GetPosX(void) const { return _posX; }
Node& SetPosY(int posY) { _posY = posY; return *this; }
int GetPosY(void) const { return _posY; }
Node& SetCostFromStartNode(int costFromStartNode) { _costFromStartNode = costFromStartNode; return *this; }
int GetCostFromStartNode(void) const { return _costFromStartNode; }
Node& SetCostToGoalNode(int costToGoalNode) { _costToGoalNode = costToGoalNode; return *this; }
int GetCostToGoalNode(void) const { return _costToGoalNode; }
int GetTotalCost(void) const { return _costFromStartNode + _costToGoalNode; }
//@}
//-----------------------------------------------------------
//! @name operator
//-----------------------------------------------------------
//@{
bool operator == (Node node)
{
return (this->_posX == node._posX && this->_posY == node._posY);
}
void operator = (Node node)
{
this->_pParentNode            = node._pParentNode;
this->_posX                    = node._posX;
this->_posY                    = node._posY;
this->_costFromStartNode    = node._costFromStartNode;
this->_costToGoalNode        = node._costToGoalNode;
}
//@}
private:
Node* _pParentNode;       // 親ノード
int   _posX;              // X座標
int   _posY;              // Y座標
int   _costFromStartNode; // スタートノードからの最小コスト
int   _costToGoalNode;    // ゴールノードまでの最小コスト
};
typedef std::shared_ptr<Node> NodePtr;
typedef std::vector<NodePtr> NodePtrVector;
namespace {
//---------------------------------------------------------------------------
//  壁判定
//! @param [in] posX X座標
//! @param [in] posY Y座標
//! @return 壁ならtrue
//---------------------------------------------------------------------------
bool IsWall(int posX, int posY)
{
if (posX < 0 || Map::WIDTH  <= posX) return true;
if (posY < 0 || Map::HEIGHT <= posY) return true;
return (Map::DATA[posX + posY * Map::WIDTH] == 0);
}
//---------------------------------------------------------------------------
//  ゴールまでの推定コストを計算
//! @param [in] posX X座標
//! @param [in] posY Y座標
//! @return ゴールまでの距離
//---------------------------------------------------------------------------
int CalcCostToGoalNode(int posX, int posY)
{
return std::abs(Map::GOAL_X - posX) + std::abs(Map::GOAL_Y - posY);
}
//---------------------------------------------------------------------------
//  ゴールまでの推定コストを計算
//! @param [in] node ノード
//! @return ゴールまでの距離
//---------------------------------------------------------------------------
int CalcCostToGoalNode(const Node& node)
{
return CalcCostToGoalNode(node.GetPosX(), node.GetPosY());
}
}
//---------------------------------------------------------------------------
//    スタートアップ
//---------------------------------------------------------------------------
int main()
{
//------------------------------------------------------------
// 変数定義
//------------------------------------------------------------
NodePtrVector openList;
NodePtrVector closeList;
//------------------------------------------------------------
// ラムダ式を用いた関数定義
//------------------------------------------------------------
// Node位置比較用関数
auto compareNodeByTotalCost = [](NodePtr pNode1, NodePtr pNode2) -> int
{
return pNode1->GetTotalCost() > pNode2->GetTotalCost();
};
// リスト内に含まれているかどうかの判定用関数
auto isInNodeList = [](NodePtrVector& list, const NodePtr& node) -> NodePtr
{
for (NodePtrVector::iterator it = list.begin(); it != list.end(); ++it)
{
NodePtr nodeItem = (*it);
if (*node == *nodeItem)
{
return nodeItem;
}
}
return nullptr;
};
//------------------------------------------------------------
// A*のアルゴリズム
//------------------------------------------------------------
// スタートノード
NodePtr pStartNode(new Node(Map::START_X, Map::START_Y));
int startNodeCostToGoalNode = CalcCostToGoalNode(*pStartNode);
pStartNode->SetCostToGoalNode(startNodeCostToGoalNode);
// ゴールノード
NodePtr pGoalNode(new Node(Map::GOAL_X, Map::GOAL_Y));
// オープンリストから取り出す
openList.push_back(pStartNode);
while (true)
{
// オープンリストが空なら検索失敗
if (openList.empty())
{
std::cout << "探索失敗" << std::endl;
exit(1);
}
// 最小コストのノードをオープンリストから取り出す
std::sort(openList.begin(), openList.end(), compareNodeByTotalCost);
NodePtr pBaseNode = openList.back();
openList.pop_back();
// ゴールノードと一致したら検索終了
if (*pBaseNode == *pGoalNode)
{
*pGoalNode = *pBaseNode;
break;
}
// 取り出したノードをクローズリストに移す
closeList.push_back(pBaseNode);
// 隣接ノードをチェック
// 今回は上下左右のみ
for (int dy = -1; dy <= 1; ++dy)
{
for (int dx = -1; dx <= 1; ++dx)
{
// 同位置判定
if (dx == 0 && dy == 0) continue;
// 斜めを考慮しない
if (dx != 0 && dy != 0) continue;
// 隣接ノード位置
int pAdjacentNodePosX = pBaseNode->GetPosX() + dx;
int pAdjacentNodePosY = pBaseNode->GetPosY() + dy;
// 壁判定
if (IsWall(pAdjacentNodePosX, pAdjacentNodePosY)) continue;
// 隣接ノードの各コスト
int adjacentNodeCostFromStart  = pBaseNode->GetCostFromStartNode() + 1;  // 親から子への移動コストは1
int adjacentNodeCostToGoalNode = CalcCostToGoalNode(pAdjacentNodePosX, pAdjacentNodePosY);
// 隣接ノード
NodePtr pAdjacentNode(new Node(pAdjacentNodePosX, pAdjacentNodePosY));
pAdjacentNode->SetParentNode(pBaseNode.get())
.SetCostFromStartNode(adjacentNodeCostFromStart)
.SetCostToGoalNode(adjacentNodeCostToGoalNode);
NodePtr pSearchedNode = nullptr;
// オープンリストに含まれているか
pSearchedNode = isInNodeList(openList, pAdjacentNode);
if (pSearchedNode)
{
// オープンリストにあったノードより隣接ノードのコストが小さければ、オープンリストのノードを上書き
if (pAdjacentNode->GetTotalCost() < pSearchedNode->GetTotalCost())
{
*pSearchedNode = *pAdjacentNode;
}
continue;
}
// クローズリストに含まれているか
pSearchedNode = isInNodeList(closeList, pAdjacentNode);
if (pSearchedNode)
{
// クローズリストにあったノードより隣接ノードのコストが小さければ、クローズリストから削除してオープンリストに追加
if (pAdjacentNode->GetTotalCost() < pSearchedNode->GetTotalCost())
{
std::remove(closeList.begin(), closeList.end(), pSearchedNode);
openList.push_back(pAdjacentNode);
}
continue;
}
// どちらにも含まれていなければオープンリストに追加
openList.push_back(pAdjacentNode);
}
}
}
//------------------------------------------------------------
// 結果
//------------------------------------------------------------
// ゴールノードから親ノードを辿ることで、スタートノードまでの経路が算出される
Node* pNode = pGoalNode.get();
while (true)
{
std::cout << "X:" << pNode->GetPosX() << ", Y:" << pNode->GetPosY() << std::endl;
if ((pNode = pNode->GetParentNode()) == nullptr)
{
break;
}
}
}

結果は以下の通りになりました。
無事に最短経路を通ることができてますね手(チョキ)
■■■■■■■■■■■■
■*■***
■*■*■*■
■***■*■■■
■■**
*■■
■*
■■■*■■■■■■
■■■****■
■■■*****■*■
■■■*■
■■■■■■■■■■■■
それではまた次回に晴れ

RECRUIT

大阪・東京共にスタッフを募集しています。
特にキャリア採用のプログラマー・アーティストに興味がある方は下のボタンをクリックしてください

RECRUIT SITE