Настройка поиска A*

Я пишу небольшой раздел программы, в котором я должен написать алгоритм поиска пути. Функция принимает так называемые «маршруты», каждый из которых определяет начальную и конечную точки в 2D-пространстве. Алгоритм требуется, чтобы найти кратчайший и наиболее эффективный (в определенной степени) путь (от источника), чтобы пройти по этим маршрутам, сводя к минимуму общее пройденное расстояние.

Я провел небольшое исследование и пошел по пути, который, как я думал, может сработать. До сих пор я преобразовал маршруты в ориентированный граф, который связан, как если бы это была идеализированная дорожная карта. Затем я попытался выполнить поиск A* на этом графике. Эвристика, которую я использовал, вычисляет общее расстояние «маршрутов», оставшихся для прохождения, а значение расстояния от начала (G), которое я использовал, было просто совокупным расстоянием, пройденным до текущей точки. Это работает для некоторых входных данных, но другие вообще не возвращают путь, и я не могу понять, почему.

Возможно ли, что моя эвристика неверна, и это где-то вызывает просчет, или более вероятно, что сама процедура A * неверна? или я просто на совершенно неправильном пути здесь?

Я приведу функцию getPath ниже (написанную на Java) на всякий случай, если это поможет.

Заранее спасибо.

public ArrayList<Vector2> getPath()
{
    PriorityQueue<SearchNode> openList = new PriorityQueue<SearchNode>(10, new SearchNodeComparator());
    ArrayList<SearchNode> closedList = new ArrayList<SearchNode>();

    map.startJobs();
    searchDepth = 0;

    SearchNode start = searchableGraph.getNode(new Vector2(0, 0));
    int goalsLeft = map.getJobCount();

    start.setDistanceTraveled(0);

    openList.add(start);

    while (openList.size() > 0)
    {
        SearchNode current = openList.peek();
        searchDepth++;

        if (map.isJobEndPoint(current.getValue()))
        {
            map.completeJob(current.getValue());
            goalsLeft--;

        }

        if (reachedGoalState(current, searchableGraph.getNodes().size()))
        {
            return getFinalPath(current);
        }
        else
        {
            ArrayList<SearchNode> neighbours = getNeighbours(current);

            for (int i = 0; i < neighbours.size(); i++)
            {
                SearchNode node = neighbours.get(i);        
                System.out.print("Inspecting node" + node.getValue().toString());

                float distanceTraveled = current.getDistanceTraveled() + getDistance(current.getValue(), node.getValue());

                float heuristic = heuristic(node);

                if (!openList.contains(node) && !closedList.contains(node))
                {

                    node.setDistanceTraveled(distanceTraveled);

                    node.setDistanceToGoal(distanceTraveled + heuristic);

                    node.setParent(current);

                    openList.add(node);
                }
                else if(openList.contains(node))
                {
                    if (node.getDistanceTraveled() <= distanceTraveled)
                    {

                        node.setDistanceToGoal(distanceTraveled + heuristic);


                        node.setParent(current);
                    }

                }
            }

            openList.remove(current);
            closedList.add(current);
        }
    }

    return new ArrayList<Vector2>();
}