Dijkstra 迪杰斯特拉 寻找全局路径实现 纯代码

原理参考 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;
}

运行结果