Simple Astar implementation In Xna