Проблемы с MinHeaps

Question

Я пытаюсь разработать собственный путь для игры, которую я создаю. Он использует новые 2D коллайдеры, поэтому я не могу использовать встроенные в navmesh единицы. Поэтому вместо того, чтобы использовать актив, созданный кем-то другим, я подумал, что мне нужно сделать собственную реализацию.

Я искал кучу вещей о поиске пути и A*, которые у меня нет проблем с пониманием. Где у меня проблемы, хотя это MinHeap. Я никогда не изучал сдвиги в битах и ​​тому подобное, поэтому я изменил некоторый код, который нашел в руководстве, в соответствии со своими потребностями.

Проблема, с которой я столкнулся, заключается в том, что, как я могу сказать из своей отладки, неправильно сортируется узлы по стоимости. Из того, что я вижу, и я опубликую рисунок ниже, чтобы вы знали, о чем я говорю, каждый узел, который добавляется в открытый список, остается там, где он добавлен, так что это больше похоже на поиск Дейкстры, чем на A * один.

Может ли кто-нибудь взглянуть на код и сказать мне, если я делаю что-то очень неправильно... потому что это сводит меня с ума.

Это код:

using System;

public class NodeList<T> where T : IComparable<T>
{        
    private int count;
    private int capacity;
    private T temp;
    private T mheap;
    private T[] array;
    private T[] tempArray;

    public int Count
    {
        get { return this.count; }
    }

    public NodeList() : this(16) { }

    public NodeList(int capacity)
    {
        this.count = 0;
        this.capacity = capacity;
        array = new T[capacity];
    }

    public void BuildHead()
    {
        int position;
        for (position = (this.count - 1) >> 1; position >= 0; position--)
        {
            this.MinHeapify(position);
        }
    }

    public void Add(T item)
    {            
        this.count++;
        if (this.count > this.capacity)
        {
            DoubleArray();
        }
        this.array[this.count - 1] = item;
        int position = this.count - 1;

        int parentPosition = ((position - 1) >> 1);

        while (position > 0 && array[parentPosition].CompareTo(array[position]) > 0)    
        {
            temp = this.array[position];
            this.array[position] = this.array[parentPosition];
            this.array[parentPosition] = temp;
            position = parentPosition;
            parentPosition = ((position - 1) >> 1);
        }
    }            

    private void DoubleArray()
    {
        this.capacity <<= 1;
        tempArray = new T[this.capacity];
        CopyArray(this.array, tempArray);
        this.array = tempArray;
    }

    private static void CopyArray(T[] source, T[] destination)
    {
        int index;
        for (index = 0; index < source.Length; index++)
        {
            destination[index] = source[index];
        }
    }

    public T Peek()
    {
        if (this.count == 0)
        {
            throw new InvalidOperationException("Heap is empty");
        }
        return this.array[0];
    }


    public T ExtractFirst()
    {
        if (this.count == 0)
        {
            throw new InvalidOperationException("Heap is empty");
        }
        temp = this.array[0];            
        this.array[0] = this.array[this.count - 1];
        this.count--;
        this.MinHeapify(0);
        return temp;
    }

    private void MinHeapify(int position)
    {
        do
        {
            int left = ((position << 1) + 1);
            int right = left + 1;
            int minPosition;

            if (left < count && array[left].CompareTo(array[position]) < 0)
            {
                minPosition = left;
            }
            else
            {
                minPosition = position;
            }

            if (right < count && array[right].CompareTo(array[minPosition]) < 0)    
            {
                minPosition = right;
            }

            if (minPosition != position)
            {
                mheap = this.array[position];
                this.array[position] = this.array[minPosition];
                this.array[minPosition] = mheap;
                position = minPosition;
            }
            else
            {
                return;
            }

        } while (true);
    }
}

Мой путь ищет от конечной точки до начала (от того, где игрок нажимает на игрока), поэтому "Начальная точка" - это серый узел в середине экрана: проблема с поиском пути

Вот почему это проблема: выпуск 2