Манхэттенская эвристическая функция для A-звезды (A *)

Я нашел этот алгоритм здесь .

У меня проблема, я не могу понять, как настроить и передать мою эвристическую функцию.

    static public Path AStar(TNode start, TNode destination,
        Func distance,
        Func estimate) where TNode : IHasNeighbours
    {
        var closed = new HashSet();
        var queue = new PriorityQueue>();
        queue.Enqueue(0, new Path(start));
        while (!queue.IsEmpty)
        {
            var path = queue.Dequeue();
            if (closed.Contains(path.LastStep))
                continue;
            if (path.LastStep.Equals(destination))
                return path;
            closed.Add(path.LastStep);
            foreach (TNode n in path.LastStep.Neighbours)
            {
                double d = distance(path.LastStep, n);
                var newPath = path.AddStep(n, d);
                queue.Enqueue(newPath.TotalCost + estimate(n), newPath);
            }
        }
        return null;
    }

Как видите, он принимает 2 функции, расстояние и функцию оценки.

Используя функцию Manhattan Heuristic Distance, мне нужно принять 2 параметра. Нужно ли мне изменять его источник и измените его на принятие 2 параметров TNode, чтобы я мог передать ему оценку Манхэттена? Это означает, что 4-й параметр будет выглядеть так:

Func estimate) where TNode : IHasNeighbours

и измените функцию оценки на:

queue.Enqueue(newPath.TotalCost + estimate(n, path.LastStep), newPath);

Моя функция Манхэттена:

    private float manhattanHeuristic(Vector3 newNode, Vector3 end)
    {
        return (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
    }