A* Path Finding in C++