A* algorithm. Finds the shortest path in graph from start vertex to goal vertex. dist_fun is a function that takes 2 vertex indices and returns the distance between them. h is a "heuristic", a function h(n) that estimates the cost to reach goal vertex from node n.