A*算法 寻路径 实现，纯代码

参考 

设计到的启发公式

f = g + h

g： 起始节点start到当前搜索节点current的代价(步数)cost ，是比较确定的一个数值

h: 当前搜索节点curret到 终点节点goal的代价(步数)cost ,这是一个大概的估计 不是真的要这么对代价(步数)，

h计算可以根据 曼哈顿 距离就算      h = abs(currret[x] - goal[x]) + abs(currret[x] - goal[x])

 

 

 

步骤英文版 

 

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 admissible heuristic. However in games we often have inadmissible heuristics.

C语言代码实现

#include <stdio.h>
#include <string.h>
#include <math.h>

#define ROWSIZE 10
#define COLSIZE	10

#define PATHSIZE ROWSIZE+COLSIZE
#define BIGDATA	0xff

char map[ROWSIZE][COLSIZE];

int path[PATHSIZE]; 	

char obstacle_row[] = {2,2,3,3,3,3,4,4,5,5,5,5,6,6,7,7,7};
char obstacle_col[] = {2,7,2,7,8,6,2,7,2,3,4,7,2,7,2,7,8};
void create_map(char map[][COLSIZE],char row[],char col[],int obst_n)
{
	int i,j;
	for(i = 0;i < obst_n;i++)
	{
		map[row[i]][col[i]] = 1;
	}	
}
int map2index(int i,int j)
{
	if(i >= 0 && i < ROWSIZE && j >=0 && j < COLSIZE)
	{
		return i*COLSIZE + j;
	}
	else
	{
		printf("越界\n");
		return 0;
	}
}
void index2map(int index,int *a,int *b)
{
	*a = index / COLSIZE;
	*b = index % COLSIZE;
}
void display_map(char map[][COLSIZE])
{
	int i,j;
	
	for(i = 0;i < ROWSIZE;i++)
	{
		for(j = 0;j < COLSIZE;j++)
		{
			switch(map[i][j])
			{
				case 1:
					printf("■ ");
					//printf("\e[1;31m""■ ""\e[0m");
					break;
				case 2:printf("\e[1;32m""■ ""\e[0m");break;
				case 3:printf("\e[1;34m""✈ ""\e[0m");break;
				case 4:printf("\e[1;31m""★ ""\e[0m");break;
				default: printf("0 ");
			}
		}
		printf("\n");
	}
}

void display_matrix(int m[][COLSIZE])
{
	int i,j;
	for(i = 0;i < ROWSIZE;i++)
	{
		for(j = 0;j < COLSIZE;j++)
		{
			printf("%03d ",m[i][j]);
		}
		printf("\n");
	}
}
void display_path(char map[][COLSIZE],int parent[][COLSIZE],int s0,int s1,int g0,int g1)
{
	int i,j;
	int index;
	int top = -1;
	int st[PATHSIZE];
	i = g0;
	j = g1;
	//index2map(index,&i,&j);
	index = parent[i][j];
	while(index != 0)
	{
		map[i][j] = 2;
		index = parent[i][j];
		st[++top] = index;
		index2map(index,&i,&j);
	}
	top--;
	map[s0][s1] = 3;
	map[g0][g1] = 4;
	display_map(map);
	printf("top :%d\n",top);
	i = 0;
	while(top > 0)
	{
		//path[i++] = st[top--];
		j = st[top--];
		printf("j: %d\n",j);
		path[i++] = j;
	}
	#if 0
	for(j = 0;j <i;i++)
	{
		printf("%d ",path[j]);
	}
	//#endif
	printf("\n-------------------\n");
	
	for(i = 0;i < ROWSIZE;i++)
	{
		for(j = 0;j < COLSIZE;j++)
		{
			/*
			if(map[i][j])
				printf("■ ");
			else
				printf("0 ");
			*/
			printf("%d ",parent[i][j]);
		}
		printf("\n");
	}
	#endif
}
int h(int c0,int c1,int g0,int g1)
{
	return abs(g0 - c0) + abs(g1 - c1);
}

int find_min_cost(int cost[][COLSIZE],int vist[][COLSIZE],int *min0,int *min1,int g0,int g1)
{
	int i,j;
	int dist = 0;
	int mini_g = BIGDATA;
	for(i = 0;i < ROWSIZE;i++)
	{
		for(j = 0;j < COLSIZE;j++)
		{
			dist = h(i,j,g0,g1);
			
			if((cost[i][j] + dist < mini_g) && vist[i][j] == 1)
			{
				mini_g = cost[i][j] + dist;
				//printf("dist : %d\n",dist);
				*min0 = i;
				*min1 = j;
			}
		}
	}
	printf("(%d %d)\n",*min0,*min1);
	return mini_g;
}

