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\t", (array[i] + j)->data);
        }
        printf("\n");
    }
}

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!\n");
    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\n", endLNode->path_next->x,endLNode->path_next->y);
        endLNode->path_next = endLNode->path_next->path_next;
    }
    return 0;
}