void TDijkstraAlgorithm::Calculate()
{
    OpenNodes->AddNode(StartNodeId, 0, 0);

    while (!OpenNodes->IsEmpty()) {
        OpenNodes->FindNextBestNode();

        uint64_t nodeId = OpenNodes->GetBestNodeId();
        double pathLengthFromStart = OpenNodes->GetBestNodePathLengthFromStart();

        ++SeenNodesCount;

        if (nodeId == FinishNodeId) {
            Path->PathWasFound(pathLengthFromStart);
            if (NeedFullOutput) {
                Path->ReconstructPath();
            }
            return;
        }

        ClosedNodes->AddNode(nodeId);

        for (TNeighbor neighbor : Graph->GetNode(nodeId).Neighbors) {
            if (ClosedNodes->Contains(neighbor.Id)) {
                continue;
            }

            double pathLengthFromStartThroughNode =
                pathLengthFromStart +
                neighbor.Distance;


            if (!OpenNodes->Contains(neighbor.Id)) {
                OpenNodes->AddNode(neighbor.Id, pathLengthFromStartThroughNode, 0);
                Path->SetNodeParent(neighbor.Id, nodeId);
            } else {
                if (pathLengthFromStartThroughNode < OpenNodes->GetPathLengthFromStart(neighbor.Id)) {
                    OpenNodes->AddNode(neighbor.Id, pathLengthFromStartThroughNode,
                                       HeuristicCostEstimate(
                                           Graph->GetNode(neighbor.Id),
                                           Graph->GetNode(FinishNodeId)));
                    Path->SetNodeParent(neighbor.Id, nodeId);
                }
            }

        }



    }

}
示例#2
0
文件: path2.cpp 项目: plol/jps
int FindPath( const int nStartX, const int nStartY,
        const int nTargetX, const int nTargetY,
        const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
        int* pOutBuffer, const int nOutBufferSize ) {

    static_assert(sizeof(int)*2 == sizeof(long long), "");


    std::set<Position> open_set, closed_set;

    std::map<Position, Position> came_from;

    std::map<Position, int> g_score;
    std::map<Position, int> f_score;
    //std::unordered_set<Position, position_hash> open_set, closed_set;

    //std::unordered_map<Position, Position, position_hash> came_from;

    //std::unordered_map<Position, int, position_hash> g_score;
    //std::unordered_map<Position, int, position_hash> f_score;


    Position start(nStartX, nStartY);
    Position target(nTargetX, nTargetY);

    open_set.insert(start);
    
    g_score[start] = 0;
    f_score[start] = HeuristicCostEstimate(start, target);


    while (!open_set.empty()) {

        Position current = *open_set.begin();

        for (Position alt : open_set) {
            if (f_score[alt] < f_score[current]) {
                current = alt;
            }
        }

        if (current == target) {
            int i = g_score[current]+0;
            int ret = i;
            if (i >= nOutBufferSize) {
                return -2;
            }

            while (i > 0) {
                i -= 1;
                pOutBuffer[i] = current.getIndex(nMapWidth);
                current = came_from[current];
            }
            return ret;
        }


        open_set.erase(current);

        closed_set.insert(current);

        Position neighbors[4] = {
            current.leftOf(),
            current.rightOf(),
            current.above(),
            current.below()
        };

        for (int i = 0; i < 4; i += 1) {
            Position neighbor = neighbors[i];

            if (!WithinBounds(neighbor, nMapWidth, nMapHeight) || pMap[neighbor.getIndex(nMapWidth)] == 0 ) {
                continue;
            }

            int new_g_score = g_score[current] + 1;
            int new_f_score = new_g_score + HeuristicCostEstimate(neighbor, target);

            if (closed_set.find(neighbor) != closed_set.end() && new_f_score >= f_score[neighbor]) {
                continue;
            }

            if (open_set.find(neighbor) == open_set.end() || new_f_score < f_score[neighbor]) {
                came_from[neighbor] = current;

                g_score[neighbor] = new_g_score;
                f_score[neighbor] = new_f_score;

                if (open_set.find(neighbor) == open_set.end()) {
                    open_set.insert(neighbor);
                }
            }
        }
    }
    return -1;
}