Update AStar.cpp