A*搜索算法

先了解一下什么是A*算法。

A*搜寻算法，俗称A星算法。A*算法是用于寻找两点之间的最短路径,同时它也是一种静态路网中求解最短路最有效的直搜索方法.这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。常用于游戏中的NPC（Non-Player-ControlledCharacter）的移动计算，或线上游戏的BOT（ROBOT）的移动计算上。该算法像Dijkstra算法一样，可以找到一条最短路径；也像BFS一样，进行启发式的搜索。
A算法是一种启发式搜索算法，启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。

A星算法核心公式：

估价函数：

估价函数f(n)被定义为从初始节点S0出发，约束经过节点n到达目标节点Sg的所有路径中最小路径代价的估计值。它的一般形式为： f(n)=g(n)+h(n)

其中，g(n)是从初始节点S0到节点n的实际代价；h(n)是从节点n到目标节点Sg的最优路径的估计代价。 

G值是怎么计算的？
假设现在我们在某一格子，邻近有8个格子可走，当我们往上、下、左、右这4个格子走时，移动代价为10；当往左上、左下、右上、右下这4个格子走时，移动代价为14；即走斜线的移动代价为走直线的1.4倍。
这就是G值最基本的计算方式，适用于大多数2.5Drpg页游。
根据游戏需要，G值的计算可以进行拓展。如加上地形因素对寻路的影响。格子地形不同，那么选择通过不同地形格子，移动代价肯定不同。同一段路，平地地形和丘陵地形，虽然都可以走，但平地地形显然更易走。
我们可以给不同地形赋予不同代价因子，来体现出G值的差异。如给平地地形设置代价因子为1，丘陵地形为2，在移动代价相同情况下，平地地形的G值更低，算法就会倾向选择G值更小的平地地形。
H值是如何预估出来的？
很显然，在只知道当前点，结束点，不知道这两者的路径情况下，我们无法精确地确定H值大小，所以只能进行预估。
有多种方式可以预估H值，如曼哈顿距离、欧式距离、对角线估价，最常用最简单的方法就是使用曼哈顿距离进行预估：
H = 当前方块到结束点的水平距离 + 当前方块到结束点的垂直距离
题外话：A星算法之所以被认为是具有启发策略的算法，在于其可通过预估H值，降低走弯路的可能性，更容易找到一条更短的路径。其他不具有启发策略的算法，没有做预估处理，只是穷举出所有可通行路径，然后从中挑选一条最短的路径。这也是A星算法效率更高的原因。

A*算法流程：
    首先将起始结点S放入OPEN表，CLOSE表置空，算法开始时：
      1、如果OPEN表不为空，从表头取一个结点n，如果为空算法失败。
      2、n是目标解吗？是，找到一个解（继续寻找，或终止算法）。
      3、将n的所有后继结点展开，就是从n可以直接关联的结点（子结点），如果不在CLOSE表中，就将它们放入OPEN表，并把S放入CLOSE表，同时计算每一个后继结点的估价值f(n)，将OPEN表按f(x)排序，最小的放在表头，重复算法，回到1。

A*算法与广度、深度优先和Dijkstra 算法的联系就在于：当g(n)=0时，该算法类似于DFS，当h(n)=0时，该算法类似于BFS。且同时，如果h(n)为0，只需求出g(n)，即求出起点到任意顶点n的最短路径，则转化为单源最短路径问题，即Dijkstra算法。这一点，可以通过上面的A*搜索树的具体过程中将h(n)设为0或将g(n)设为0而得到。

A*搜索算法（python）

A*算法（附c源码）

Apath.c

#include "Apath.h"

LinkList InitList()
{
    LinkList L = (LinkList)malloc(sizeof(LNode));
    if (L == NULL)
    {
        printf("Defeat!");
        exit(1);
    }
    memset(L,0,sizeof(LNode));
    return L;
}//LinkList()

LNode** malloc_array2D(int row, int col)
{
    LNode** map = (LNode**)malloc(row*sizeof(LNode*) + row*col*sizeof(LNode));
    LNode* head = (LNode*)(map + row);
    for (int i = 0; i < row; ++i)
        map[i] = head + i*col;
    return map;
}

