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;
}
執行結果