一个明星算法。找到一条路径但不是最短路径

Question

基本上，算法在两点之间返回正确的路径，但不是最短的。任何人都可以帮我找到错误。我已经花了很多时间没有任何结果......提前感谢你

AStar::AStar(float** adj, int &initial, int &end, int num){
this->adjMatrix = adj;
this->source = initial;
this->dest = end;
this->numOfVertices = num;

//closedset - closedList
this->mark = new bool[this->numOfVertices];
//came_from
this->predecessor = new int[this->numOfVertices];
//openset - openList
this->distance = new float[this->numOfVertices];
//score
this->score = new float[this->numOfVertices];
}

void AStar::initialize(){
for(int i=0;i<numOfVertices;i++) {
    mark[i] = false;
    predecessor[i] = -1;
    distance[i] = INFINITY;
    score[i] = 0;


 }
    distance[source]= 0;
}

    //Tested
int AStar::getClosestUnmarkedNode(){
    int minDistance = INFINITY;
    int closestUnmarkedNode;
    for(int i=0;i<numOfVertices;i++) {
        if((!mark[i]) && ( minDistance >= distance[i])) {
            minDistance = distance[i];
            closestUnmarkedNode = i;
        }
    }
return closestUnmarkedNode;
}

//Tested
vector<int> AStar::getAdjNodes(int &node){
    vector<int> adjNodes;
    for(int i = 0; i<this->numOfVertices; i++){
        if (this->adjMatrix[node][i] != 0)
            adjNodes.push_back(i);
    }
    return adjNodes;
}

void AStar::calculateDistance(){
initialize();
int current;
vector<int> adjNodes;
int neighbor;
float tentative = 0;
while(current != dest) {
    //Look for the smallest value in openList vector
    current = getClosestUnmarkedNode();
    //Stop of we reached the end
    if (current == this->dest){
        this->printPath(this->source);
    }
    //Remove the current point from the openList
    distance[current] = INFINITY;
    //Add the current point to the closedList
    mark[current] = true;
    //Get all current's adjacent points
    adjNodes = getAdjNodes(current);
    for(int i = 0; i<adjNodes.size(); i++){
        neighbor = adjNodes.at(i);
        if(mark[neighbor]) continue;
        tentative = this->adjMatrix[current][neighbor];
        //cout<<tentative<<endl;
        if((distance[neighbor] == INFINITY) || (tentative < this->score[neighbor])){
            this->predecessor[neighbor] = current;
            this->score[neighbor] = tentative;
            if (this->distance[neighbor] == INFINITY)
                this->distance[neighbor] = tentative;
        }
    }
}
}

void AStar::printPath(int node){
    if(node == source){
        cout<<node<<"..";
    }else if(predecessor[node] == -1){
    cout<<"No path from "<<source<<" to "<<node<<endl;
}else {
    printPath(predecessor[node]);
    cout<<node<<"..";
    this->result.push_back(node);
    }
}

vector<int> AStar::output(){
    this->result.push_back(this->source);

    int i = this->dest;
        if(i == source){
            cout<<source<<".."<<source;
        }else{
            printPath(i);
            cout<<"->"<<distance[i]<<endl;
        }

    return this->result;
}

我非常感谢任何评论/建议