一、A*算法概述
A*算法是结合Dijkstra算法与BFS算法,求出最短路径的方法;估价函数f(n) = g(n) + h(n),其中g(n)是从起点到n节点的已知最短距离,h(n)是从节点n到目标点的估计距离。
二、A*算法思想
结合在Dijkstra算法文章中的思想(https://blog.csdn.net/bless_you0_0/article/details/84849360),Dijkstra算法中的距离由A*算法的f(n)来替代,每次找到一个最小的f(n)这个就是最短的,不可能有其他路径比这个更短;然后搜索该节点周围顶点,有已经搜索过的,有新加入的,新加入的直接更新f(n)的值即可,搜索过的更新最小值,由于在网格中直连中总是能够达到最小的,所以已经搜索过的不必更新最小值,直接将未搜索过的计算F值即可,最终找到最短路径。
三、A*算法程序
// Astar.cpp: 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include <iostream>
#include <stdio.h>
#include <math.h>
using namespace std;
#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) //类似于Dijkstra算法中的访问列表
{ //times++;
p = shift(&q); //从Close表中取出最小F 类似于Dijkstra算法中取出最短的Cost一样
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,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1 },
{ 0,0,0,1,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,1 },
{ 0,0,0,1,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,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 (cin >> srcX >> srcY >> dstX >> dstY)
{
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);
}
四、总结
Dijkstra算法与A*算法的核心思想都是探索周围节点并选取最短距离,这时候能保证这个距离为两节点间的最短距离。选取这个节点后将相邻节点加入搜索范围,相邻节点分为两类,一类是已经搜索过的,这时候加入一个节点要判断到已搜索节点的距离是否变短,更新最小值;另一类是未搜索的,直接计算距离记录即可。当搜索到指定节点即可获得最短路径和最短距离。