Astar算法不起作用（可能是错误的启发式值？）

Question

我试图遵循Wiki的A * star伪代码，但是在创建启发式值方法时遇到了问题，因此我仅将其用作最短到达率，但是代码不断将起始节点添加到closedSet中。

public uint TotalNodes = 0;
        uint[][] Neighbors = null;
        uint[][] Weights = null;

        public Graph(uint v)
        {
            TotalNodes = v + 1;
            Neighbors = new uint[TotalNodes][];
            Weights = new uint[TotalNodes][];

            for (int i = 1; i < TotalNodes; i++)
            {
                for (int j = 1; j < TotalNodes; j++)
                {
                    Neighbors[i] = new uint[TotalNodes];
                    Weights[i] = new uint[TotalNodes];
                }
            }
        }

        public void AddNeighbor(uint parent, uint neighbor, uint distance)
        {
            Neighbors[parent][neighbor] = neighbor;
            Weights[parent][neighbor] = distance;
        }

        public void RunAstarAlgorithm(uint start, uint goal)
        {
            uint[] closedSet = new uint[TotalNodes];
            uint[] openSet = new uint[TotalNodes];
            uint[] g_score = new uint[TotalNodes];
            uint[] f_score = new uint[TotalNodes];
            uint openSetNodes = 0, closedSetNodes = 0;
            uint currentNode = 0;
            uint tentativePathCost = 0;

            openSet[0] = start;
            for (uint i = 0; i < TotalNodes; i++)
            {
                if (i == start) f_score[i] = 0;
                    else f_score[i] = uint.MaxValue;
            }

            f_score[start] = g_score[start] + (uint)HeuristicCostEstimate(start,goal);

            while (openSet.Length != 0)
            {
                for (uint i = 1; i < TotalNodes; i++)
                {
                    if (f_score[i] == f_score.Min()) currentNode = i;
                }

                if (currentNode == goal)
                {
                    ReconstructPath(closedSet, currentNode);
                    return;
                }

                for (uint i = 0; i < TotalNodes; i++) if (openSet[i] == currentNode) openSet[i] = 0;
                closedSet[closedSetNodes] = currentNode;
                closedSetNodes++;


                foreach (uint neighbor in Neighbors[currentNode])
                {
                    for (uint i = 0; i < TotalNodes; i++)
                    {
                        if (closedSet.Contains(neighbor)) continue;
                    }
                    tentativePathCost += g_score[currentNode] + Weights[currentNode][neighbor];

                    if (!openSet.Contains(neighbor) || tentativePathCost < g_score[neighbor])
                    {
                        g_score[neighbor] = tentativePathCost;
                        f_score[neighbor] = g_score[neighbor] + (uint)HeuristicCostEstimate(neighbor,goal);
                        if (!openSet.Contains(neighbor))
                        {
                            openSet[openSetNodes] = neighbor;
                            openSetNodes++;
                        }
                    }
                }
            }
        }

查找启发式值：

public int HeuristicCostEstimate(uint start, uint goal)
        {
            Queue<uint> queue = new Queue<uint>();
            queue.Enqueue(start);

            bool[] visitedNodes = new bool[TotalNodes];
            int[] distances = new int[TotalNodes];
            for (uint i = 0; i < TotalNodes; i++) distances[i] = -1;
            distances[start] = 0;

            while(queue.Count > 0)
            {
                uint node = queue.Dequeue();           
                if (node == goal) break;

                foreach(uint neighbor in Neighbors[node])
                {
                    if (neighbor == 0) continue;
                    if(distances[neighbor] == -1)
                    {
                        distances[neighbor] = distances[node] + 1;
                        queue.Enqueue(neighbor);
                    }
                }
            }
            return distances[goal];
        }

所以我有这张图： 7 node graph 和前。我想从1转到5，所以路径应该是1-2-3-6-5对吗？

我的图形以锯齿状阵列表示（忽略第一个（0）点，出于打印目的，我从1开始）

Neighbors数组（第一行表示节点，列表示其neigbhors：

   (0)(1)(2)(3)(4)(5)(6)(7)
(0) 0  0  0  0  0  0  0  0
(1) 0  0  1  0  0  0  0  1
(2) 0  2  0  2  2  0  0  0
(3) 0  0  3  0  3  0  3  0
(4) 0  0  4  4  0  4  0  0
(5) 0  0  0  0  5  0  5  0
(6) 0  0  0  6  0  6  0  6
(7) 0  7  0  0  0  0  7  0 

和权重（距离）数组（列代表它们的近邻，到它们的距离是多少）：

   (0)(1)(2)(3)(4)(5)(6)(7)
(0) 0  0  0  0  0  0  0  0
(1) 0  0  2  0  0  0  0  5
(2) 0  2  0  3  7  0  0  0
(3) 0  0  3  0  4  0  3  0
(4) 0  0  7  4  0  6  0  0
(5) 0  0  0  0  5  0  1  0
(6) 0  0  0  2  0  1  0 10
(7) 0  5  0  0  0  0 10  0 

那么我如何使用A *算法实现最短路径？