距离估计与实际值越接近,估价函数取得就越好
例如对于几何路网来说,可以取两节点间曼哈顿距离做为距离估计,即f=g(n) + (abs(dx - nx) + abs(dy - ny));这样估价函数f(n)在g(n)一定的情况下,会或多或少的受距离估计值h(n)的制约,节点距目标点近,h值小,f值相对就小,能保证最短路的搜索向终点的方向进行。明显优于Dijkstra算法的毫无方向的向四周搜索。
算法实现(路径搜索)
创建两个表,OPEN表保存所有已生成而未考察的节点,CLOSED表中记录已访问过的节点。
算起点的h(s);
将起点放入OPEN表;
while(OPEN!=NULL){ 从OPEN表中取f(n)最小的节点n; if(n节点==目标节点) break; for(当前节点n的每个子节点X) { 计算f(X); if(XinOPEN) if(新的f(X)<OPEN中的f(X)) { 把n设置为X的父亲; 更新OPEN表中的f(n); } if(XinCLOSE) continue; if(Xnotinboth) { 把n设置为X的父亲; 求f(X); 并将X插入OPEN表中;//还没有排序 } }//endfor 将n节点插入CLOSE表中; 按照f(n)将OPEN表中的节点排序;//实际上是比较OPEN表内节点f的大小,从最小路径的节点向下进行。}//endwhile(OPEN!=NULL)
保存路径,即从终点开始,每个节点沿着父节点移动直至起点,这就是你的路径;
用C语言实现A*最短路径搜索算法
#include <stdio.h>#include <math.h> #define MaxLength 100 //用于优先队列(Open表)的数组#define Height 15 //地图高度#define Width 20 //地图宽度 #define Reachable 0 //可以到达的结点#define Bar 1 //障碍物#define Pass 2 //需要走的步数#define Source 3 //起点#define Destination 4 //终点 #define Sequential 0 //顺序遍历#define NoSolution 2 //无解决方案#define Infinity 0xfffffff #define East (1 << 0)#define South_East (1 << 1)#define South (1 << 2)#define South_West (1 << 3)#define West (1 << 4)#define North_West (1 << 5)#define North (1 << 6)#define North_East (1 << 7) typedef struct{ signed char x, y;} Point; const Point dir[8] ={ {0, 1}, // East {1, 1}, // South_East {1, 0}, // South {1, -1}, // South_West {0, -1}, // West {-1, -1}, // North_West {-1, 0}, // North {-1, 1} // North_East}; unsigned char within(int x, int y){ return (x >= 0 && y >= 0 && x < Height && y < Width);} typedef struct{ int x, y; unsigned char reachable, sur, value;} MapNode; typedef struct Close{ MapNode *cur; char vis; struct Close *from; float F, G; int H;} Close; typedef struct //优先队列(Open表){ int length; //当前队列的长度 Close* Array[MaxLength]; //评价结点的指针} Open; static MapNode graph[Height][Width];static int srcX, srcY, dstX, dstY; //起始点、终点static Close close[Height][Width]; // 优先队列基本操作void initOpen(Open *q) //优先队列初始化{ q->length = 0; // 队内元素数初始为0} void push(Open *q, Close cls[Height][Width], int x, int y, float g){ //向优先队列(Open表)中添加元素 Close *t; int i, mintag; cls[x][y].G = g; //所添加节点的坐标 cls[x][y].F = cls[x][y].G + cls[x][y].H; q->Array[q->length++] = &(cls[x][y]); mintag = q->length - 1; for (i = 0; i < q->length - 1; i++) { if (q->Array[i]->F < q->Array[mintag]->F) { mintag = i; } } t = q->Array[q->length - 1]; q->Array[q->length - 1] = q->Array[mintag]; q->Array[mintag] = t; //将评价函数值最小节点置于队头} Close* shift(Open *q){ return q->Array[--q->length];} // 地图初始化操作void initClose(Close cls[Height][Width], int sx, int sy, int dx, int dy){ // 地图Close表初始化配置 int i, j; for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { cls[i][j].cur = &graph[i][j]; // Close表所指节点 cls[i][j].vis = !graph[i][j].reachable; // 是否被访问 cls[i][j].from = NULL; // 所来节点 cls[i][j].G = cls[i][j].F = 0; cls[i][j].H = abs(dx - i) + abs(dy - j); // 评价函数值 } } cls[sx][sy].F = cls[sx][sy].H; //起始点评价初始值 // cls[sy][sy].G = 0; //移步花费代价值 cls[dx][dy].G = Infinity;} void initGraph(const int map[Height][Width], int sx, int sy, int dx, int dy){ //地图发生变化时重新构造地 int i, j; srcX = sx; //起点X坐标 srcY = sy; //起点Y坐标 dstX = dx; //终点X坐标 dstY = dy; //终点Y坐标 for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { graph[i][j].x = i; //地图坐标X graph[i][j].y = j; //地图坐标Y graph[i][j].value = map[i][j]; graph[i][j].reachable = (graph[i][j].value == Reachable); // 节点可到达性 graph[i][j].sur = 0; //邻接节点个数 if (!graph[i][j].reachable) { continue; } if (j > 0) { if (graph[i][j - 1].reachable) // left节点可以到达 { graph[i][j].sur |= West; graph[i][j - 1].sur |= East; } if (i > 0) { if (graph[i - 1][j - 1].reachable && graph[i - 1][j].reachable && graph[i][j - 1].reachable) // up-left节点可以到达 { graph[i][j].sur |= North_West; graph[i - 1][j - 1].sur |= South_East; } } } if (i > 0) { if (graph[i - 1][j].reachable) // up节点可以到达 { graph[i][j].sur |= North; graph[i - 1][j].sur |= South; } if (j < Width - 1) { if (graph[i - 1][j + 1].reachable && graph[i - 1][j].reachable && map[i][j + 1] == Reachable) // up-right节点可以到达 { graph[i][j].sur |= North_East; graph[i - 1][j + 1].sur |= South_West; } } } } }} int bfs(){ int times = 0; int i, curX, curY, surX, surY; unsigned char f = 0, r = 1; Close *p; Close* q[MaxLength] = { &close[srcX][srcY] }; initClose(close, srcX, srcY, dstX, dstY); close[srcX][srcY].vis = 1; while (r != f) { p = q[f]; f = (f + 1) % MaxLength; curX = p->cur->x; curY = p->cur->y; for (i = 0; i < 8; i++) { if (! (p->cur->sur & (1 << i))) { continue; } surX = curX + dir[i].x; surY = curY + dir[i].y; if (! close[surX][surY].vis) { close[surX][surY].from = p; close[surX][surY].vis = 1; close[surX][surY].G = p->G + 1; q[r] = &close[surX][surY]; r = (r + 1) % MaxLength; } } times++; } return times;} int astar(){ // A*算法遍历 //int times = 0; int i, curX, curY, surX, surY; float surG; Open q; //Open表 Close *p; initOpen(&q); initClose(close, srcX, srcY, dstX, dstY); close[srcX][srcY].vis = 1; push(&q, close, srcX, srcY, 0); while (q.length) { //times++; p = shift(&q); curX = p->cur->x; curY = p->cur->y; if (!p->H) { return Sequential; } for (i = 0; i < 8; i++) { if (! (p->cur->sur & (1 << i))) { continue; } surX = curX + dir[i].x; surY = curY + dir[i].y; if (!close[surX][surY].vis) { close[surX][surY].vis = 1; close[surX][surY].from = p; surG = p->G + sqrt((curX - surX) * (curX - surX) + (curY - surY) * (curY - surY)); push(&q, close, surX, surY, surG); } } } //printf("times: %d\n", times); return NoSolution; //无结果} const int map[Height][Width] = { {0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,1,1}, {0,0,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1}, {0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,0,0,0,0,1}, {0,0,0,0,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,1}, {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,0,0,1,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0}, {0,1,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0}, {0,0,0,0,1,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0}, {0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,1,0,0,0,1}, {0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0}}; const char Symbol[5][3] = { "□", "▓", "▽", "☆", "◎" }; void printMap(){ int i, j; for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { printf("%s", Symbol[graph[i][j].value]); } puts(""); } puts("");} Close* getShortest(){ // 获取最短路径 int result = astar(); Close *p, *t, *q = NULL; switch(result) { case Sequential: //顺序最近 p = &(close[dstX][dstY]); while (p) //转置路径 { t = p->from; p->from = q; q = p; p = t; } close[srcX][srcY].from = q->from; return &(close[srcX][srcY]); case NoSolution: return NULL; } return NULL;} static Close *start;static int shortestep;int printShortest(){ Close *p; int step = 0; p = getShortest(); start = p; if (!p) { return 0; } else { while (p->from) { graph[p->cur->x][p->cur->y].value = Pass; printf("(%d,%d)→\n", p->cur->x, p->cur->y); p = p->from; step++; } printf("(%d,%d)\n", p->cur->x, p->cur->y); graph[srcX][srcY].value = Source; graph[dstX][dstY].value = Destination; return step; }} void clearMap(){ // Clear Map Marks of Steps Close *p = start; while (p) { graph[p->cur->x][p->cur->y].value = Reachable; p = p->from; } graph[srcX][srcY].value = map[srcX][srcY]; graph[dstX][dstY].value = map[dstX][dstY];} void printDepth(){ int i, j; for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { if (map[i][j]) { printf("%s ", Symbol[graph[i][j].value]); } else { printf("%2.0lf ", close[i][j].G); } } puts(""); } puts("");} void printSur(){ int i, j; for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { printf("%02x ", graph[i][j].sur); } puts(""); } puts("");} void printH(){ int i, j; for (i = 0; i < Height; i++) { for (j = 0; j < Width; j++) { printf("%02d ", close[i][j].H); } puts(""); } puts("");} int main(int argc, const char **argv){ initGraph(map, 0, 0, 0, 0); printMap(); while (scanf("%d %d %d %d", &srcX, &srcY, &dstX, &dstY) != EOF) { if (within(srcX, srcY) && within(dstX, dstY)) { if (shortestep = printShortest()) { printf("从(%d,%d)到(%d,%d)的最短步数是: %d\n", srcX, srcY, dstX, dstY, shortestep); printMap(); clearMap(); bfs(); //printDepth(); puts((shortestep == close[dstX][dstY].G) ? "正确" : "错误"); clearMap(); } else { printf("从(%d,%d)不可到达(%d,%d)\n", srcX, srcY, dstX, dstY); } } else { puts("输入错误!"); } } return (0);}