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

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