C语言解决A*寻路问题

//A*寻路
#include<stdio.h>
#include<math.h>
#define MAX 10
#define Length 100
int map[10][10]={
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
};
int step[MAX][MAX] = {0};//从起点到该坐标的最短步数
typedef struct node{
    int x;
    int y;
    int value;
}Node;
Node qu[Length];//用来模拟优先队列
int front=-1,rear=-1;
int xb = 4, yb = 0;//起点坐标
int xe = 0, ye = 6;//终点坐标
int g(int x,int y){//计算该坐标与终点的曼哈顿距离
    return abs(xe-x)+abs(ye-y);
}
//打印地图
void print_map(){
    for(int i=0;i<MAX;i++){
        for(int j=0;j<MAX;j++){
            printf("%d ",map[i][j]);
        }
        printf("\n");
    }
}
//判断该坐标是否合法
int Judge(int x, int y){
    if(x<0||x>=MAX||y<0||y>=MAX)
        return 1;
    if(map[x][y]==1)
        return 1;
    return 0;
}
//实现优先队列的插入
void Insert(int x, int y ,int value){
    if(rear == -1){
        rear++;
        qu[rear].x = x;
        qu[rear].y = y;
        qu[rear].value = value;
    }
    else{
        int r = rear;
        while(value < qu[r].value&&r >= front+1){
            qu[r+1] = qu[r];
            r--;
        }
        qu[r+1].x = x;
        qu[r+1].y = y;
        qu[r+1].value = value;
        rear++;
    }
}
//得到模拟优先队列中的最小权值坐标
Node Get(){
    return qu[++front];
}
//输出找到的路径
void print_road(Node come_from[MAX][MAX], int x, int y){
    Node input[Length];
    int x1, y1, x2, y2, idx = -1;
    x1 = x, y1 = y;
    while(!(x1==-1&&y1==-1)){
        idx++;
        input[idx].x = x1;
        input[idx].y = y1;
        x2 = x1;
        y2 = y1;
        x1 = come_from[x2][y2].x;
        y1 = come_from[x2][y2].y;
    }
    printf("找到的最优路径为：");
    while(idx!=0){
        printf("(%d,%d)", input[idx].x, input[idx].y);
        idx--;
    }
    printf("(%d,%d)\n",input[0].x,input[0].y);
}
//A*寻路
void AStarFindRoad(){
    step[xb][yb] = 0;
    Node come_from[MAX][MAX];
    come_from[xb][yb].x = -1;
    come_from[xb][yb].y = -1;
    int x,y;
    int x1,y1;
    Insert(xb,yb,step[xb][yb]+g(xb,yb));
    while(front != rear){
        Node q;
        q = Get();
        x = q.x;
        y = q.y;
        if(x == xe&& y== ye){
            print_road(come_from,x,y);
            break;
        }
        int di = 0;
        while(di<4){
            switch(di){
                case 0:x1 = x - 1;y1 = y;break;
                case 1:x1 = x;y1 = y + 1;break;
                case 2:x1 = x + 1;y1 = y;break;
                case 3:x1 = x;y1 = y - 1;break;
            }
            if(Judge(x1,y1) == 0){
                if((step[x1][y1] == 0&&!(x1 == xb&&y1 == yb))||(step[x][y]+1<step[x1][y1])){
                    come_from[x1][y1].x = x;
                    come_from[x1][y1].y = y;
                    step[x1][y1] = step[x][y] + 1;
                    Insert(x1,y1,step[x][y]+1+g(x1,y1));
                }
            }
            di++;
        }
    }
}
int main(){
    printf("地图：\n");
    print_map();
    AStarFindRoad();
}