A* 算法

#include <iostream>
#include <cstdio>
#include <vector>
#include <queue>
#include <set>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include <memory>
#include <limits>
#include <string>
#include <algorithm>

using namespace std;

struct Node
{
    int x, y;
    int F, G, H;
    Node* parent;
    Node() {}
    Node(int x, int y) : x(x), y(y), F(0), G(0), H(0),parent(nullptr) {}
    Node(int x, int y, int f, int g, int h) : x(x), y(y), F(f), G(g), H(h), parent(nullptr) {}

    friend bool operator < (const Node& c1, const Node& c2)
    {
        return c1.F > c2.F;
    }

    Node(const Node& rhs)
    {
        this->x = rhs.x;
        this->y = rhs.y;
        this->F = rhs.F;
        this->G = rhs.G;
        this->H = rhs.H;
        this->parent = rhs.parent;
    }
};

vector<vector<int>> mat(100, vector<int>(100));

/*
leftTop   top   rightTop
left      cur    right
leftDown  down  rightDown
*/
int ajacentX[8] = {1, 1, 0, -1, -1, -1, 0, 1};
int ajacentY[8] = {0, 1, 1, 1, 0, -1, -1, -1};

int inOpenlist(const vector<Node>& openlist, const Node& node)
{
    for (auto i = 0; i < openlist.size(); ++i)
        if (openlist[i].x == node.x && openlist[i].y == node.y)
            return i;
    return -1;
}

bool AStar(Node& start, Node& stop, vector<Node>& res)
{
    vector<Node> openlist;
    unordered_map<string, Node*> closedlist;
    closedlist.reserve(1000);

    openlist.push_back(start);

    while (!openlist.empty())
    {
        //least F node int the openlist
        Node *cur = new Node(openlist[0]);
        pop_heap(openlist.begin(), openlist.end());
        openlist.pop_back();

        closedlist[to_string(cur->x)+","+to_string(cur->y)] = cur;
        if (cur->x == stop.x && cur->y == stop.y)
        {
            while (cur)
            {
                res.push_back(*cur);
                cur = cur->parent;
            }
            return true;
        }

        for (int i = 0; i < 8; ++i) {
            Node ajNode(cur->x+ ajacentX[i], cur->y + ajacentY[i]);
            
            string nodeName = to_string(ajNode.x) + "," + to_string(ajNode.y);
            if (mat[ajNode.x][ajNode.y] == 1 || closedlist.find(nodeName) != closedlist.end())
                continue;
            
            ajNode.G = i & 0x01 ? 14 + cur->G : 10 + cur->G;

            //if the ajacent node is already in the openlist
            int index = inOpenlist(openlist, ajNode);
            if (index != -1)
            {
                if (cur->G + ajNode.G < openlist[index].G)
                {
                    openlist[index].G = cur->G + ajNode.G;
                    openlist[index].F = openlist[index].G + openlist[index].H;
                    sort_heap(openlist.begin(), openlist.end());
                }
            }
            //if the ajacent node isn't in the openlist
            else
            {
                ajNode.H = 10 * (abs(stop.y - ajNode.y) + abs(stop.x - ajNode.x));
                ajNode.F = ajNode.G + ajNode.H;
                ajNode.parent = cur;
                openlist.push_back(ajNode);
                push_heap(openlist.begin(), openlist.end());
            }
        }
    }

    return false;
}

int _tmain(int argc, TCHAR *argv[])
{
    int N, Sx, Sy, Tx, Ty;
    cin >> N >> Sx >> Sy>> Tx >> Ty;
    
    for (int i = 1; i <= N; ++i)
        for (int j = 1; j <= N; ++j)
            cin >> mat[i][j];


    Node start(Sx, Sy), stop(Tx, Ty);
    vector<Node> res;
    res.reserve(100);
    bool find;
    find = AStar(start, stop, res);
    if (!find)
    {
        cout << "no path to destnation!" << endl;
    }

    for (auto& coord : res)
    {
        cout << coord.x << "," << coord.y << endl;
    }

    return 0;
}