astar.cpp

A*算法解决找路径的问题

#include<iostream>
#include<cassert>

#include<windows.h>


#define MAPMAXSIZE 200  //地图面积最大为 100x100  1152x768
#define MAXINT (100)   //定义一个最大整数, 地图上任意两点距离不会超过它
 
#define STACKSIZE (500) //保存搜索节点的堆栈大小

#define tile_num(x,y) ((y)*map_w+(x))  //将 x,y 坐标转换为地图上块的编号
         
//由块编号得出 x,y 坐标
int Endx,Endy;
// 树结构, 比较特殊, 是从叶节点向根节点反向链接
typedef struct node *TREE;

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

typedef struct node2 *LINK;

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

typedef struct DataNode{
	int left;
	int right;
	int top;
	int bottom;
}DataNode;
DataNode mapdata[5][5];

LINK queue;               // 保存没有处理的行走方法的节点
TREE stack[STACKSIZE];    // 保存已经处理过的节点 (搜索完后释放)
int stacktop;

//__int64 map[96*gPv.EditBackHigh][64*gPv.EditBackWidth];   //地图数据 0000 0000 0000 0000 0000 0000 0000 0000  unsigned
//高16位为静态图在地图中的层数  低16位为静态图的类型属性
int dis_map[MAPMAXSIZE][MAPMAXSIZE];         //保存搜索路径时,中间目标地最优解

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

int count;
// 初始化队列
	
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;
}
//由块号返回x坐标
int tile_xx(int n)
{ 
	int i;
	i=n%map_w;
	if(i==0)
	i=4;
	return i;
}

//由块号返回y坐标
int tile_yy(int n)
{
	int i;
	i=n/map_w;
	if(n%map_w==0)
	i--;
	return i;

}
// 待处理节点入队列, 依靠对目的地估价距离插入排序
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()
{
//	MessageBox(NULL,"ok","ok",1);
 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(Endx-x)+abs(Endy-y);
 return distance;
}

// 尝试下一步移动到 x,y 可行否
int trytile(int x,int y,TREE father,int direction)
{
 TREE p=father;
 int h;
 if(x<1 || x>4 || y<1 || y>4)
		 return 1;
 switch(direction)
 {
 case 1:	 
	 if(mapdata[x][y].bottom!=1)
		 return 1;
	 break;
 case 2:
	 if(mapdata[x][y].left!=1)
		 return 1;
	 break;
 case 3:
	 if(mapdata[x][y].top!=1)
		 return 1;
	 break;
 case 4:
	 if(mapdata[x][y].right!=1)
		 return 1;
	 break;
 }


 while (p) {
   if (x==tile_xx(p->tile) && y==tile_yy(p->tile)) return 1; //如果 (x,y) 曾经经过,失败
   p=p->father;
   }
 h=father->h+1;
 if (h>=1000) 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;
}

////////////////////////////////////////////////////////////
// 路径寻找主函数//没有考虑终点在死节点上的情况!!!!!!!!!!!!!
int FindPath(int start_x,int start_y,int end_x,int end_y,POINT *Path)
////////////////////////////////////////////////////////////
{
	
Endx=end_x;
Endy=end_y;
 TREE root;	
 int i,j,m;

 count=0;
 for(i=0;i<500;i++)	{Path[i].x=0;Path[i].y=0;} 
 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;
    root=get_from_queue();
    if (root==NULL) {
      
      return 0;
    }
    x=tile_xx(root->tile);
    y=tile_yy(root->tile);
     if (x==end_x && y==end_y) break; // 达到目的地成功返回

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

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

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

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

 for (i=0;root;i++) {
    Path[i].x=tile_xx(root->tile);
	Path[i].y=tile_yy(root->tile);
    root=root->father;
    }
 
 m=i;
 
 freetree();

 return m;
}

//初始化地图
void initmapdata()
{
	
	mapdata[1][1].left=-1;
	mapdata[1][1].right=0;
	mapdata[1][1].top=1;
	mapdata[1][1].bottom=-1;

	mapdata[2][1].left=0;
	mapdata[2][1].right=1;
	mapdata[2][1].top=1;
	mapdata[2][1].bottom=-1;

	mapdata[3][1].left=1;
	mapdata[3][1].right=1;
	mapdata[3][1].top=1;
	mapdata[3][1].bottom=-1;

	mapdata[4][1].left=1;
	mapdata[4][1].right=-1;
	mapdata[4][1].top=1;
	mapdata[4][1].bottom=-1;

	mapdata[1][2].left=-1;
	mapdata[1][2].right=0;
	mapdata[1][2].top=1;
	mapdata[1][2].bottom=1;

	mapdata[2][2].left=0;
	mapdata[2][2].right=1;
	mapdata[2][2].top=1;
	mapdata[2][2].bottom=1;

	mapdata[3][2].left=1;
	mapdata[3][2].right=0;
	mapdata[3][2].top=1;
	mapdata[3][2].bottom=1;

	mapdata[4][2].left=0;
	mapdata[4][2].right=-1;
	mapdata[4][2].top=1;
	mapdata[4][2].bottom=1;

	mapdata[1][3].left=-1;
	mapdata[1][3].right=1;
	mapdata[1][3].top=0;
	mapdata[1][3].bottom=1;

	mapdata[2][3].left=1;
	mapdata[2][3].right=0;
	mapdata[2][3].top=1;
	mapdata[2][3].bottom=1;

	mapdata[3][3].left=0;
	mapdata[3][3].right=1;
	mapdata[3][3].top=1;
	mapdata[3][3].bottom=1;

	mapdata[4][3].left=1;
	mapdata[4][3].right=-1;
	mapdata[4][3].top=1;
	mapdata[4][3].bottom=1;

	mapdata[1][4].left=-1;
	mapdata[1][4].right=1;
	mapdata[1][4].top=-1;
	mapdata[1][4].bottom=0;

	mapdata[2][4].left=1;
	mapdata[2][4].right=1;
	mapdata[2][4].top=-1;
	mapdata[2][4].bottom=1;

	mapdata[3][4].left=1;
	mapdata[3][4].right=0;
	mapdata[3][4].top=-1;
	mapdata[3][4].bottom=1;

	mapdata[4][4].left=0;
	mapdata[4][4].right=-1;
	mapdata[4][4].top=-1;
	mapdata[4][4].bottom=1;	
}

void  main()
{
	int i,m;
	POINT path[500];
	initmapdata();
	m=FindPath(1,1,4,4,path);
	printf("入口为(1,1),出口为(4,4)\n");
	printf("行走路径为：\r\n");
	Sleep(800);
	 for(i=m-1;i>=0;i--)
	 {
		 //Sleep(500);
		 printf("%d,%d\n",path[i].x,path[i].y);
	 }
	 int x1,x2,y1,y2;
	Sleep(800);
	 printf("\n请输入其它入口和出口,并按回车键\n");
	 printf("请输入入口x、y，如2,1\n");
	 scanf("%d,%d",&x1,&y1);
	 printf("请输入出口x、y，如3,3\n");
	 scanf("%d,%d",&x2,&y2);

	 m=FindPath(x1,y1,x2,y2,path);
	 if(m>0)
		printf("行走路径为：\r\n");
	 else
		 printf("输入有误或不存在通路\n");
	 for(i=m-1;i>=0;i--)
	 {
		 printf("%d,%d\n",path[i].x,path[i].y);
	 }
	 
	 
 
      
}