A*寻路算法C++实现

#include 
#include 
#include 
#include 
#include 

struct Point
{
    int x = 0;
    int y = 0;
    bool operator==(const Point &p) const
    {
        return x == p.x && y == p.y;
    }
    Point operator+(const Point &p) const
    {
        return {x + p.x, y + p.y};
    }
};

struct Node
{
    Node *parent = nullptr;
    Point p;
    int g = 0;
    int h = 0;
    int f() const { return g + h; }
};

struct compareLess
{
    bool operator()(Node *a, Node *b)
    {
        return a->f() > b->f();
    }
};

int calcManhattonDist(const Point &p1, const Point &p2)
{
    return abs(p1.x - p2.x) + abs(p1.y - p2.y);
}

std::vector<Node *> aStarSeach(const std::vector<std::vector<int>> &grid, Node *start, Node *goal)
{
    start->g = 0;
    start->h = calcManhattonDist(start->p, goal->p);
    std::priority_queue<Node *, std::vector<Node *>, compareLess> openList;
    std::unordered_map<int, Node *> closedList;
    openList.push(start);

    const int rows = grid.size();
    const int cols = grid.front().size();
    const auto hashKey = [=](const Point &p)
    {
        return p.y * cols + p.x;
    };

    const auto isValid = [&](const Point &p)
    {
        if (p.x < 0 || p.y < 0 || p.x >= cols || p.y >= rows || grid[p.y][p.x] == 1)
            return false;
        return true;
    };

    const std::vector<Point> move = {{-1, 0}, {1, 0}, {0, 1}, {0, -1}};

    while (!openList.empty())
    {
        auto currentNode = openList.top();
        openList.pop();
        closedList[hashKey(currentNode->p)] = currentNode;

        if (currentNode->p == goal->p)
        {
            std::vector<Node *> path;
            while (currentNode)
            {
                path.push_back(currentNode);
                currentNode = currentNode->parent;
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        for (const auto &m : move)
        {
            auto neighbor = currentNode->p + m;

            if (!isValid(neighbor) || closedList.find(hashKey(neighbor)) != closedList.end())
                continue;

            Node *neighborNode = new Node();
            neighborNode->p = neighbor;
            neighborNode->g = currentNode->g + 1;
            neighborNode->h = calcManhattonDist(neighbor, goal->p);
            neighborNode->parent = currentNode;

            bool inOpen = false;
            std::vector<Node *> nodeCopy;
            while (!openList.empty())
            {
                auto temp = openList.top();
                openList.pop();
                if (temp->p == neighbor)
                {
                    inOpen = true;
                    if (temp->g > neighborNode->g)
                    {
                        temp->g = neighborNode->g;
                        temp->h = neighborNode->h;
                        temp->parent = neighborNode->parent;
                    }
                }
                nodeCopy.push_back(temp);
            }
            for (const auto &n : nodeCopy)
                openList.push(n);

            if (!inOpen)
                openList.push(neighborNode);
            else
                delete neighborNode;
        }
    }

    return {};
}

int main()
{
    std::vector<std::vector<int>> grid = {
        {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, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 0, 1, 0, 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}};
    auto start = new Node();
    start->p = {0, 3};
    auto goal = new Node();
    goal->p = {7, 3};
    const auto path = aStarSeach(grid, start, goal);
    std::cout << "path size " << path.size() << std::endl;
    for (const auto n : path)
    {
        std::cout << n->p.x << " " << n->p.y << std::endl;
    }
    return 0;
}