LNode** Translate_array(int array[][10], int row, int col)
{
    LNode **map = malloc_array2D(10, 10);
    for (int i = 0; i < row; ++i)
        for (int j = 0; j < col; ++j)
        {
            (map[i] + j)->data = array[i][j];
            (map[i] + j)->G = 0;
            (map[i] + j)->H = 0;
            (map[i] + j)->F = 0;    //(map[i] + j)->G + (map[i] + j)->H;
            (map[i] + j)->x = i;
            (map[i] + j)->y = j;
            (map[i] + j)->Close_flag = 0;
            (map[i] + j)->OPen_flag = 0;
            (map[i] + j)->next = NULL;
            (map[i] + j)->path_next = NULL;
        }
    return map;
}//Translate_array()

void free_array2D(LNode **arr)
{
    free(arr);
}

void output(LNode** array, int row, int col)  //二维数组的访问必须指明位数，否则编译器不能解析
{
    //for (int i = 0; i < row; ++i)
    //    for (int j = 0; j < col; ++j)
    //    {
    //        (array[i] + j)->F = j;
    //    }
    for (int i = 0; i < row; ++i)
    {
        for (int j = 0; j < col; ++j)
        {
            printf("%d	", (array[i] + j)->data);
        }
        printf("
");
    }
}

LNode* find_start_LNode(LNode** Arr, int row, int col)    //从数组中找到始点
{
    LNode* start_LNode = NULL;
    for (int i = 0; i < row; ++i)
    {
        for (int j = 0; j < col; ++j)
        {
            if (2 == (Arr[i] + j)->data)
            {
                start_LNode = (Arr[i] + j);
                //起点H=0,G=0,F=0
                start_LNode->G = 0;
                start_LNode->H = 0;
                start_LNode->F = 0;        //起点，则默认所有值为0
                return start_LNode;        //返回节点
            }
        }
    }
    return NULL;
}
LNode* find_end_LNode(LNode** Arr, int row, int col)        //从数组中找到终点
{
    LNode* end_LNode = NULL;
    for (int i = 0; i < row; ++i)
    {
        for (int j = 0; j < col; ++j)
        {
            if (3 == (Arr[i] + j)->data)
            {
                end_LNode = (*(Arr + i) + j);
                end_LNode->F = 0;
                end_LNode->G = 0;
                end_LNode->H = 0;
                return end_LNode;        //返回节点
            }
        }
    }
    return NULL;
}

int count_LNode_G(LNode* curLNode, LNode* aheadLNode)        //计算节点的G值
{
    if (curLNode->x == aheadLNode->y && curLNode->y == aheadLNode->y)
        return 0;
    if (aheadLNode->x - curLNode->x != 0 && aheadLNode->y - curLNode->y !=0)
        curLNode->G = aheadLNode->G + 14;
    else
        curLNode->G = aheadLNode->G + 10;
    return curLNode->G;
}
int count_LNode_H(LNode* curLNode, LNode* endLNode)        //计算节点的H值
{
    curLNode->H = abs(endLNode->x - curLNode->x) * 10 + abs(endLNode->y - curLNode->y) * 10;
    return curLNode->H;
}
int count_LNode_F(LNode* curLNode)        //计算节点的F值
{
    curLNode->F = curLNode->G + curLNode->H;
    return curLNode->F;
}

void push_OpenList_Node(LinkList L, LNode *elem)        //按从小到大的顺序
{
    LNode *p, *q;
    p = q = L;
    while (p->next != NULL && p->F < elem->F)
    {
        q = p;
        p = p->next;
    }
    if (p->F < elem->F) q = p;
    elem->next = q->next;
    q->next = elem;
    //插入成功，更改属性值OPen_flag = 1
    elem->OPen_flag = 1;
}
LNode* pop_OpenList_minNode(LinkList L_OpenList )        //返回开放列表中F值最小的节点
{
    LNode *elem = NULL;
    if (L_OpenList->next)        //为了安全，防止访问空指针
    {
        L_OpenList->next->OPen_flag = 0;
        elem = L_OpenList->next;
        L_OpenList->next = L_OpenList->next->next;
        elem->next = NULL;
    }
    else
        printf("have a NULL point in pop_OpenList_mimNode()");
    return elem;
}
bool insert_Into_CloseList(LNode* min_Open, LinkList L_CloseList)//插入OpenList中F值最小的节点到CloseList中去
{
    //对于CloseList中的节点并不需要排序,采用头插法
    min_Open->next = L_CloseList->next;
    L_CloseList->next = min_Open;
    min_Open->Close_flag = 1;
    return TURE;
}

bool isExist_openList(LNode* curLNode)
{
    return curLNode->OPen_flag;
}
bool isExist_closeList(LNode* curLNode)
{
    return curLNode->Close_flag;
}
bool isobstacle(LNode* curLNode)
{
    if (curLNode->data == 1)
        return TURE;
    else
        return FAULT;
}
bool isJoin(LNode* cur)        //该节点是否可以加入开放列表
{
    if (cur->x > -1 && cur->y > -1)            //边界检测
    {
        if (!isExist_closeList(cur) && !isobstacle(cur))        //既不在关闭列表里，也不是障碍物
        {
            return TURE;
        }
        else
            return FAULT;
    }
    return FAULT;
}
void insert_open(LNode *Node, LNode* ahead, LNode* endLNode, LinkList open_list, LNode** Arr)
{
    if (isJoin(Node))
    {
        if (isExist_openList(Node))
        {
            if (Node->x - ahead->x != 0 && Node->y - ahead->y != 0) {
                if (Node->F > (ahead->F + 14))
                {
                    count_LNode_G(Node, ahead);
                    count_LNode_F(Node);        //H值没有改变，所以还是原来的值
                    Node->path_next = ahead;        //也不用再插入
                }
            }
            else {
                if (Node->F > (ahead->F + 10))
                {
                    count_LNode_G(Node, ahead);
                    count_LNode_F(Node);        //H值没有改变，所以还是原来的值
                    Node->path_next = ahead;        //也不用再插入
                }
            }
        }
        else {
            count_LNode_G(Node, ahead);
            count_LNode_H(Node, endLNode);
            count_LNode_F(Node);
            Node->path_next = ahead;
            push_OpenList_Node(open_list, Node);
        }
    }
}
void check_around_curNode(LNode* cur, LNode* endLNode, LinkList open_list, LNode** Arr)
{
    int x = cur->x;
    int y = cur->y;
    insert_open(Arr[x] + y - 1, cur, endLNode, open_list, Arr);
    insert_open(Arr[x] + y + 1, cur, endLNode, open_list, Arr);
    insert_open(Arr[x + 1] + y, cur, endLNode, open_list, Arr);
    insert_open(Arr[x + 1] + y - 1, cur, endLNode, open_list, Arr);
    insert_open(Arr[x + 1] + y + 1, cur, endLNode, open_list, Arr);
    insert_open(Arr[x - 1] + y, cur, endLNode, open_list, Arr);
    insert_open(Arr[x - 1] + y + 1, cur, endLNode, open_list, Arr);
    insert_open(Arr[x - 1] + y - 1, cur, endLNode, open_list, Arr);
}

 Apath.h

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdbool.h>

#ifndef APATH_H
#define APATH_H
#endif

#define TURE 1
#define FAULT 0

//约定：0是可走的，1表示障碍物不可走，2表示起点，3表示终点，4表示路径
#define int_0 0
#define int_1 1
#define int_2 2
#define int_3 3
#define int_4 4

#define MAP_MAX_X 10   //地图边界，二维数组大小
#define MAP_MAX_Y 10

typedef struct LNode {
    int data;    //对应数组中的数值
    int F;   //F = G + H;
    int G;   //G：从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径
    int H;   //H：从指定的方格移动到终点 B 的估算成本
    int x, y;   //对应数组中的坐标
    bool OPen_flag;  //在开放列表中为1，不在为0
    bool Close_flag;  //在关闭列表中为1，不在为0
    struct LNode* next;                    //用于链表排序
    struct LNode* path_next;            //用于最终找到的路径
}LNode, *LinkList;

LinkList InitList();  //返回一个初始化的链表
LNode** malloc_array2D(int row, int col);
void free_array2D(LNode **arr);
LNode** Translate_array(int array[10][10], int row, int col);    //将一个普通数组翻译为单链表节点的数组
void output(LNode **array, int row, int col);

LNode* find_start_LNode(LNode** Arr, int row, int col);    //从数组中找到始点
LNode* find_end_LNode(LNode** Arr, int row, int col);        //从数组中找到终点

//忘记这些要干嘛了，重写吧
bool isExist_ALNode_in_List(LNode* curLNode, LinkList L_OpenList);    //查看节点是否在链表中，在返回ture,不在返回fault
//对关闭列表中的当前节点进行检查，看它周围的节点是否在OpenList链表里，不在:添加进去；在：检查经过它到达起点的G是否最小，是：修改，不是：不修改
//LNode* check_CloseList_curLNode(LNode* curLNode, LNode* endLNode, LinkList L_OpenList, LinkList L_CloseList, LNode** Arr);   

LNode* pop_OpenList_minNode(LinkList L_OpenList);        //返回开放列表中F值最小的节点
void push_OpenList_Node(LinkList L, LNode *elem);   //插入一个节点并排序
bool insert_Into_CloseList(LNode* min_Open, LinkList L_CloseList);//插入OpenList中F值最小的节点到CloseList中去


int count_LNode_G(LNode* curLNode, LNode* aheadLNode);        //计算节点的G值
int count_LNode_H(LNode* curLNode, LNode* endLNode);        //计算节点的H值
int count_LNode_F(LNode* curLNode);        //计算节点的F值

bool isExist_openList(LNode* curLNode);    //查看节点是否在链表中，在返回ture,不在返回fault
bool isExist_closeList(LNode* curLNode);
bool isobstacle(LNode* curLNode);
void check_around_curNode(LNode* cur, LNode* endLNode, LinkList open_list, LNode** Arr);        //检查周围的节点，是否合适加入开放列表

main.c

#include <stdio.h>
//#ifndef APATH_H
#include "Apath.h"
//#endif

//为简单，干脆把把下面数组转为链表结构的数组
//约定：0是可走的，1表示障碍物不可走，2表示起点，3表示终点，4表示路径
int array[10][10] = {
    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
    { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 0, 1, 3, 0, 0, 0 },
    { 0, 0, 2, 0, 0, 1, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
    { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } };

int main()
{
    int row = MAP_MAX_X, col = MAP_MAX_Y;
    printf("hello world!
");
    LNode **map = Translate_array(array,row, col); //这里将数组的地图转为节点map的地图
    output(map,10,10);
    LinkList open_List = InitList();     //定义并初始化一个开放列表
    LinkList close_List = InitList();    //一个封闭列表
    LNode* startLNode = find_start_LNode(map, row, col);
    LNode* endLNode = find_end_LNode(map, row, col);

    LNode* curLNode = startLNode;        //当前节点=开始节点
    curLNode->G = 0;        //计算节点的三个值
    count_LNode_H(curLNode, endLNode);
    count_LNode_F(curLNode);
    push_OpenList_Node(open_List, curLNode);        //先将开始节点插入开放列表

    while (curLNode->data != 3)
    {
        //LNode *e = NULL;
        curLNode = pop_OpenList_minNode(open_List);
        insert_Into_CloseList(curLNode, close_List);
        //2、查看起点周围的点是否在开放列表里，不在加入，在检测经过该点F值是否最小等；
        check_around_curNode(curLNode, endLNode, open_List, map);
    }
    while (endLNode->path_next)
    {
        printf("x:%d---y:%d
", endLNode->path_next->x,endLNode->path_next->y);
        endLNode->path_next = endLNode->path_next->path_next;
    }
    return 0;
}