Алгоритм A-star. Получить оптимальный путь

В течение нескольких дней я пытаюсь узнать что-то новое о поиске пути в графах.
Сейчас я изучаю алгоритм A-star.

Я написал свой код на основе Википедия, он работает нормально, но есть одна проблема, которую я пытался решить в течение пары часов:

Реализация из Википедии позволяет «реконструировать путь» — она ​​покажет все пути, которые использовались для поиска пути.

Есть ли возможность показать только путь, который соединяет начальную и конечную точки без указания промежуточных шагов (когда алгоритм идет не так, как надо)?

ОБНОВЛЕНИЕ 2
Это узел:

class aNode
{
public:
QHash<quint64,Node>::iterator it;
quint64 comeFrom;
double f;
double g;
double h;
}

Поэтому я изменил типы в классе node с int на double (так как я использую расстояние между двумя узлами, имеющими реальные географические координаты) и изменил мой алгоритм.

Вот:

bool Graph::findPath(Node* node_from, Node* node_to)
{
QMap<double,aNode> open; // key - cost function f
QMap<quint64,aNode> closed; // key - Node id

aNode sNode;
sNode.it = nodes.find(node_from->id);
sNode.g = 0;
sNode.h = distance(node_from,node_to);
sNode.f = sNode.g + sNode.h;
sNode.comeFrom = 0;

open.insert(sNode.f, sNode);

while (!open.isEmpty())
{
aNode x = open.begin().value();
if (x.it->id == node_to->id)
{
int comeFrom = x.comeFrom;
while (comeFrom != 0)
{
route.push_back(comeFrom);
comeFrom = closed.find(comeFrom)->comeFrom;
}
return true;
}
open.remove(x.f);
closed.insert(x.it->id,x);
for (int i = 0 ; i < x.it->adj.size() ; i++)
{
if (!edges.contains(QPair<quint64,quint64>(x.it->id,x.it->adj[i])))
continue;
if (closed.contains(x.it->adj[i]))
continue;
aNode y;
y.it = nodes.find(x.it->adj[i]);
y.g = x.g + edges.find(QPair<quint64,quint64>(x.it->id,x.it->adj[i]))->cost;
y.h = distance(&(*y.it),node_to);
y.f = y.g + y.h;
y.comeFrom = x.it->id;
if (open.key(y,-2) == -2)
open.insert(y.f,y);
else
if (y.g < open.find(open.key(y))->g )
open.insert(y.f, y);
}
}
return false;
}

Но это все же рисование промежуточных шагов. Мне кажется, что кузнец не так с «ComeFrom» подал