2013年04月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;
}
}
}

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

■■■■■■■■■■■■
■*■***
■*■*■*■
■***■*■■■
■■**
*■■
■*
■■■*■■■■■■
■■■****■
■■■*****■*■
■■■*■
■■■■■■■■■■■■


それではまた次回に晴れ

posted by 管理人 at 14:03 | プログラミング