启发式搜索算法（A*算法）的实现

第一个程序：

 

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值

    // should really use sqrt().

    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;
    // make Open List point to first node

    OPEN->NextNode=Node;
    for (;;)
    {
        //从Open表中取得一个估价值最好的节点

        BestNode=ReturnBestNode();
        //如果该节点是目标节点就退出

        // if we've found the end, break and finish break;

        if (BestNode->NodeNum == TileNumDest)
            //否则生成子节点

            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(Successor)=g(BestNode)+cost of getting from BestNode to Successor

    g = BestNode->g+1;
    // identification purposes

    TileNumS = TileNum(x,y);
    //子节点再Open表中吗？

    // if equal to NULL then not in OPEN list, else it returns the Node in Old

    if ( (Old=CheckOPEN(TileNumS)) != NULL )
    {
        //若在

        for( c = 0; c < 8; c++)
            // Add Old to the list of BestNode's Children (or Successors).

            if( BestNode->Child[c] == NULL )
                break;
            BestNode->Child[c] = Old;
            //比较Open表中的估价值和当前的估价值（只要比较g值就可以了）

            // if our new g value is < Old's then reset Old's parent to point to BestNode

            if ( g < Old->g )
            {
                //当前的估价值小就更新Open表中的估价值

                Old->Parent = BestNode;
                Old->g = g;
                Old->f = g + Old->h;
            }
    }
    else
        //在Closed表中吗？

        // if equal to NULL then not in OPEN list, else it returns the Node in Old

        if ( (Old=CheckCLOSED(TileNumS)) != NULL )
        {
            //若在

            for( c = 0; c< 8; c++)
                // Add Old to the list of BestNode's Children (or Successors).

                if ( BestNode->Child[c] == NULL )
                    break;
                BestNode->Child[c] = Old;
                //比较Closed表中的估价值和当前的估价值（只要比较g值就可以了）

                // if our new g value is < Old's then reset Old's parent to point to BestNode

                if ( g < Old->g )
                {
                    //当前的估价值小就更新Closed表中的估价值

                    Old->Parent = BestNode;
                    Old->g = g;
                    Old->f = g + Old->h;
                    //再依次更新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!

                    PropagateDown(Old);
                }
        }
        //不在Open表中也不在Close表中

        else
        {
            //生成新的节点

            Successor = ( NODE* )calloc(1,sizeof( NODE ));
            Successor->Parent = BestNode;
            Successor->g = g;
            // should do sqrt(), but since we don't really

            Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);
            // care about the distance but just which branch looks

            Successor->f = g+Successor->h;
            // better this should suffice. Anyayz it's faster.

            Successor->x = x;
            Successor->y = y;
            Successor->NodeNum = TileNumS;
            //再插入Open表中，同时排序。

            // Insert Successor on OPEN list wrt f

            Insert(Successor);
            for( c =0; c < 8; c++)
                // Add Old to the list of BestNode's Children (or Successors).

                if ( BestNode->Child[c] == NULL )
                    break;
                BestNode->Child[c] = Successor;
        }
}

第二个程序：

/* 云风的求解最短路径代码 (Cloud Wu's Pathfinding code)*/

#define MAPMAXSIZE 100 //地图面积最大为 100x100

#define MAXINT 8192 //定义一个最大整数, 地图上任意两点距离不会超过它

#define STACKSIZE 65536 //保存搜索节点的堆栈大小


#define tile_num(x,y) ((y)*map_w+(x)) //将 x,y 坐标转换为地图上块的编号

#define tile_x(n) ((n)%map_w) //由块编号得出 x,y 坐标

#define tile_y(n) ((n)/map_w)

// 树结构, 比较特殊, 是从叶节点向根节点反向链接

typedef struct node *TREE;

struct node {
    int h;
    int tile;
    TREE father;
} ;

typedef struct node2 *LINK;

struct node2 {
    TREE node;
    int f;
    LINK next;
};

LINK queue; // 保存没有处理的行走方法的节点

TREE stack[STACKSIZE]; // 保存已经处理过的节点 (搜索完后释放)

int stacktop;
unsigned char map[MAPMAXSIZE][MAPMAXSIZE]; //地图数据

int dis_map[MAPMAXSIZE][MAPMAXSIZE]; //保存搜索路径时,中间目标地最优解


int map_w,map_h; //地图宽和高

int start_x,start_y,end_x,end_y; //地点,终点坐标


// 初始化队列

void init_queue()
{
    queue=(LINK)malloc(sizeof(*queue));
    queue->node=NULL;
    queue->f=-1;
    queue->next=(LINK)malloc(sizeof(*queue));
    queue->next->f=MAXINT;
    queue->next->node=NULL;
    queue->next->next=NULL;
}

// 待处理节点入队列, 依靠对目的地估价距离插入排序

