A*算法

A*算法
1.f(n) = g(n) + h(n) 
其中f(n) 是节点n的估价函数，g(n)实在状态空间中从初始节点到n节点的实际代价，h(n)是从n到目 标节点最佳路径的估计代价。

2.

Best_First_Search()
{
    Open  =  [起始节点]; Closed  =  [];
     while  ( Open表非空 )
    {
        从Open中取得一个节点X，并从OPEN表中删除。
         if  (X是目标节点)
        {
            求得路径PATH；返回路径PATH；
        }
         for  (每一个X的子节点Y)
        {
             if ( Y不在OPEN表和CLOSE表中 )
            {
                求Y的估价值；并将Y插入OPEN表中； // 还没有排序
            }
             else
            {
                 if ( Y在OPEN表中 )
                {
                     if ( Y的估价值小于OPEN表的估价值 )
                    更新OPEN表中的估价值；
                }
                 else   // Y在CLOSE表中
                {
                     if ( Y的估价值小于CLOSE表的估价值 )
                    {
                    更新CLOSE表中的估价值；
                    从CLOSE表中移出节点，并放入OPEN表中；
                    }
                }
            }
        } // end for

         将X节点插入CLOSE表中；
        按照估价值将OPEN表中的节点排序；
    } // end while
} // end func

g value can be lowered, and if so, you re-open it.

代码

OPEN  =  priority queue containing START
CLOSED  =  empty  set
while  lowest rank  in  OPEN  is  not the GOAL:
  current  =  remove lowest rank item from OPEN
  add current to CLOSED
   for  neighbors of current:
    cost  =  g(current)  +  movementcost(current, neighbor)
     if  neighbor  in  OPEN and cost less than g(neighbor):
      remove neighbor from OPEN, because  new  path  is  better
     if  neighbor  in  CLOSED and cost less than g(neighbor):  **
      remove neighbor from CLOSED
     if  neighbor not  in  OPEN and neighbor not  in  CLOSED:
       set  g(neighbor) to cost
      add neighbor to OPEN
       set  priority queue rank to g(neighbor)  +  h(neighbor)
       set  neighbor ' s parent to current

reconstruct reverse path from goal to start
by following parent pointers

(**) This should never happen if you have an monotone admissible heuristic. However in games we often have inadmissible heuristics.

 

3.

void  AstarPathfinder::FindPath( int  sx,  int  sy,  int  dx,  int  dy)
{
    NODE  * Node,  * BestNode;
     int  TileNumDest;
     // 得到目标位置，作判断用
    TileNumDest  =  TileNum(sx, sy);
    
     // 生成Open和Closed表
    OPEN = ( NODE *  )calloc( 1 , sizeof ( NODE ));
    CLOSED = ( NODE *  )calloc( 1 , sizeof ( NODE ));
    
     // 生成起始节点，并放入Open表中
    Node = ( NODE *  )calloc( 1 , sizeof ( NODE ));
    Node -> g  =   0 ;
    
     // 这是计算h值        
    Node -> h  =  (dx - sx) * (dx - sx)  +  (dy - sy) * (dy - sy);  // 此处按道理应用开方
    
     // 这是计算f值，即估价值
    Node -> f  =  Node -> g + Node -> h;
    Node -> NodeNum  =  TileNum(dx, dy);
    Node -> x  =  dx;
    Node -> y  =  dy;

    OPEN -> NextNode = Node;        //  make Open List point to first node
     for  (;;)
    {     // 从Open表中取得一个估价值最好的节点
        BestNode = ReturnBestNode();
         // 如果该节点是目标节点就退出
         if  (BestNode -> NodeNum  ==  TileNumDest)   //  if we've found the
             // end, break and finish
             break ;
         // 否则生成子节点
        GenerateSuccessors(BestNode,sx,sy);
    }
    PATH  =  BestNode;
}

