A*算法 c++实现

最近舍友突然说起a*算法 虽然之前看过 但是我发现自己记得不是很清楚了 而且从来没去手动实现过 趁着这次就实现一下加深理解 去网上查了下原理 看了几篇别人的实现 然后按自己理解综合一下写出来记录一下（我参考那篇程序运行有好几个问题，而且运行得出路径也是不对的，不知道他有没跑过的。）
Astar.h

#ifndef ASTAR_H
#define ASTAR_H
#include <iostream>
#include <queue>
#include <vector>
#include <stack>
#include<algorithm>
using namespace std;


typedef struct Node
{
    int x,y;
    int g; //起始点到当前点实际代价
    int h;//当前节点到目标节点最佳路径的估计代价
    int f;//估计值
    Node* father;
    Node(int x,int y)
    {
        this->x = x;
        this->y = y ;
        this->g = 0;
        this->h = 0;
        this->f = 0;
        this->father = NULL;
    }
    Node(int x,int y,Node* father)
    {
        this->x = x;
        this->y = y ;
        this->g = 0;
        this->h = 0;
        this->f = 0;
        this->father = father;
    }
}Node;
class Astar{
    public:
        Astar();
        ~Astar();
        void search(Node* startPos,Node* endPos);

        void checkPoit(int x,int y,Node* father,int g);
        void NextStep(Node* currentPoint);
        int isContains(vector<Node*>* Nodelist ,int x,int y);
        void countGHF(Node* sNode,Node* eNode,int g);
        static bool compare(Node* n1,Node* n2);
        bool unWalk(int x,int y);
        void printPath(Node* current);
        void printMap();
        vector<Node*> openList;
        vector<Node*> closeList;
        Node *startPos;
        Node *endPos;
        static const int WeightW = 10;// 正方向消耗
        static const int WeightWH = 14;//打斜方向的消耗
        static const int row = 6;
        static const int col = 8;   
};
#endif

Astar.cpp

#include "stdafx.h"
#include "Astar.h"
int map[101][101] =
{
    {0,0,0,1,0,1,0,0,0},
    {0,0,0,1,0,1,0,0,0},
    {0,0,0,0,0,1,0,0,0},
    {0,0,0,1,0,1,0,1,0},
    {0,0,0,1,0,1,0,1,0},
    {0,0,0,1,0,0,0,1,0},
    {0,0,0,1,0,0,0,1,0}
};
Astar::Astar()
{

}
Astar::~Astar()
{

}

void Astar::search( Node* startPos,Node* endPos )
{
    if (startPos->x < 0 || startPos->x > row || startPos->y < 0 || startPos->y >col ||
        endPos->x < 0 || endPos->x > row || endPos->y < 0 || endPos->y > col)
        return ;
    Node* current;
    this->startPos = startPos;
    this->endPos = endPos;
    openList.push_back(startPos);
    //主要是这块，把开始的节点放入openlist后开始查找旁边的8个节点，如果坐标超长范围或在closelist就return 如果已经存在openlist就对比当前节点到遍历到的那个节点的G值和当前节点到原来父节点的G值 如果原来的G值比较大 不用管 否则重新赋值G值 父节点 和f 如果是新节点 加入到openlist 直到opellist为空或找到终点
    while(openList.size() > 0)
    {
        current = openList[0];
        if (current->x == endPos->x && current->y == endPos->y)
        {
            cout<<"find the path"<<endl;
            printMap();
            printPath(current);
            openList.clear();
            closeList.clear();
            break;
        }
        NextStep(current);
        closeList.push_back(current);
        openList.erase(openList.begin());
        sort(openList.begin(),openList.end(),compare);
    }
}

void Astar::checkPoit( int x,int y,Node* father,int g)
{
    if (x < 0 || x > row || y < 0 || y > col)
        return;
    if (this->unWalk(x,y))
        return;
    if (isContains(&closeList,x,y) != -1)
        return;
    int index;
    if ((index = isContains(&openList,x,y)) != -1)
    {
        Node *point = openList[index];
        if (point->g > father->g + g)
        {
            point->father = father;
            point->g = father->g + g;
            point->f = point->g + point->h;
        }
    }
    else
    {
        Node * point = new Node(x,y,father);
        countGHF(point,endPos,g);
        openList.push_back(point);
    }
}

void Astar::NextStep( Node* current )
{
    checkPoit(current->x - 1,current->y,current,WeightW);//左
    checkPoit(current->x + 1,current->y,current,WeightW);//右
    checkPoit(current->x,current->y + 1,current,WeightW);//上
    checkPoit(current->x,current->y - 1,current,WeightW);//下
    checkPoit(current->x - 1,current->y + 1,current,WeightWH);//左上
    checkPoit(current->x - 1,current->y - 1,current,WeightWH);//左下
    checkPoit(current->x + 1,current->y - 1,current,WeightWH);//右下
    checkPoit(current->x + 1,current->y + 1,current,WeightWH);//右上
}

int Astar::isContains(vector<Node*>* Nodelist, int x,int y )
{
    for (int i = 0;i < Nodelist->size();i++)
    {
        if (Nodelist->at(i)->x  == x && Nodelist->at(i)->y == y)
        {
            return i;
        }
    }
    return -1;
}

void Astar::countGHF( Node* sNode,Node* eNode,int g)
{
    int h = abs(sNode->x - eNode->x) + abs(sNode->y - eNode->y) * WeightW;
    int currentg = sNode->father->g + g;
    int f = currentg + h;
    sNode->f = f;
    sNode->h = h;
    sNode->g = currentg;
}

bool Astar::compare( Node* n1,Node* n2 )
{
    //printf("%d,%d",n1->f,n2->f);
    return n1->f < n2->f;
}

bool Astar::unWalk( int x,int y)
{
    if (map[x][y] == 1)
        return true;
    return false;
}

void Astar::printPath( Node* current )
{
    if (current->father != NULL)
        printPath(current->father);
    printf("(%d,%d)",current->x,current->y);
}

void Astar::printMap()
{
    for(int i=0;i<=row;i++){
        for(int j=0;j<=col;j++){
            printf("%d ",map[i][j]);
        }
        printf("\n");
    }
}

main.h

#include "stdafx.h"

#include "Astar.h"
using namespace std;
int _tmain(int argc, _TCHAR* argv[])
{
    Astar astar;
    Node *startPos = new Node(5,1);
    Node *endPos = new Node(3,8);
    astar.search(startPos,endPos);
    return 0;
}

这个是可以直接运行的 得出的路径我仔细看过了 也是没问题的
编辑下：这个写法大的地图会有效率问题 是因为容器频繁删除导致的大小重新分配导致的 你可以添加一个辅助栈 然后像上面那样容器排序 再按排序顺序从最后一个开始压入栈 最后改为移除栈上第一个元素 不修改容器的大小了