A星 最短路径搜索算法

#include <iostream>
#include <vector>
#include <set>
#include <unordered_map>
#include <queue>
#include <tuple>
#include <cmath>

/*
 A  B      ·D
 E  F   G   H
 I  ·   K   L
 */

typedef int VertexID;
typedef std::tuple<double, double> Position;

struct VertexEdges
{
    Position pos;
    std::set<VertexID> edges;
};

std::unordered_map<VertexID, VertexEdges> mapEdges = {
    { 0 , {Position{0,0}, std::set<VertexID>{1,4}}}, // A
    { 1 , {Position{0,1}, std::set<VertexID>{0,5}}}, // B
    { 2 , {Position{0,2}, std::set<VertexID>{}}}, // C
    { 3 , {Position{0,3}, std::set<VertexID>{7}}}, // D
    { 4 , {Position{1,0}, std::set<VertexID>{0, 5, 8}}}, // E
    { 5 , {Position{1,1}, std::set<VertexID>{1, 4, 6}}}, // F
    { 6 , {Position{1,2}, std::set<VertexID>{5, 7, 10}}}, // G
    { 7 , {Position{1,3}, std::set<VertexID>{3, 6, 11}}}, // H
    { 8 , {Position{2,0}, std::set<VertexID>{4}}}, // I
    { 9 , {Position{2,1}, std::set<VertexID>{}}}, // J
    { 10, {Position{2,2}, std::set<VertexID>{6, 11}}}, // K
    { 11, {Position{2,3}, std::set<VertexID>{7, 10}}}, // L
};

struct PathNode
{
    VertexID pos;
    VertexID from;
    double sourceDistance = std::numeric_limits<double>::max();
    double estimateDistance = std::numeric_limits<double>::max();

    bool operator < (const PathNode& other) const noexcept
    {
        if (estimateDistance != other.estimateDistance) {
            return estimateDistance < other.estimateDistance;
        }
        else {
            return pos < other.pos;
        }
    }
};

double distance(const Position& pos1, const Position& pos2)
{
    auto& [x1, y1] = pos1;
    auto& [x2, y2] = pos2;
    double distance = std::sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
    return distance;
}


bool aStar(std::vector<PathNode>& pathReuslt,
    const std::unordered_map<VertexID, VertexEdges>& mapEdges,
    VertexID source,
    VertexID dest)
{
    const VertexEdges& destPos = mapEdges.at(dest);
    PathNode sourceNode = { source, source, 0, 0 };

    std::unordered_map<VertexID, PathNode> searchedNode;
    searchedNode.reserve(mapEdges.size());
    searchedNode[sourceNode.pos] = sourceNode;

    std::priority_queue<PathNode> openNodes;
    openNodes.push(sourceNode);

    while (openNodes.size() > 0) {
        PathNode currentNode = openNodes.top();
        openNodes.pop();

        const VertexEdges& currentPos = mapEdges.at(currentNode.pos);
        for (auto& neighbor : currentPos.edges) {
            const VertexEdges& neighborPos = mapEdges.at(neighbor);
            double edgeLength = distance(currentPos.pos, neighborPos.pos);
            double neighborDistance = currentNode.sourceDistance + edgeLength;
            auto& neighborNode = searchedNode[neighbor];

            if (neighborDistance < neighborNode.sourceDistance) {
                PathNode newDistanceNode;
                newDistanceNode.sourceDistance = neighborDistance;
                newDistanceNode.estimateDistance = neighborDistance + distance(currentPos.pos, destPos.pos);
                newDistanceNode.from = currentNode.pos;
                newDistanceNode.pos = neighbor;
                neighborNode = newDistanceNode;
                openNodes.push(newDistanceNode);

                if (neighbor == dest) {
                    pathReuslt.clear();
                    pathReuslt.push_back(searchedNode[dest]);
                    while (dest != source) {
                        dest = searchedNode[dest].from;
                        pathReuslt.push_back(searchedNode[dest]);
                    }
                    std::reverse(pathReuslt.begin(), pathReuslt.end());
                    return true;
                }
            }
        }
    }
    return false;
}

int main(int argc, const char* argv[]) {
    std::vector<PathNode> path;
    bool success = aStar(path, mapEdges, 0, 3);
    for (auto& node : path) {
        std::cout << node.pos << "\t";
    }
}

output：

0 1 5 6 7 3