一个小笔记(5)：A*算法

A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法
其实百科有

http://baike.baidu.com/link?url=CvmkWQIAmztYgMq3Nk1WyWkDiC0koVQALKzE4wBF4CWbYBtT19iWMBdSht9LBf7ZjUnA509U-JGWvxDYBk5LCq

 

咳咳，直接上代码。各种注释也算是有助理解了，毕竟这还是抄的~

 

// A*寻路算法.cpp : 定义控制台应用程序的入口点。

// Win32控制台程序

#include
<math.h>

#include
<list>

 

using
namespace
std;

 

/*

把地图当成一个个的格子

公式：F=G+H

F：相对路径长度

G：从起点沿着产生的路径，移动到指定点的耗费（路径长度）

H：预估值，从指定的格子移动到终点格子的预计耗费

使用两个表来保存相关数据

启动列表：有可能将要经过的点存到启动列表

关闭列表：不会再被遍历的点

 

步骤

1、将起点格子加入启动列表中

2、在启动列表中查找权值（F值）最小的格子

3、查找它周围的能走的格子

4、把这些格子加入启动列表中，已经在启动或关闭列表中的格子不用加入

5、把这些加入启动列表的格子的"父格子"设为当前格子

6、再把当前格子从启动列表中删除，加入关闭列表中

7、如果终点在启动列表中，则找到路径，退出流程，不进行第9步

8、如果启动列表中没有格子了，说明没有找到路径，退出流程，不进行第9步

9、跳转第2步

*/

 

// 0：可行走的点

// 1：阻挡点

// 2：路径

// 3：起点

// 4：终点

int
g_PathLattice[10][10] =

{

    { 0,0,0,0,0,0,0,0,0,0 },

    { 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,0,0,0,0,0 },

    { 0,0,3,0,1,0,4,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,0,0,0,0 },

    { 0,0,0,0,0,0,0,0,0,0 },

    { 0,0,0,0,0,0,0,0,0,0 },

};

 

struct
Node

{

    int
row; // 行

    int
rank; // 列

    int
f;

    int
g;

    int
h;

    Node * pParent; // 当前结点路径的前一个结点（父格子）

};

 

#define
LatticeLen 10 // 格子边长

 

// 函数前向声明

int
Distance(int
row1, int
rank1, int
row2, int
rank2);

bool
IsNodeInList(Node * pNode, list<Node *> list);

Node * GetNearestNode(list<Node *> list, Node * Rec);

void
GetNearNodeList(Node * pNode, list<Node *> & listNear,

    list<Node *> listStart, list<Node *> listEnd, Node * pEndNode);

void
EraseFromList(Node * pNode, list<Node *> & listStart);

void
ClearList(list<Node *> nodeList);

 

int
main()

{

    // 起点

    int
rowStart;

    int
rankStart;

 

    // 终点

    int
rowEnd;

    int
rankEnd;

 

    // 查找起点和终点的位置

    for (int
i = 0; i < 10; i++)

    {

        for (int
j = 0; j < 10; j++)

        {

            if (g_PathLattice[i][j] == 3)

            {

                rowStart = i;

                rankStart = j;

            }

            if (g_PathLattice[i][j] == 4)

            {

                rowEnd = i;

                rankEnd = j;

            }

        }

    }

 

    // 起点

    Node * nodeStart = new
Node;

    nodeStart->row = rowStart;

    nodeStart->rank = rankStart;

    nodeStart->g = 0;

    nodeStart->h = Distance(rowStart, rankStart, rowEnd, rankEnd);

    nodeStart->f = nodeStart->h;

    nodeStart->pParent = nullptr;

 

    // 终点

    Node * nodeEnd = new
Node;

    nodeEnd->row = rowEnd;

    nodeEnd->rank = rankEnd;

 

    // 定义启动列表和关闭列表

    list<Node *> listStart;

    list<Node *> listEnd;

 

    // 把起点加入启动列表

    listStart.push_back(nodeStart);

 

    // 当前结点

    Node * pNowNode = nullptr;

 

    // 如果终点在启动列表中，则已经找到路径，退出循环

    while (!IsNodeInList(nodeEnd, listStart))

    {

        Node * Rec = nullptr;

        // 查找权值最小的格子作为当前点

        pNowNode = GetNearestNode(listStart, Rec);

 

        // 如果没有找到，则说明没有路径

        if (pNowNode == nullptr)

        {

            break;

        }

 

        // 存放当前格子周围能加入启动列表的格子

        list<Node *> listNear;

        GetNearNodeList(pNowNode, listNear, listStart, listEnd, nodeEnd);

 

        // 将当前结点加入关闭列表中

        listEnd.push_back(pNowNode);

 

        // 将当前结点从启动列表中删除

        EraseFromList(pNowNode, listStart);

 

        // 将周围点加入启动列表中

        for (list<Node *>::iterator
it = listNear.begin();

            it
!=
listNear.end(); it++)

        {

            listStart.push_back(*it);

        }

    }

 

    if (pNowNode == nullptr)

    {

        printf("路径不存在\n");

        ClearList(listStart);

        ClearList(listEnd);

        delete
nodeEnd;

 

        return 0;

    }

 

    // 在启动列表中找到终点

    Node * pNodeFind = nullptr;

    for (list<Node *>::iterator
it = listStart.begin();

        it
!=
listStart.end(); it++)

    {

        if ((*it)->row == nodeEnd->row &&

            (*it)->rank == nodeEnd->rank)

        {

            pNodeFind = (*it);

            break;

        }

    }

 

    while (pNodeFind)

    {

        g_PathLattice[pNodeFind->row][pNodeFind->rank] = 2;

        pNodeFind = pNodeFind->pParent;

    }

 

    for (int
i = 0; i < 10; i++)

    {

        for (int
j = 0; j < 10; j++)

        {

            if (g_PathLattice[i][j] == 0)

            {

                printf("^ ");

            }

            else
if (g_PathLattice[i][j] == 1)

            {

                printf("* ");

            }

            else
if (g_PathLattice[i][j] == 2)

            {

                printf("# ");

            }

        }

        printf("\n");

    }

 

    ClearList(listStart);

    ClearList(listEnd);

 

    delete
nodeEnd;

 

    return 0;

}

 

