Reduce execution time on this shortest path algorithm in c++?

Question

I\'m currently trying to reduce the execution time on this algorithm I made (in the \'\'main loop\'\' part). I\'m figuring the only thing I have missing is a way to exit the