距离估计与实际值越接近，估价函数取得就越好
例如对于几何路网来说，可以取两节点间曼哈顿距离做为距离估计，即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);
}
发表评论
◎欢迎参与讨论，请在这里发表您的看法、交流您的观点。