int
Distance(int
row1, int
rank1, int
row2, int
rank2)

{

    // 格子的中点坐标

    int
x1 = rank1 * LatticeLen + LatticeLen / 2;

    int
y1 = row1 * LatticeLen + LatticeLen / 2;

    int
x2 = rank2 * LatticeLen + LatticeLen / 2;

    int
y2 = row2 * LatticeLen + LatticeLen / 2;

 

    return (int)sqrt((double)((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)));

}

 

bool
IsNodeInList(Node * pNode, list<Node *> NodeList)

{

    for (list<Node *>::iterator
it = NodeList.begin();

        it
!=
NodeList.end(); it++)

    {

        if (pNode->row == (*it)->row && pNode->rank == (*it)->rank)

        {

            return
true;

        }

    }

 

    return
false;

}

 

Node * GetNearestNode(list<Node *> NodeList, Node * Rec)

{

    int
tempF = 1000000;

    for (list<Node *>::iterator
it = NodeList.begin();

        it
!=
NodeList.end(); it++)

    {

        if ((*it)->f < tempF)

        {

            Rec = *it;

            tempF = (*it)->f;

        }

    }

 

    return
Rec;

}

 

void
GetNearNodeList(Node * pNode, list<Node *> & listNear,

    list<Node *> listStart, list<Node *> listEnd, Node * pEndNode)

{

    // 将结点旁边的8个点加入到listNear中

    // 在启动或关闭列表中的点不能加入listNear

    // 阻挡点不能加入listNear

    for (int
i = -1; i <= 1; i++)

    {

        for (int
j = -1; j <= 1; j++)

        {

            if (i == 0 && j == 0)

            {

                // 自己格子

                continue;

            }

 

            int
rowTemp = pNode->row + i;

            int
rankTemp = pNode->rank + j;

 

            if (rowTemp < 0 || rankTemp < 0 || rowTemp > 9 || rankTemp > 9)

            {

                // 越界

                continue;

            }

 

            if (g_PathLattice[rowTemp][rankTemp] == 1)

            {

                // 阻挡点

                continue;

            }

 

            Node
node;

            node.row = rowTemp;

            node.rank = rankTemp;

            if (IsNodeInList(&node, listStart))

            {

                // 在启动列表中

                continue;

            }

            if (IsNodeInList(&node, listEnd))

            {

                // 在关闭列表中

                continue;

            }

 

            Node * pNearNode = new
Node;

            pNearNode->g = pNode->g + Distance(pNode->row, pNode->rank, rowTemp, rankTemp);

            pNearNode->h = Distance(rowTemp, rankTemp, pEndNode->row, pEndNode->rank);

            pNearNode->f = pNearNode->g + pNearNode->h;

            pNearNode->row = rowTemp;

            pNearNode->rank = rankTemp;

            pNearNode->pParent = pNode;

            listNear.push_back(pNearNode);

        }

    }

}

 

void
EraseFromList(Node * pNode, list<Node *> & listStart)

{

    for (list<Node *>::iterator
it = listStart.begin();

        it
!=
listStart.end(); it++)

    {

        if (pNode->row == (*it)->row && pNode->rank == (*it)->rank)

        {

            listStart.erase(it);

            return;

        }

    }

}

 

void
ClearList(list<Node *> nodeList)

{

    for (list<Node *>::iterator
it = nodeList.begin();

        it
!=
nodeList.end(); it++)

    {

        delete
*it;

    }

}