[Pathfinding] Moved calculation of heuristic cost to function.