A* 搜索算法

最新推荐文章于 2024-05-23 18:00:27 发布

null_ptr

最新推荐文章于 2024-05-23 18:00:27 发布

阅读量792

点赞数

分类专栏：程序设计

本文链接：https://blog.csdn.net/derek_yi/article/details/13748825

版权

程序设计专栏收录该内容

27 篇文章 0 订阅

订阅专栏

营救公主：

公主被魔王抓走了，王子需要拯救出美丽的公主。他进入了魔王的城堡，魔王的城堡是一座很大的迷宫。为了使问题简单化，我们假设这个迷宫是一个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;
}