I’ve not tested these, but some suggestions:
-
Don’t add all vertices to the PQ initially, only add the start location.
-
you can exit the loop once you’ve reached the destination.
-
if you’ve already visited the current node, there’s no need to process it again.