AndEngineのA*アルゴリズムのバグ修正

たぶんバグだと思う
違ったとしても責任はもちません


場所はorg.andengine.util.algorithm.path.astarの中のAStarPathFinder.java

findPath() メソッドの

visitedNodes.put(currentNodeID, currentNode);

のすぐ上に

openNodes.remove(currentNodeID);

を追加。


たぶんこれが無いと経路がない場合（袋小路状態のとき）にNULLが帰ってこない
あと最適経路じゃないところを返してくることがある・・・とおもう

修正後＋日本語のコメント付きのfindPath()内全ソースを以下に

 @Override
 public Path findPath(final IPathFinderMap<T> pPathFinderMap, final int pXMin, final int pYMin, final int pXMax, final int pYMax, final T pEntity, final int pFromX, final int pFromY, final int pToX, final int pToY, final boolean pAllowDiagonal, final IAStarHeuristic<T> pAStarHeuristic, final ICostFunction<T> pCostFunction, final float pMaxCost, final IPathFinderListener<T> pPathFinderListener) {
  if(((pFromX == pToX) && (pFromY == pToY)) || pPathFinderMap.isBlocked(pFromX, pFromY, pEntity) || pPathFinderMap.isBlocked(pToX, pToY, pEntity)) {
   return null;
  }

  /* いくつかのフィールドをローカルに持ってくる */
  /* Drag some fields to local variables. */
  final Node fromNode = new Node(pFromX, pFromY, pAStarHeuristic.getExpectedRestCost(pPathFinderMap, pEntity, pFromX, pFromY, pToX, pToY));

  final long fromNodeID = fromNode.mID;
  final long toNodeID = Node.calculateID(pToX, pToY);

  final LongSparseArray<Node> visitedNodes = new LongSparseArray<Node>();
  final LongSparseArray<Node> openNodes = new LongSparseArray<Node>();
  final IQueue<Node> sortedOpenNodes = new SortedQueue<Node>(new ShiftList<Node>());

  final boolean allowDiagonalMovement = pAllowDiagonal;

  /* アルゴリズムの初期化 */
  /* Initialize algorithm. */
  openNodes.put(fromNodeID, fromNode);
  sortedOpenNodes.enter(fromNode);

  Node currentNode = null;
  while(openNodes.size() > 0) {
   /* 現在のノードをオープンリストで一番コストが小さいものにする */
   /* The first Node in the open list is the one with the lowest cost. */
   currentNode = sortedOpenNodes.poll();
   final long currentNodeID = currentNode.mID;
   /* 現在のノードが目的地のノードなら探索終了 */
   if(currentNodeID == toNodeID) {
    break;
   }
   
   /* 現在のノードを訪問済みリストに追加 */
   openNodes.remove(currentNodeID); // オープンリストから探索済みノードを削除する処理を追加
   visitedNodes.put(currentNodeID, currentNode);

   /* 近傍のすべての位置でループ */
   /* Loop over all neighbors of this position. */
   for(int dX = -1; dX <= 1; dX++) {
    for(int dY = -1; dY <= 1; dY++) {
     if((dX == 0) && (dY == 0)) {
      continue;
     }

     if(!allowDiagonalMovement && (dX != 0) && (dY != 0)) {
      continue;
     }

     final int neighborNodeX = dX + currentNode.mX;
     final int neighborNodeY = dY + currentNode.mY;
     final long neighborNodeID = Node.calculateID(neighborNodeX, neighborNodeY);

     /* 範囲外か移動不能なノードの場合は飛ばす */
     if(!IntBoundsUtils.contains(pXMin, pYMin, pXMax, pYMax, neighborNodeX, neighborNodeY) || pPathFinderMap.isBlocked(neighborNodeX, neighborNodeY, pEntity)) {
      continue;
     }
     
     /* 訪問済みのノードの場合は飛ばす */
     if(visitedNodes.indexOfKey(neighborNodeID) >= 0) {
      continue;
     }

     Node neighborNode = openNodes.get(neighborNodeID);
     final boolean neighborNodeIsNew;
     /* 調査中のノードがオープンリストに存在しているかどうかのチェック */
     /* Check if neighbor exists. */
     if(neighborNode == null) {
      /* 存在していない場合はノードを生成 */
      neighborNodeIsNew = true;
      neighborNode = new Node(neighborNodeX, neighborNodeY, pAStarHeuristic.getExpectedRestCost(pPathFinderMap, pEntity, neighborNodeX, neighborNodeY, pToX, pToY));
     } else {
      /* 存在している場合 */
      neighborNodeIsNew = false;
     }

     /* コストを再計算 */
     /* Update cost of neighbor as cost of current plus step from current to neighbor. */
     final float costFromCurrentToNeigbor = pCostFunction.getCost(pPathFinderMap, currentNode.mX, currentNode.mY, neighborNodeX, neighborNodeY, pEntity);
     final float neighborNodeCost = currentNode.mCost + costFromCurrentToNeigbor;
     if(neighborNodeCost > pMaxCost) {
      /* コストの最大値以上 -> 新しく追加したノードでない場合はオープンリストから削除 */
      /* Too expensive -> remove if isn't a new node. */
      if(!neighborNodeIsNew) {
       openNodes.remove(neighborNodeID);
      }
     } else {
      /* コストの最大値を超えていない */
      neighborNode.setParent(currentNode, neighborNodeCost);
      if(neighborNodeIsNew) {
       /* 新しく追加したノードの場合はオープンリストに追加 */
       openNodes.put(neighborNodeID, neighborNode);
      } else {
       /* 正しい位置に挿入するために一度キューから削除 */
       /* Remove so that re-insertion puts it to the correct spot. */
       sortedOpenNodes.remove(neighborNode);
      }
      /* キューを追加 */
      sortedOpenNodes.enter(neighborNode);

      if(pPathFinderListener != null) {
       pPathFinderListener.onVisited(pEntity, neighborNodeX, neighborNodeY);
      }
     }
    }
   }
  }

  /* Cleanup. */
  // TODO We could just let the GC do its work.
  visitedNodes.clear();
  openNodes.clear();
  sortedOpenNodes.clear();

  /* 経路が見つかったかチェック(無ければnullを返す) */
  /* Check if a path was found. */
  if(currentNode.mID != toNodeID) {
   return null;
  }

  /* 経路の長さを計算 */
  /* Calculate path length. */
  int length = 1;
  Node tmp = currentNode;
  while(tmp.mID != fromNodeID) {
   tmp = tmp.mParent;
   length++;
  }

  /* 親タイルを辿って経路を形成する */
  /* Traceback path. */
  final Path path = new Path(length);
  int index = length - 1;
  tmp = currentNode;
  while(tmp.mID != fromNodeID) {
   path.set(index, tmp.mX, tmp.mY);
   tmp = tmp.mParent;
   index--;
  }
  path.set(0, pFromX, pFromY);

  return path;
 }

個人的に思うのは、
このアルゴリズムで使うpath（org.andengine.util.algorithm.path.Path）と
スプライトアニメーションのpath（org.andengine.entity.modifier.PathModifier.Path）の互換性を上げてほしい・・・名前も同じで面倒だし
まあ自分でクラス拡張すればいいのかもしれないですが