Update Astar.cpp