A*算法和A*跳步算法的CPP实现

 - by Hector

A*算法本身很简单,很早之前就了解过,用c++简单实现了一下,并又跳步算法进行了优化。

A*算法

A*算法不同于深度和广度优先算法,是一种启发式搜索算法
其主要逻辑是不断估算当前点到目的节点的大概距离,选取最优的路径不断前进,直至到达终点。过程如下:
1. 将当前点放如一个未处理的节点列表,叫做openList,另外有个已处理的节点列表叫做closeList.
2. 从openList中找到最优节点n(可用堆算法|优先队列),针对其每一个子节点x:

  • a. 如果x在open中,如果x比open中节点的更好,则替换open中的节点,否则跳过x
  • b. 如果x在close中,如果x比close中的节点更好,则要删掉close中的节点,并将x放到open中,代表重新处理。否则跳过x.
  • c. 如果x不在open和close中,则放入open中,代表需要处理。

3. 重复2,直至到目标节点。到了目标节点后,从目标节点一步步后退,即可找到来的路径。

估价函数 f'(n) = g'(n) + h'(n)
f'(n)是估价函数,g'(n)是起点到终点的最短路径值,h'(n)是n到目标的最短路经的启发值。

跳步优化

跳步优化不同于常规的A*算法寻找当前节点所有的子节点,而是在于“跳过”一些明显不必经过的路,
抽象地说,就是在搜索时大量减掉明显无用的分支,使得朴素的搜索过程变得高效。
参考:

  • 1. 代码:https://github.com/qiao/PathFinding.js/blob/master/src/finders/JumpPointFinder.js
  • 2. 解说:http://plusplus7.com/lemonbag/jpsstudy

源码放在github: https://github.com/myourys/Algorithm 其中AStar.cpp文件。

/*=============================================================================
#     FileName: AStar.cpp
#         Desc: A星寻路算法
#       Author: Hector
#        Email: myourys@gmail.com
#     HomePage: http://www.yiwuye.com
#      Version: 0.0.1
#   LastChange: 2014-08-21 18:48:40
#      History:
=============================================================================*/
/*
 * A*算法不同于深度和广度优先算法,是一种启发式搜索算法
 * 其主要逻辑是不断估算当前点到目的节点的大概距离,选取最优的路径不断前进,直至到达终点。过程如下:
 * 1. 将当前点放如一个未处理的节点列表,叫做openList,另外有个已处理的节点列表叫做closeList.
 * 2. 从openList中找到最优节点n(可用堆算法|优先队列),针对其每一个子节点x:
 *      a. 如果x在open中,如果x比open中节点的更好,则替换open中的节点,否则跳过x
 *      b. 如果x在close中,如果x比close中的节点更好,则要删掉close中的节点,并将x放到open中,代表重新处理。否则跳过x.
 *      c. 如果x不在open和close中,则放入open中,代表需要处理。
 * 3. 重复2,直至到目标节点。到了目标节点后,从目标节点一步步后退,即可找到来的路径。
 *
 * 估价函数 f'(n) = g'(n) + h'(n)
 * f'(n)是估价函数,g'(n)是起点到终点的最短路径值,h'(n)是n到目标的最短路经的启发值。
 */

/*
 * 跳步优化
 * 跳步优化不同于常规的A*算法寻找当前节点所有的子节点,而是在于“跳过”一些明显不必经过的路,
 * 抽象地说,就是在搜索时大量减掉明显无用的分支,使得朴素的搜索过程变得高效。
 * 参考:
 * 1. 代码:https://github.com/qiao/PathFinding.js/blob/master/src/finders/JumpPointFinder.js
 * 2. 解说:http://plusplus7.com/lemonbag/jpsstudy
 */

#include <iostream>
#include <queue> /* priority_queue不支持遍历,利用vector容器和push_heap(堆算法)模拟一个优先容器 */
#include <vector>
#include <sstream>

#define kUseJumpNode 0

#define STR(A) #A

using namespace std;

/* 地图
 * 第一行 行列数
 * 第二行 起点,终端坐标
 * 地图 0:可通过 1:不可通过 坐标起点为左下角
 * 这里我们设定一个节点到相邻节点消耗为10,如果是对接线则为14.
 */

const char* MapStr = STR(
9 10                \n
6 3    6 6          \n
0 0 0 0 1 0 0 0 0 0 \n
0 0 0 0 1 0 0 0 0 0 \n
0 0 0 0 1 0 0 0 0 0 \n
0 0 1 1 1 0 0 0 0 0 \n
0 0 1 1 0 0 0 0 0 0 \n
0 0 1 0 0 0 0 0 0 0 \n
0 0 1 0 1 0 0 0 0 0 \n
0 0 0 0 1 0 0 0 0 0 \n
0 0 0 0 1 0 0 0 0 0 \n
);

class Point
{
public:
    int x,y;

