Estoy ejecutando primero la búsqueda en el gráfico anterior para encontrar la ruta más corta desdeNode 0 aNode 6.

Mi código

public List<Integer> shortestPathBFS(int startNode, int nodeToBeFound){
        boolean shortestPathFound = false;
        Queue<Integer> queue = new LinkedList<Integer>();
        Set<Integer> visitedNodes = new HashSet<Integer>();
        List<Integer> shortestPath = new ArrayList<Integer>();
        queue.add(startNode);
        shortestPath.add(startNode);

        while (!queue.isEmpty()) {
            int nextNode = queue.peek();
            shortestPathFound = (nextNode == nodeToBeFound) ? true : false;
            if(shortestPathFound)break;
            visitedNodes.add(nextNode);
            System.out.println(queue);
            Integer unvisitedNode = this.getUnvisitedNode(nextNode, visitedNodes);

            if (unvisitedNode != null) {
                    queue.add(unvisitedNode);
                    visitedNodes.add(unvisitedNode);
                    shortestPath.add(nextNode); //Adding the previous node of the visited node 
                    shortestPathFound = (unvisitedNode == nodeToBeFound) ? true : false;
                    if(shortestPathFound)break;
                } else {
                    queue.poll();
                }
        }
        return shortestPath;
    }

Necesito rastrear los nodos a través de los cuales el BFS algo. atravesado para llegar al nodo 6, como[0,3,2,5,6]. Para eso he creado una lista llamadashortestPath & tratando de almacenar los nodos anteriores de los nodos visitados, para obtener la lista de nodos.Referido

Pero no parece funcionar. El camino más corto es[0,3,2,5,6]

En la lista lo que obtengo esShortest path: [0, 0, 0, 0, 1, 3, 3, 2, 5]

Es parcialmente correcto pero da el extra1 .

Si vuelvo a empezar desde el primer elemento0 delshortestPath lista y comienza a recorrer y retroceder. Me gusta1 no tiene una ventaja para3, así que retrocedo y me muevo de0 a3 a5, Obtendré la respuesta pero no estoy seguro si esa es la forma correcta.

¿Cuál es la forma ideal de obtener los nodos para el camino más corto?