Бесконечный цикл в алгоритме Дейкстры?

Я пытаюсь реализовать алгоритм Дейкстры, чтобы найти кратчайший путь между двумя пересечениями (вершинами) в графе. К сожалению, я получаю бесконечный цикл в цикле while, и я не могу понять, почему.

NodeDist представляет собой хэш-карту между пересечениями и двойниками и находит расстояние между узлами в графе. Расстояние определяется длиной «улицы» (ребер) в графе. Предыдущий — это хэш-карта, которая отслеживает пересечения с пересечениями, а именно пересечение, на которое смотрели до пересечения, на которое мы смотрим сейчас.

public List<IntersectionI> dijkstraPath(IntersectionI start, IntersectionI end){
    ArrayList<IntersectionI> path = new ArrayList<IntersectionI>();
    Iterator<IntersectionI> it = graph.myGraph.keySet().iterator();
    //Initializing all unvisited node distances as infinity.
    while (it.hasNext()){
        IntersectionI next = it.next();
        nodeDist.put(next, INFINITY);
    }
    //Remove the start node, put in 0 distance. 
    nodeDist.remove(start);
    nodeDist.put(start, (double) 0);
    queue.add(start);
    //computes paths
    while (!queue.isEmpty()){
        IntersectionI head = queue.poll();
        if (nodeDist.get(head) == INFINITY)
            break;
        visited.put(head, true);
        List<StreetI> str = head.getStreetList();
        for (StreetI e : str){
            Point pt1 = e.getFirstPoint();
            Point pt2 = e.getSecondPoint();
            IntersectionI p1 = graph.pointGraph.get(pt1);
            IntersectionI p2 = graph.pointGraph.get(pt2);
            if (head.getLocation().equals(p1)){
                double dist = e.getDistance();
                double addedDist = nodeDist.get(start)+dist;
                double p2Dist = nodeDist.get(p2);
                if (addedDist < p2Dist){
                    previous.put(p2, head);
                    Point p22 = p2.getLocation();
                    p22.setCost(addedDist);
                    nodeDist.put(p2, addedDist);
                    queue.add(p2);
                }

            }
            else {
                double dist = e.getDistance();
                double addedDist = nodeDist.get(start)+dist;
                if (addedDist < nodeDist.get(p1)){
                    previous.put(p1, head);
                    Point p11 = p1.getLocation();
                    p11.setCost(addedDist);
                    nodeDist.put(p1, addedDist);
                    queue.add(p1);
                }
            }
        }
    }
    //gets shortest path
    for (IntersectionI vertex = end; vertex != null; vertex = previous.get(vertex))
        path.add(vertex);
    System.out.println("ya");
    Collections.reverse(path);
    return path;
}

//The comparator that sorts by intersection distance.
public class distCompare implements Comparator<IntersectionI> {
    @Override
    public int compare(IntersectionI x, IntersectionI y) {
        Point xPo = x.getLocation();
        Point yPo = y.getLocation();
        if (xPo.getCost() < yPo.getCost())
            return 1;
        else if (yPo.getCost() < xPo.getCost())
            return -1;
        else return 0;

    }
}