版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/qq_29796781/article/details/81868855
原理参考 https://www.cnblogs.com/chxer/p/4542068.html
代码实现
#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 <= COLSIZE+1;i++)
printf("✠ ");
printf("\n");
for(i = 0;i < ROWSIZE;i++)
{
printf("✠ ");
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("✠ ");
printf("\n");
}
for(i = 0;i <= COLSIZE +1;i++)
printf("✠ ");
printf("\n");
}
void display_matrix(int m[][COLSIZE])
{
int i,j;
for(i = 0;i < ROWSIZE;i++)
{
for(j = 0;j < COLSIZE;j++)
{
/*
if(map[i][j])
printf("■ ");
else
printf("0 ");
*/
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("节点: %d\n",j);
path[i++] = j;
}
#if 0
for(j = 0;j <i;i++)
{
printf("%d ",path[j]);
}
#endif
printf("\n-------------------\n");
}
int h(int c0,int c1,int g0,int g1)
{
return abs(g0 - c0) + abs(g1 - c0);
}
int find_min_cost(int cost[][COLSIZE],char vist[][COLSIZE],int *min0,int *min1)
{
int i,j;
int mini_g = BIGDATA;
for(i = 0;i < ROWSIZE;i++)
{
for(j = 0;j < COLSIZE;j++)
{
if((cost[i][j] < mini_g) && vist[i][j] == 1)
{
mini_g = cost[i][j];
*min0 = i;
*min1 = j;
}
}
}
return mini_g;
}
void find_path_dijkstra(char map[][COLSIZE],int s,int g)
{
printf("进来 s:%d g:%d\n",s,g);
#if 1
char 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,0xff,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) == BIGDATA) || (c0 == g0 && c1 == g1))
{
printf("path over\n");
break;
}
vist[c0][c1] = 2;//移除该点
}
display_matrix(cost);
display_path(map,parent,s0,s1,g0,g1);
#endif
}
int main(void)
{
char start[] = {1,1};// 起始点
//char start[] = {3,9};
//char end[] = {3,5};
//char end[] = {8,4};
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;
}
运行结果