    Point(int _x,int _y):x(_x),y(_y){};
    Point(){x =-1;y=-1;};
    Point& operator=(const Point & other){this->x = other.x;this->y=other.y;return *this;};
    bool operator==(const Point & other){return this->x == other.x && this->y== other.y;};
};

typedef struct _Node{
    Point pos;//当前点在迷宫中的位置坐标
    int g;//起始点到当前点实际代价
    int h;//当前节点到目标节点最佳路径的估计代价
    int f;//估计函数:f = g + h。
    _Node *father;//指向其父节点的指针
}Node;

//比较函数,用于堆操作
struct NodeCompare
{
    bool operator ()(Node *&n1, Node *&n2) const
    {
        return n1->f > n2->f; //小值优先
    }
};

class AStar{

public:
    AStar():_map(nullptr){};
    ~AStar(){clear();};
    int readMap(const char* str);
    void printMap();
    bool findRoad();
    void printRoad();
    void clear();
private:
    int estimate(const Point& pos); //估计点到目的地的值
    void initNode(Node &node,const Point& pos,int g,Node* father);
    void expandNode(Node* n);
    void getNabors(vector<Point> *nabors,Node* n);
    void getJumpNodes(vector<Point> *jumps,Node* n);
    bool validPoint(const Point& p);
private:
    Point _mapSize;
    Point _startPos;
    Point _endPos;
    int **_map;
    vector<Node*> _closeList;
    vector<Node*> _openList;
};

void AStar::clear()
{
    for(auto iter:_closeList)
        delete iter;
    for(auto iter:_openList)
        delete iter;
    if(_map)
    {
        for(int i =0;i<_mapSize.x;i++)
            delete []_map[i];
        delete []_map;
    }

}

int AStar::estimate(const Point& pos)
{
    return 10* (abs(_endPos.x - pos.x) + abs(_endPos.y-pos.y));
}

int AStar::readMap(const char* str)
{
    istringstream istr(str);

    istr>>_mapSize.x>>_mapSize.y;
    istr>>_startPos.x>>_startPos.y;
    istr>>_endPos.x>>_endPos.y;

    _map = new int *[_mapSize.x];
    for(int i = _mapSize.x-1;i>=0;i--)
    {
        _map[i] = new int[_mapSize.y];
        for(int j = 0;j< _mapSize.y;j++)
        {
            istr>>_map[i][j];
        }
    }
    return 0;
}

void AStar::printMap()
{
    for(int i = _mapSize.x-1;i>=0;i--)
    {
        for(int j = 0;j< _mapSize.y;j++)
        {
            cout<<_map[i][j]<<" ";
        }
        cout<<endl;
    }
}

void AStar::initNode(Node &node,const Point& pos,int g,Node* father)
{
    node.pos= pos;
    node.g = g;
    node.h = estimate(pos);
    node.f = node.g +node.h;
    node.father = father;
}

bool AStar::validPoint(const Point& p)
{
    if(p.x< 0 || p.y<0 || p.x > (_mapSize.x -1) || p.y > (_mapSize.y -1) )
        return false;
    return true;
}

bool AStar::findRoad()
{
    Node *start = new Node();
    initNode(*start,_startPos,0,nullptr);

    _openList.push_back(start);
    push_heap(_openList.begin(),_openList.end(),NodeCompare());
    while(1)
    {
        if(_openList.empty())
        {
            cout<<"can't find the road."<<endl;
        }
        Node *n = _openList.front();

        if(n->pos == _endPos) //到达终点
        {
            cout<<"Find the Road:"<<endl;
            return true;
        }

        //将第一个元素移到最后,并将剩余区间重新排序,组成新的heap
        pop_heap(_openList.begin(),_openList.end(),NodeCompare());
        _openList.pop_back();//删除最后一个元素n
        expandNode(n);
        _closeList.push_back(n);
    }
    return false;
}

void AStar::expandNode(Node* n)
{
    vector<Point> nodeList;
    if(kUseJumpNode)
        getJumpNodes(&nodeList,n);
    else
        getNabors(&nodeList,n);

    for(auto& pos:nodeList)
    {
        if(_map[pos.x][pos.y] == 1) //不可通过的点
            continue;

        int gVal;
        if(pos.x == n->pos.x || pos.y == n->pos.y)
            gVal= n->g +10;
        else
            gVal=n->g +14; //对角线

        vector<Node*>::iterator openResult;
        for( openResult=_openList.begin();openResult!= _openList.end();++openResult)
        {
            if((*openResult)->pos == pos) //在open列表
                break;
        }

        //在open列表,并且open列表中已经是最优的,跳过
        if(openResult!= _openList.end() && (*openResult)->g <= gVal)
            continue;

        //closed列表
        vector<Node*>::iterator closeResult;
        for( closeResult=_closeList.begin();closeResult!= _closeList.end();closeResult++)
        {
            if((*closeResult)->pos == pos) //在close列表
                break;
        }

        //在close列表,并且close列表中已经是最新的了,跳过
        if(closeResult!= _closeList.end() && (*closeResult)->g <= gVal)
            continue;

        //新节点为最优节点
        //如果在close列表
        if(closeResult != _closeList.end())
        {
            _closeList.erase(closeResult);
            delete *closeResult;
        }

        //如果在open列表
        if(openResult!= _openList.end())
        {
            _openList.erase(openResult);
            make_heap(_openList.begin(),_openList.end(),NodeCompare());
            delete *openResult;
        }

        Node *child = new Node();
        initNode(*child,pos,gVal,n);
        _openList.push_back(child);
        push_heap(_openList.begin(),_openList.end(),NodeCompare());
    }
}

