Mira en google el algoritmo de dijkstra.
Basicamente hace lo q necesitas lo unico q tendras q implementarlo, yo lo hice para la carrera y no es muy complejo, lo he buscao x si te servia pero no se donde parara, espero haberte ayudado.
Salu2