A* 搜索算法

营救公主：

公主被魔王抓走了，王子需要拯救出美丽的公主。他进入了魔王的城堡，魔王的城堡是一座很大的迷宫。为了使问题简单化，我们假设这个迷宫是一个N*M的二维方格。迷宫里有一些墙，王子不能通过。王子只能移动到相邻（上下左右四个方向)的方格内，并且一天只能移动一步，就是说，如果王子在(x,y）一步只能移动到(x-1,y),(x+1,y),(x,y-1),(x,y+1)其中的一个位置上。地图由‘S’，‘P’，‘.’，‘*’四种符号构成，‘.’表示王子可以通过，‘*’表示墙，王子不能通过；'S'表示王子的位置；‘P’表示公主的位置；T表示公主存活的剩余天数，王子必须在T天内到达公主的位置，才能救活公主。如下图所示：



上面是一个5*5的迷宫，红色箭头标识的是从S到P的一条路径。这条路径是最短的一条。如果题目中给的T是5的话，那么就无法救出公主。

实现一个算法，计算王子到达公主位置的天数，如果不可达，返回-1。

int get_min_step(char *map, int row, int col)

来自WIKI：

A*搜索算法，俗称A星算法。这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。

常用于游戏中的NPC的移动计算，或在线游戏的BOT的移动计算上。

该算法像Dijkstra算法一样，可以找到一条最短路径；也像BFS一样，进行启发式的搜索。

在此算法中，如果以 g(n)表示从起点到任意顶点n的实际距离，h(n)表示任意顶点n到目标顶点的估算距离，那么 A*算法的公式为：f(n)=g(n)+h(n)。

这个公式遵循以下特性：

如果h(n)为0，只需求出g(n)，即求出起点到任意顶点n的最短路径，则转化为单源最短路径问题，即Dijkstra算法

如果h(n)<=“n到目标的实际距离”，则一定可以求出最优解。而且h(n)越小，需要计算的节点越多，算法效率越低。

伪码：

function A*(start,goal)
closedset := the empty set                 //已经被估算的节点集合
openset := set containing the initial node //将要被估算的节点集合
came_from := empty map
g_score[start] := 0                        //g(n)
h_score[start] := heuristic_estimate_of_distance(start, goal)    //h(n)
f_score[start] := h_score[start]            //f(n)=h(n)+g(n)，由于g(n)=0，所以……
while openset is not empty
x := the node in openset having the lowest f_score[] value
if x = goal
return reconstruct_path(came_from,goal)
remove x from openset
add x to closedset
foreach y in neighbor_nodes(x)  //foreach=for each
if y in closedset
continue
tentative_g_score := g_score[x] + dist_between(x,y)

if y not in openset
add y to openset

tentative_is_better := true
elseif tentative_g_score < g_score[y]
tentative_is_better := true
else
tentative_is_better := false
if tentative_is_better = true
came_from[y] := x
g_score[y] := tentative_g_score
h_score[y] := heuristic_estimate_of_distance(y, goal)
f_score[y] := g_score[y] + h_score[y]
return failure

function reconstruct_path(came_from,current_node)
if came_from[current_node] is set
p = reconstruct_path(came_from,came_from[current_node])
return (p + current_node)
else
return current_node

根据伪码直译的C代码，无任何优化，VC测试可用，寻路范围较小，所以用数组，有待改造成链表。

#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MAX_ROW_NUM     20

typedef struct tagCELL_INFO
{
int state; /* 0-road 1-wall 2-jessi 3-princess */
int g_value;
int h_value;
int f_value;
}CELL_INFO;

typedef struct tagMAP_INO_ST
{
CELL_INFO cells[MAX_ROW_NUM * MAX_ROW_NUM];
char open_list[MAX_ROW_NUM * MAX_ROW_NUM];
char close_list[MAX_ROW_NUM * MAX_ROW_NUM];
int came_from[MAX_ROW_NUM * MAX_ROW_NUM];
int real_row;
int real_col;
int start_pos;
int dst_pos;
}MAP_INFO_ST;

MAP_INFO_ST map_info;

int min_step = 0;

#define INVALID_NODE_VALUE 	0xFFFFFF

int xdebug = 1;
#define xprintf if(xdebug)printf

int open_set_is_empty()
{
int i;

for(i = 0; i < (MAX_ROW_NUM * MAX_ROW_NUM); i++) {
if (map_info.open_list[i] != 0) return 0;
}
return 1;
}

int estimate_of_distance(int start, int goal)
{
int x1, y1;
int x2, y2;

x1 = start/map_info.real_col;
y1 = start%map_info.real_col;
x2 = goal/map_info.real_col;
y2 = goal%map_info.real_col;

return abs(x1 - x2) + abs(y1 - y2);
}

int least_score_in_open_set()
{
int i;
int min_idx = 0, min_f = INVALID_NODE_VALUE;

for(i = 0; i < (MAX_ROW_NUM * MAX_ROW_NUM); i++) {
if (map_info.open_list[i] != 0) {
if (map_info.cells[i].f_value < min_f) {
min_idx = i;
min_f = map_info.cells[i].f_value;
}
}
}

return min_idx;
}

/*  function reconstruct_path(came_from,current_node)
if came_from[current_node] is set
p = reconstruct_path(came_from,came_from[current_node])
return (p + current_node)
else
return current_node
*/
int reconstruct_path(int curr_node)
{
int p;
int x = curr_node/map_info.real_col;
int y = curr_node%map_info.real_col;

min_step++;
xprintf("%d(%d,%d) <-- ", curr_node, x, y);
if (map_info.came_from[curr_node] != INVALID_NODE_VALUE) {
p = reconstruct_path(map_info.came_from[curr_node]);
return p + curr_node;
} else
return curr_node;
}

int neighbor_of_x(int start_pos, int offset)
{
int x,y;
int pos;

x = start_pos/map_info.real_col;
y = start_pos%map_info.real_col;

if (offset == 0) { // down cell
if (x == map_info.real_row - 1) return -1;
pos = (x + 1)*map_info.real_col + y;
} else if (offset == 1) { // up cell
if (x == 0) return -1;
pos = (x - 1)*map_info.real_col + y;
} else if (offset == 2) { // right cell
if (y == map_info.real_col - 1) return -1;
pos = x*map_info.real_col + y + 1;
} else if (offset == 3) { // left cell
if (y == 0) return -1;
pos = x*map_info.real_col + y - 1;
}

if (map_info.cells[pos].state == 1) return -1;
return pos;
}

int astart_run(int start_pos, int goal_pos)
{
int i;
int x, y;
int tentative_g_score;
int tentative_is_better;

// openset := set containing the initial node
map_info.open_list[start_pos] = 1;

// came_from := empty map
for(i = 0; i < MAX_ROW_NUM * MAX_ROW_NUM; i++) {
map_info.came_from[i] = INVALID_NODE_VALUE;
}

// set g/h/f of initial node
map_info.cells[start_pos].g_value = 0;
map_info.cells[start_pos].h_value = estimate_of_distance(start_pos, goal_pos);
map_info.cells[start_pos].f_value = map_info.cells[start_pos].h_value;

while(!open_set_is_empty()) {
//x := the node in openset having the lowest f_score[] value
x = least_score_in_open_set();
xprintf("\r\n process mnode %d", x);
if (x == goal_pos) {
xprintf("\r\n reach the goal: ");
return reconstruct_path(goal_pos);
}

//remove x from openset, add x to closedset
map_info.open_list[x] = 0;
map_info.close_list[x] = 1;

//foreach y in neighbor_nodes(x)
for( i = 0; i < 4; i++) {
y = neighbor_of_x(x, i);
if (y < 0) continue;
xprintf("\r\n process snode %d", y);

// if y in closedset
if (map_info.close_list[y]) continue;

//tentative_g_score := g_score[x] + dist_between(x,y)
tentative_g_score = map_info.cells[x].g_value + 1;

if (!map_info.open_list[y]) {
map_info.open_list[y] = 1;
tentative_is_better = 1;
} else if (tentative_g_score < map_info.cells[y].g_value) {
tentative_is_better = 1;
} else {
tentative_is_better = 0;
}

if (tentative_is_better) {
// came_from[y] := x
map_info.came_from[y] = x;

map_info.cells[y].g_value = tentative_g_score;
map_info.cells[y].h_value = estimate_of_distance(y, goal_pos);
map_info.cells[y].f_value = map_info.cells[y].g_value + map_info.cells[y].h_value;
}
}
}

return -1;
}

int get_min_step(char *map, int row, int col)
{
int i, j, pos;

memset(&map_info, 0, sizeof(MAP_INFO_ST));
map_info.real_row = row;
map_info.real_col = col;
for(i = 0; i < row; i++) {
for(j = 0; j < col; j++) {
pos = i*map_info.real_col + j;
if ('.' == map[pos]) {
map_info.cells[pos].state = 0;
} else if ('*' == map[pos]) {
map_info.cells[pos].state = 1;
} else if ('S' == map[pos]) {
map_info.cells[pos].state = 2;
map_info.start_pos = pos;
} else if ('P' == map[pos]) {
map_info.cells[pos].state = 3;
map_info.dst_pos = pos;
} else {
return -1;
}
}
}

min_step = 0;
astart_run(map_info.start_pos, map_info.dst_pos);

return min_step - 1;
}

int _tmain(int argc, _TCHAR* argv[])
{
int i,j;
char map_input1[4][4] = {'.','.','.','.',
'.','.','.','.',
'.','.','.','.',
'S','*','*','P'};
char map_input2[9][9] = { 	'.','S','.','.','.','.','.','.','.',
'.','*','*','.','.','.','.','.','.',
'.','.','.','*','*','.','.','.','.',
'*','*','*','.','.','*','.','.','.',
'.','.','.','.','.','.','*','.','.',
'.','.','.','.','.','.','.','.','.',
'.','.','.','.','.','.','.','*','*',
'.','.','.','.','*','*','*','.','.',
'.','.','.','.','.','.','P','.','.',};
int result;

result = get_min_step(&map_input1[0][0], 4, 4);
printf("\r\n result=%d", result);

result = get_min_step(&map_input2[0][0], 9, 9);
printf("\r\n result=%d", result);

return 0;
}