Added pathfinding utility function based on the A* algorithm.