Graph search algorithm to solve shortest path problems