Schau dir den Dijkstra- bzw. A*-Algorithmus an. Damit wird das gemacht.