Path finding algorithm Astar in C++