void enter_queue(TREE node,int f)
{
    LINK p=queue,father,q;
    while(f>p->f) {
        father=p;
        p=p->next;
        assert(p);
    }
    q=(LINK)malloc(sizeof(*q));
    assert(queue);
    q->f=f,q->node=node,q->next=p;
    father->next=q;
}

// 将离目的地估计最近的方案出队列

TREE get_from_queue()
{
    TREE bestchoice=queue->next->node;
    LINK next=queue->next->next;
    free(queue->next);
    queue->next=next;
    stack[stacktop++]=bestchoice;
    assert(stacktop<STACKSIZE);
    return bestchoice;
}

// 释放栈顶节点

void pop_stack()
{
    free(stack[--stacktop]);
}

// 释放申请过的所有节点

void freetree()
{
    int i;
    LINK p;
    for (i=0;i<stacktop;i++)
        free(stack[i]);
    while (queue) {
        p=queue;
        free(p->node);
        queue=queue->next;
        free(p);
    }
}

// 估价函数,估价 x,y 到目的地的距离,估计值必须保证比实际值小

int judge(int x,int y)
{
    int distance;
    distance=abs(end_x-x)+abs(end_y-y);
    return distance;
}

// 尝试下一步移动到 x,y 可行否

int trytile(int x,int y,TREE father)
{
    TREE p=father;
    int h;
    if (map[y][x]!=' ') return 1; // 如果 (x,y) 处是障碍,失败

    while (p) {
        if (x==tile_x(p->tile) && y==tile_y(p->tile)) return 1; //如果 (x,y) 曾经经过,失败

        p=p->father;
    }
    h=father->h+1;
    if (h>=dis_map[y][x]) return 1; // 如果曾经有更好的方案移动到 (x,y) 失败

    dis_map[y][x]=h; // 记录这次到 (x,y) 的距离为历史最佳距离

    
    // 将这步方案记入待处理队列

    p=(TREE)malloc(sizeof(*p));
    p->father=father;
    p->h=father->h+1;
    p->tile=tile_num(x,y);
    enter_queue(p,p->h+judge(x,y));
    return 0;
}

// 路径寻找主函数

void findpath(int *path)
{
    TREE root;
    int i,j;
    stacktop=0;
    for (i=0;i<map_h;i++)
        for (j=0;j<map_w;j++)
            dis_map[i][j]=MAXINT;
        init_queue();
        root=(TREE)malloc(sizeof(*root));
        root->tile=tile_num(start_x,start_y);
        root->h=0;
        root->father=NULL;
        enter_queue(root,judge(start_x,start_y));
        for (;;) {
            int x,y,child;
            TREE p;
            root=get_from_queue();
            if (root==NULL) {
                *path=-1;
                return;
            }
            x=tile_x(root->tile);
            y=tile_y(root->tile);
            if (x==end_x && y==end_y) break; // 达到目的地成功返回

            
            child=trytile(x,y-1,root); //尝试向上移动

            child&=trytile(x,y+1,root); //尝试向下移动

            child&=trytile(x-1,y,root); //尝试向左移动

            child&=trytile(x+1,y,root); //尝试向右移动

            if (child!=0)
                pop_stack(); // 如果四个方向均不能移动,释放这个死节点

        }
        
        // 回溯树，将求出的最佳路径保存在 path[] 中

        for (i=0;root;i++) {
            path[i]=root->tile;
            root=root->father;
        }
        path[i]=-1;
        freetree();
}

void printpath(int *path)
{
    int i;
    for (i=0;path[i]>=0;i++) {
        gotoxy(tile_x(path[i])+1,tile_y(path[i])+1);
        cprintf("\xfe");
    }
}

int readmap()
{
    FILE *f;
    int i,j;
    f=fopen("map.dat","r");
    assert(f);
    fscanf(f,"%d,%d\n",&map_w,&map_h);
    for (i=0;i<map_h;i++)
        fgets(&map[i][0],map_w+1,f);
    fclose(f);
    start_x=-1,end_x=-1;
    for (i=0;i<map_h;i++)
        for (j=0;j<map_w;j++) {
            if (map[i][j]=='s') map[i][j]=' ',start_x=j,start_y=i;
            if (map[i][j]=='e') map[i][j]=' ',end_x=j,end_y=i;
        }
        assert(start_x>=0 && end_x>=0);
        return 0;
}

void showmap()
{
    int i,j;
    clrscr();
    for (i=0;i<map_h;i++) {
        gotoxy(1,i+1);
        for (j=0;j<map_w;j++)
            if (map[i][j]!=' ') cprintf("\xdb");
            else cprintf(" ");
    }
    gotoxy(start_x+1,start_y+1);
    cprintf("s");
    gotoxy(end_x+1,end_y+1);
    cprintf("e");
}

int main()
{
    int path[MAXINT];
    readmap();
    showmap();
    getch();
    findpath(path);
    printpath(path);
    getch();
    return 0;
}