Create astar.cpp

This is the pathfinding algorithm.