Настройване на 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>();
}