void  AstarPathfinder::GenerateSuccessors(NODE  * BestNode, int  dx, int  dy)
{
     int  x, y;
     // 哦！依次生成八个方向的子节点，简单！
     //  Upper-Left
     if  ( FreeTile(x = BestNode -> x - TILESIZE, y = BestNode -> y - TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Upper
     if  ( FreeTile(x = BestNode -> x, y = BestNode -> y - TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Upper-Right
     if  ( FreeTile(x = BestNode -> x + TILESIZE, y = BestNode -> y - TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Right
     if  ( FreeTile(x = BestNode -> x + TILESIZE, y = BestNode -> y) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Lower-Right
     if  ( FreeTile(x = BestNode -> x + TILESIZE, y = BestNode -> y + TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Lower
     if  ( FreeTile(x = BestNode -> x, y = BestNode -> y + TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Lower-Left
     if  ( FreeTile(x = BestNode -> x - TILESIZE, y = BestNode -> y + TILESIZE) )
        GenerateSucc(BestNode,x,y,dx,dy);
     //  Left
     if  ( FreeTile(x = BestNode -> x - TILESIZE, y = BestNode -> y) )
        GenerateSucc(BestNode,x,y,dx,dy);
}

void  AstarPathfinder::GenerateSucc(NODE  * BestNode, int  x, int  y, int  dx, int  dy)
{
     int  g, TileNumS, c  =   0 ;
    NODE  * Old,  * Successor;

     // 计算子节点的g值          
    g  =  BestNode -> g + 1 ;  // g(Successor)=g(BestNode)+cost of getting
     // from BestNode to Successor
    TileNumS  =  TileNum(x,y);   //  identification purposes

     // 子节点再Open表中吗？       
     if  ( (Old = CheckOPEN(TileNumS))  !=  NULL )  //  if equal to NULL then
         // not in OPEN list, else it returns the Node in Old
    {
         // 若在
         for ( c  =   0 ; c  <   8 ; c ++ )
             if ( BestNode -> Child[c]  ==  NULL )  //  Add Old to the list of
                 //  BestNode's Children (or Successors).
                 break ;
        BestNode -> Child[c]  =  Old;
         // 比较Open表中的估价值和当前的估价值（只要比较g值就可以了）
         if  ( g  <  Old -> g )   //  if our new g value is < Old's then
             // reset Old's parent to point to BestNode
        {
             // 当前的估价值小就更新Open表中的估价值
            Old -> Parent  =  BestNode;
            Old -> g  =  g;
            Old -> f  =  g  +  Old -> h;
        }
    }
     else   // 在Closed表中吗？
    {
         if  ( (Old = CheckCLOSED(TileNumS))  !=  NULL )  //  if equal to NULL then
             //  not in OPEN list, else it returns the Node in Old
        {
             // 若在
             for ( c  =   0 ; c <   8 ; c ++ )
                 if  ( BestNode -> Child[c]  ==  NULL )  //  Add Old to the list of
                     // BestNode's Children (or Successors).
                     break ;
            BestNode -> Child[c]  =  Old;
             // 比较Closed表中的估价值和当前的估价值（只要比较g值就可以了）
             if  ( g  <  Old -> g )   //  if our new g value is < Old's then
                 //  reset Old's parent to point to BestNode
            {
                 // 当前的估价值小就更新Closed表中的估价值
                Old -> Parent  =  BestNode;
                Old -> g  =  g;
                Old -> f  =  g  +  Old -> h;
                 // 再依次更新Old的所有子节点的估价值
                PropagateDown(Old);   //  Since we changed the g value of
                 // Old,we need to propagate this new
                 // value downwards, i.e.
                 //  do a Depth-First traversal of the tree!
            }
        }
         else // 不在Open表中也不在Close表中
        {    
             // 生成新的节点
            Successor  =  ( NODE *  )calloc( 1 , sizeof ( NODE ));
            Successor -> Parent  =  BestNode;
            Successor -> g  =  g;
            Successor -> h  =  (x - dx) * (x - dx)  +  (y - dy) * (y - dy);   //  should do
             //  sqrt(), but since we don't really
            Successor -> f  =  g + Successor -> h;  //  care about the distance but
             // just which branch looks
            Successor -> x  =  x;               //  better this should suffice.
             //  Anyayz it's faster.
            Successor -> y  =  y;
            Successor -> NodeNum  =  TileNumS;
             // 再插入Open表中，同时排序。
            Insert(Successor);      //  Insert Successor on OPEN list wrt f
             for ( c  = 0 ; c  <   8 ; c ++ )
                 if  ( BestNode -> Child[c]  ==  NULL )  //  Add Old to the
                     //  list of BestNode's Children (or Successors).
                     break ;
            BestNode -> Child[c]  =  Successor;
        }
    }
} 

参考资料：
[翻译]A*寻路初探 GameDev.net
http://blog.vckbase.com/panic/archive/2005/03/20/3778.html

[翻译]A*分层寻路

http://blog.vckbase.com/panic/archive/2005/07/21/9906.html

A* Pathfinding for Beginners

http://www.policyalmanac.org/games/aStarTutorial.htm

Pathfinding in World of  WarCraft

http://www.ai-blog.net/archives/000152.html

Fixing Pathfinding Once and For All
http://www.ai-blog.net/archives/000152.html

A*(A星)算法(一)
http://hi.baidu.com/%BA%DA%B5%C4%B7%A2%D7%CF/blog/item/60e3483dce5bb8c29e3d62e0.html

初识A*算法
http://blog.csdn.net/zheng80037/archive/2007/06/04/1636953.aspx

[翻译]在A*寻路中使用二叉堆
http://blog.vckbase.com/panic/archive/2005/03/28/4144.html

二叉堆的模板代码－－续“关于一道算法题《编写算法，从10亿个浮点数当中，选出其中最大的10000个》”
http://blog.vckbase.com/panic/archive/2006/06/19/20869.html