Issue on line 414, looks like pq is empty, rather than holding the list of nodes like in 2D.