void AStar::getNabors(vector<Point> *nabors,Node* n)
{
     for(int x = n->pos.x-1;x<= n->pos.x+1; x++) //针对每一个子节点
    {
        if(x < 0 || x > _mapSize.x -1)
            continue;
        for(int y = n->pos.y-1; y<= n->pos.y+1; y++)
        {

            if(y < 0 || y > _mapSize.y -1)
                continue;
            Point pos(x,y);

            if(pos == n->pos)
                continue;
            nabors->push_back(pos);
        }
    }
}

void AStar::getJumpNodes(vector<Point> *jumps,Node* n)
{
    if(!n->father)
    {
        getNabors(jumps,n);
        return;
    }

    auto fPos = n->father->pos;

    int x = n->pos.x;
    int y = n->pos.y;

    int dx = x - fPos.x;
    int dy = y - fPos.y;

    /*
     * 这里假设 1 0 从左下角不能到右上角
     *          0 1
     */
    if(dx!= 0 && dy!= 0) //父节点和当前节点在对角线上
    {
        //当前节点旁边的两个点,并且远离父节点
        if(validPoint(Point(x,y+dy)) && _map[x][y+dy]==0)
            jumps->push_back(Point(x,y+dy));
        if(validPoint(Point(x+dx,y)) && _map[x+dx][y]==0)
            jumps->push_back(Point(x+dx,y));

        //对角线的另一个点
        if(validPoint(Point(x+dx, y+ dy)) && ( _map[x][y+dy]==0 || _map[x+dx][y]==0))
            jumps->push_back(Point(x+dx,y+dy));

        //靠近父节点的两个角
        if(validPoint(Point(x-dx,y+dy)) && _map[x - dx][y] ==1 && _map[x][y+dy]==0)
            jumps->push_back(Point(x - dx, y + dy));
        if(validPoint(Point(x+dx,y-dy)) && _map[x][y-dy] ==1 && _map[x+dx][y]==0)
            jumps->push_back(Point(x + dx, y - dy));
    }
    else //父节点和当前节点在水平或者垂直线上
    {
        if(dx == 0) //水平线上
        {
            if(validPoint(Point(x, y + dy)) && _map[x][y + dy]==0)
            {
                jumps->push_back(Point(x, y + dy));
                if(validPoint(Point(x+1, y) ) && _map[x+1][y]==1)
                    jumps->push_back(Point(x+dx, y+dy));
                if(validPoint(Point(x-1, y) ) && _map[x-1][y]==1)
                    jumps->push_back(Point(x-1, y+dy));
            }
        }
        else
        {
            if(validPoint(Point(x+dx, y))  && _map[x+dx][y]==0)
            {
                jumps->push_back(Point(x+dx, y));
                if(validPoint(Point(x, y+1) ) && _map[x][y+1]==1)
                    jumps->push_back(Point(x+dx, y+1));
                if(validPoint(Point(x, y-1) ) && _map[x][y-1]==1)
                    jumps->push_back(Point(x+dx, y-1));
            }
        }
    }
}

void AStar::printRoad()
{
    int **tmpMap = new int*[_mapSize.x];
    for(int i=0;i<_mapSize.x;i++)
        tmpMap[i] = new int[_mapSize.y];
    for(int i=0;i<_mapSize.x;i++)
        for(int j=0;j<_mapSize.y;j++)
            tmpMap[i][j]=_map[i][j];
    Node* n= _openList.front();
    while(n)
    {
        tmpMap[n->pos.x][n->pos.y] = -1;
        n = n->father;
    }

    for(int i=_mapSize.x-1;i>=0;i--)
    {
        for(int j=0;j<_mapSize.y;j++)
        {
            if(tmpMap[i][j] == -1)
                cout<<"# ";
            else
                cout<<tmpMap[i][j]<<' ';
        }

        cout<<endl;
    }

    for(int i=0;i<_mapSize.x;i++)
        delete []tmpMap[i];
    delete []tmpMap;
}

int main()
{
    AStar astar;
    astar.readMap(MapStr);
    astar.printMap();
    cout<<endl;
    if(astar.findRoad())
        astar.printRoad();
    return 0;
}

Leave a comment