Change to real A*. Should now produce actually optimal paths.