Xna C# Pathfinding (A* Algorithm)