A star (A*) implementation, a litle faster than dijkstra