Improved heuristic cost function (was not symmetric).