Звездный алгоритм. Найдите путь, но не самый короткий

По сути, алгоритм возвращает правильный путь между двумя точками, но не является самым коротким. Может кто-нибудь помочь мне найти ошибку. Я уже трачу на это много времени без какого-либо результата … Заранее спасибо

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

Буду очень признателен за любой комментарий / предложение