I am using BFS to get the path for my car with the following code. But it seems that the program cannot add any node in the path... could anyone point out my problem, please?

private void pathFinding_BFS(Node start, Node end){ LinkedList<Node> queue = new LinkedList<Node>(); queue.add(start); start.setMarked(); while (!queue.isEmpty()){ Node temp = queue.poll(); if (temp.equals(end)){ temp.setPrevNodeInPath(temp); return; } for (Node n : temp.getAdjacentNode()){ if (!n.isMarked()){ n.setMarked(); n.setPrevNodeInPath(temp); queue.add(n); } } } } public List<Point> getShortestPath(Point start, Point end){ Node src = new Node(start); Node destination = new Node(end); pathFinding_BFS(src, destination); List<Point> path = new ArrayList<Point>(); while (destination.getPrevNodeInPath(destination) != null){ destination = destination.getPrevNodeInPath(destination); path.add(destination.getPointInPath(destination)); } Collections.reverse(path); return path; } public class Node{ private Point loc; private Node prevNodeInPath; private boolean marked = false; public Node(Point p){ loc = p; } public void setPrevNodeInPath(Node n){ prevNodeInPath = n; } public Node getPrevNodeInPath(Node n){ return prevNodeInPath; } public Point getPointInPath(Node n){ return loc; } public void setMarked(){ this.marked = true; } public boolean isMarked(){ return marked; } public List<Node> getAdjacentNode(){ List<Node> adj = new ArrayList<Node>(); Point up = new Point(loc.x, loc.y+1); Point down = new Point(loc.x, loc.y-1); Point left = new Point(loc.x-1, loc.y); Point right = new Point(loc.x+1, loc.y); if (ground.contains(up)) adj.add(new Node(up)); if (ground.contains(down)) adj.add(new Node(down)); if (ground.contains(left)) adj.add(new Node(left)); if (ground.contains(right)) adj.add(new Node(right)); return adj; } }

Thank you!!