void find_path_dijkstra(char map[][COLSIZE],int s,int g)
{
	printf("进来 s:%d g:%d\n",s,g);
	#if 1
	int vist[ROWSIZE][COLSIZE] = {0};
	int parent[ROWSIZE][COLSIZE] = {0};
	int cost[ROWSIZE][COLSIZE] = {0};
	int c0,c1;
	int s0 = 0,s1 = 0;
	int g0 = 0,g1 = 0;
	index2map(s,&s0,&s1);
	index2map(g,&g0,&g1);
	memset(cost,999,sizeof(cost));
	//display_map(parent);
	//display_map(cost);
	c0 = s0; c1 = s1;
	vist[c0][c1] = 2;
	cost[c0][c1] = 0;
	while(1)
	{
		if((c0-1) >= 0 && !map[c0-1][c1])
		{
			if(vist[c0-1][c1] == 0)//关闭列表
			{
				cost[c0-1][c1] = cost[c0][c1] + 1;
				vist[c0-1][c1] = 1;
				parent[c0-1][c1] = map2index(c0,c1);
			}
			else if((vist[c0-1][c1] == 1) && (cost[c0][c1] + 1 < cost[c0-1][c1]))//开列表
			{
				cost[c0-1][c1] = cost[c0][c1] + 1;
				parent[c0-1][c1] = map2index(c0,c1);
			}
		}
		if((c0+1) < ROWSIZE && !map[c0+1][c1])
		{
			if(vist[c0+1][c1] == 0)
			{
				cost[c0+1][c1] = cost[c0][c1] + 1;
				vist[c0+1][c1] = 1;
				parent[c0+1][c1] = map2index(c0,c1);
			}
			else if((vist[c0+1][c1] == 1) && (cost[c0][c1] + 1 < cost[c0+1][c1]))
			{
				cost[c0+1][c1] = cost[c0][c1] + 1;
				parent[c0+1][c1] = map2index(c0,c1);
			}
		}
		if((c1-1) >= 0 && !map[c0][c1-1])
		{
			if(vist[c0][c1-1] == 0)
			{
				cost[c0][c1-1] = cost[c0][c1] + 1;
				vist[c0][c1-1] = 1;
				parent[c0][c1-1] = map2index(c0,c1);
			}
			else if((vist[c0][c1-1] == 1) && (cost[c0][c1] + 1 < cost[c0][c1-1]))
			{
				cost[c0][c1-1] = cost[c0][c1] + 1;
				parent[c0][c1-1] = map2index(c0,c1);
			}
		}
		if((c1+1) < COLSIZE && !map[c0][c1+1])
		{
			if(vist[c0][c1+1] == 0)
			{
				cost[c0][c1+1] = cost[c0][c1] + 1;
				vist[c0][c1+1] = 1;
				parent[c0][c1+1] = map2index(c0,c1);
			}
			else if((vist[c0][c1+1] == 1) && (cost[c0][c1] + 1 < cost[c0][c1+1]))
			{
				cost[c0][c1+1] = cost[c0][c1] + 1;
				parent[c0][c1+1] = map2index(c0,c1);
			}
		}
		
		
		if((find_min_cost(cost,vist,&c0,&c1,g0,g1) == BIGDATA) || (c0 == g0 && c1 == g1))
		{
			printf("path over\n");
			break;
		}

		vist[c0][c1] = 2;
	}
	//display_matrix(vist);
	//display_matrix(cost);
	display_path(map,parent,s0,s1,g0,g1);
	#endif
}

int main(void)
{
	char start[] = {1,1};
	//char end[] = {3,5};
	//char end[] = {8,4};
	//char end[] = {4,8};
	char end[] = {8,8};
	int s,g;
	int n = sizeof(obstacle_col)/sizeof(obstacle_col[0]);
	create_map(map,obstacle_row,obstacle_col,n);
	display_map(map);
	s = map2index(start[0],start[1]);
	g = map2index(end[0],end[1]);
	
	find_path_dijkstra(map,s,g);
	
	return 0;
}

运行结果