A *花费太多时间来达到目标

Question

我实现了这个A *算法，这应该可行。但是当我运行这个时，我会得到50秒的计算时间，或者根本没有达到目标。

我的f（x）=距离（g（x）+启发式（欧几里德距离）

template<class T>
void Graph<T>::updateFlood(Vertex<T>* vertex, T goal) {
    if((vertex->closed != NULL) && (!(vertex->closed)))
        return;
    for(int i = 0; i < vertex->adj.size(); i++) {
        vertex->adj[i].getDest()->setDistance(vertex->adj[i].getDest()->getDistance()  + vertex->adj[i].getLength());
        vertex->adj[i].getDest()->fx = vertex->adj[i].getDest()->getDistance() + heuristic_aStar(vertex, vertexSet[addVertex(goal)],1);
        updateFlood(vertex->adj[i].getDest(), goal);
    }
}

template <class T>
void Graph<T>::aStar(const T &start, const T &goal, bool dist){ 
    for(unsigned i = 0; i < vertexSet.size(); i++) {
        vertexSet[i]->path = NULL;
        vertexSet[i]->distance = INF;
        vertexSet[i]->fx = INF;
        vertexSet[i]->closed = NULL;
        vertexSet[i]->inQueue = false;
    }
    Vertex<T> *s = vertexSet[addVertex(start)];
    s->setDistance(0);
    s->closed = false;
    s->fx = s->getDistance() + heuristic_aStar(s, vertexSet[addVertex(goal)], dist);
    vector<Vertex<T>*> pq;      //nextVertices
    pq.push_back(s);
    s->inQueue = true;
    make_heap(pq.begin(), pq.end(), vertex_greater_than_fx<T>());
    while (!pq.empty()) {
        Vertex<T> *current = pq.front();
        pop_heap(pq.begin(), pq.end());
        pq.pop_back();
        current->inQueue = false; //extra(?)
        if (current == vertexSet[addVertex(goal)]){
            cout << "chegou " << endl;
            //current->path = vertexSet[addVertex(goal)];
            break;
        }
        current->closed = true;
        for(unsigned i = 0; i < current->adj.size(); i++) {
            Edge<T> edge = current->adj[i];
            Vertex<T> *neighbour = edge.dest;
            double weight = dist ? edge.length : neighbour->time;
            if(neighbour->closed == NULL) {
                cout << "Not closed or open" << endl;
                neighbour->closed = false; //closed list
                neighbour->distance = current->distance + weight; 
                neighbour->path = current;
                neighbour->fx = neighbour->distance + heuristic_aStar(neighbour, vertexSet[addVertex(goal)], dist);
                pq.push_back(vertexSet[addVertex(neighbour->getIntersection())]);
            }
            if (!neighbour->closed) {
                cout << "open list" << endl;//open list
                if (neighbour->distance > current->distance + weight) {
                    neighbour->distance = current->distance + weight;
                    neighbour->path = current;
                    neighbour->fx = neighbour->distance + heuristic_aStar(neighbour, vertexSet[addVertex(goal)], dist);
                }
            }
            if(neighbour->closed) {
                cout << "closed node" << endl;
                if(neighbour->distance > current->distance + weight) {
                    cout << "ein" << endl;
                    neighbour->path = current;
                    neighbour->distance = current->distance + weight;
                    neighbour->fx = neighbour->distance + heuristic_aStar(neighbour, vertexSet[addVertex(goal)], dist);
                    //updateFlood(neighbour, goal);
                }
            }
            cout << "heap" << endl;
            make_heap(pq.begin(), pq.end(), vertex_greater_than_fx<T>());
        }
    }
    return;
}