C++ 星型算法。找到一条路，但不是最短的

基本上，该算法返回两点之间的正确路径，但不是最短路径。有人能帮我找到错误吗。我已经花了很多时间在这上面，但没有任何结果。。。先谢谢你

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;
}

AStar:：AStar（float**adj、int和initial、int和end、int num）{
这->调整矩阵=调整；
此->源=初始值；
此->目的地=结束；
这->numoftexts=num；
//closedset-closedList
this->mark=newbool[this->numoftexts]；
//你来自哪里
this->preducer=newint[this->numoftexts]；
//openset-openList
此->距离=新浮动[此->顶点数]；
//得分
此->分数=新浮动[此->数值]；
}
void AStar:：initialize（）{
对于（int i=0；iadjMatrix[node][i]！=0）
调整节点。向后推（i）；
}
返回节点；
}
void AStar:：calculateInstance（）{
初始化（）；
电流；
向量节点；
内部邻居；
浮点数=0；
while（当前！=dest）{
//在openList向量中查找最小值
当前=getClosestUnmarkedNode（）；
//我们到了终点
如果（当前==此->目的地）{
此->打印路径（此->源）；
}
//从openList中删除当前点
距离[电流]=无穷大；
//将当前点添加到closedList
标记[当前]=真；
//获取当前的所有相邻点
adjNodes=getAdjNodes（当前）；
对于（int i=0；iadjMatrix[当前][邻居]；
//coutdistance[邻居]==无穷大）
此->距离[邻居]=暂定；
}
}
}
}
void AStar:：printPath（int节点）{
if（节点==源）{
如果这是A*，你的启发式函数在哪里？…你有一个O（n）搜索以获得最近的节点，而不是使用基于堆数据结构的优先级队列…这会为运行添加多项式时间因子。因此，开始时这是错误的。接下来，您没有huristic，您将该值标记为分数的权重。相反，启发式距离（您对优先级队列排序的依据）应该是（到节点的最小距离）+（到目的地的启发式）。你们这里得到的是一个比*更接近最小生成树的算法。。。