Added the AStar Algorithm to the graph algorithms.
Didn't find good heuristic yet.