Merge heuristic path cost functions into one

This functionality also possibly opens up easy extension of new similar costs into the same functionality without copying and pasting code.