Dijkstra 迪傑斯特拉 尋找全域性路徑實現 純程式碼
阿新 • • 發佈:2018-12-21
原理參考 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; }
執行結果