A*アルゴリズムまとめ

数年ぶりにA*アルゴリズムを実装したので、まとめなおしておきます。何回かじっくり読むとようやく理解が出来てきました。基本的にそんなに難しくないアルゴリズムです。

概要

「A*アルゴリズム」は、A-Star（エースター）と読み、パス探索アルゴリズムの一つです。
ノードネットワーク上にスタート地点とゴール地点を結ぶパスが存在すれば、最悪でもそのパスの存在を確認できるアルゴリズムです（見落とすことがない）。
内容はシンプルな再帰計算で、コスト計算の部分にヒューリスティックと呼ばれるパラメータがあり、その算出アルゴリズムを変更することにより、各種アプリの状況に応じた高速化と最適化が望めます。

2007-07-16 修正

「A* - Wikipedia」の擬似コードを見ていたら、自分の書いたコードでは最適なルートを計算しない可能性があることが分かりましたので修正しました。下記のC++的擬似コード内では、隣接ノードを計算するところです。
参考書を見直してみたら、「AI Game Programming Wisdom」の擬似コード(P.107)にはそういう部分があったけど、「ゲーム開発者のためのAI入門」の擬似コード(P.128)には書かれてないですね。

C++的擬似コード（簡略コード）

擬似コードというか、実際に動かしたC++コードを簡略化したコードを載せておきます。各種参考書にあるものより実践的で、細かいところが分かると思います。

class CNode .. 3Dノードデータ

class CNode{
public: // パラメータ.
  VEC3 pos; // ノードの座標 [x,y,z]
  list<CNode*> mNeighborList; // 隣接ノードのリスト.
  
  int g; // スタート地点からこのノードまでのコスト.
  int h; // このノードからゴールまでの推定コスト.
  CNode* mpParent; // パスにしたときの一つ前のノード.

public: // メソッド.
  // 前のノードを設定.
  void setParent( CNode* pParent );
  // 指定ノードまでのコストを計算.
  void calcCostFrom( CNode* pNode );
  // ゴールノードを渡し、現在のコストを計算.
  void calcCost( CNode* pGoal );
  // (g+h)を返します.
  int  getTotalCost();

  // 親ノード、ゴールノードを仮定してコストを返します.
  int  getEstimateCost( CNode* pParent, CNode* pGoal );
};

setParent .. ノードポインタ（参照）をコピーするだけ。スタートノードの場合は必要ないので、NULLなどでそれが分かるように設定できる必要もある。
calcCostFrom .. ノード間のコストは、とりあえず「距離の二乗」で計算すると簡単で便利。単に「距離」としないのは、比較に使うだけなのでsqrt計算を省略するためです。
calcCost .. pParentのgetTotalCostとpParentとの距離を加算してgを計算, ゴールノードとの距離の二乗からhを計算します。

ノードに関係する関数を準備

// A*でパスを計算します.
calcPath(list<CNode*>& path, CNode* pStart, CNode* pGoal);
  
// リスト中最もコストが低いノードを返す補助関数.
getBestNode( list<CNode*>& nodeList );

// リストにノードが入っているかどうかを返す補助関数.
isInTheList( CNode* pNode, list<CNode*>& list );

A*計算部分

calcPath(
  list<CNode*>& path, // 出来たパスを格納するノードリスト.
  CNode* pStart,      // スタートノードへのポインタ.
  CNode* pGoal )      // ゴールノードへのポインタ.
{
  bool         bReachGoal = false; // パス作成完了フラグ.
  list<CNode*> aOpenList;    // オープンリスト.
  list<CNode*> aClosedList;  // クローズドリスト.
  
  //------------------------- 
  // A* 本編.
  //------------------------- 
  pStart->setParent( NULL ); // 最初のノードに設定.
  pStart->calcCost( pGoal ); // コスト計算.
  aOpenList.push_back( pStart ); // オープンリストに追加.
  while( aOpenList.size() > 0 ){
    CNode* pB = getBestNode( aOpenList );
    if( pB == NULL ){ return; } // パス作成失敗→終了.
    if( pB == pGoal ){
      bReachGoal = true;
      break; // ゴールまで到達したので終了.
    }

    // pBの隣接ノードについて調べます.
    list<CNode*>::iterator ppBN
      = pB->mNeighborList.begin();
    for( ; ppBN!=pB->mNeighborList.end(); ppBN++ ){
      if( isInTheList((*ppBN),aOpenList) ){
        // オープンリストに入っているとき.
        // 現在のノードを親にしたとき、既存コストより小さくなれば、更新.
        if( getEstimateCost( *ppBN, pGoal ) < (*ppBN)->getCost() ){
          (*ppBN)->setParent( pB );    // 親を設定.
          (*ppBN)->calcCost( pGoal );  // コストを計算.
        }
      }
      else if( isInTheList((*ppBN),aClosedList) ){
        // クローズドリストに入っているとき.
        // 現在のノードを親にしたとき、既存コストより小さくなれば、更新.
        if( getEstimateCost( *ppBN, pGoal ) < (*ppBN)->getCost() ){
          (*ppBN)->setParent( pB );    // 親を設定.
          (*ppBN)->calcCost( pGoal );  // コストを計算.
          aClosedList.remove( *ppBN ); // クローズドリストから削除.
          aOpenList.push_back( *ppBN );// オープンリストに追加.
        }
      }
      else{
        // まだリストに入っていないのでオープンリストに追加.
        (*ppBN)->setParent( pB );    // 親を設定.
        (*ppBN)->calcCost( pGoal );  // コストを計算.
        aOpenList.push_back( *ppBN );// オープンリストに追加.
      }
    }
    // pBは計算し終えたのでクローズドリストに移します.
    aOpenList.remove( pB );
    aClosedList.push_back( pB );
  }

  //------------------------- 
  // パスをまとめなおして終了.
  //------------------------- 
  if( bReachGoal ){
    CNode* pNode = pGoal;
    while( pNode==NULL ){
      path->push_front( pNode );
      pNode = pNode->getParent();
    }
  }
}