Corrected implementation of pathfinding with A*.