无法弄清楚我的 A* 算法出了什么问题

我目前正在我的 2D 横向卷轴游戏中实现 A* 寻路算法，但我遇到了一些困难。

我

以为我正确地实现了它，但显然不是，因为它不起作用。

我使用优先级队列来提高效率，并遵循此处讨论的算法：

http://www.codeproject.com/Articles/5758/Path-finding-in-C

但是，我没有使用他们的优先级队列，因为许多评论指出它有一些问题。相反，我使用以下优先级队列：

https://bitbucket.org/BlueRaja/high-speed-priority-queue-for-c/src

这是我的函数，然后我将讨论这个问题：

List<Waypoint> FindPathTo(Waypoint Goal)
{
    TileMath goalMath = TileMath.GetTileMath((int)Goal.x, (int)Goal.y);
    Waypoint startPt = new Waypoint((Vector2)this.transform.position);
    HeapPriorityQueue<Waypoint> OpenList = new HeapPriorityQueue<Waypoint>(MAX_NODES);
    HeapPriorityQueue<Waypoint> ClosedList = new HeapPriorityQueue<Waypoint>(MAX_NODES);
    List<Waypoint> Solution = new List<Waypoint>();
    OpenList.Enqueue(startPt, openPriority);
    openPriority--;
    print(Goal.GetPoint()); //testing
    print(OpenList.Count); //testing
    while (OpenList.Count > 0)
    {
        Waypoint CurrNode = OpenList.Dequeue();
        TileMath currMath = TileMath.GetTileMath((int)CurrNode.x, (int)CurrNode.y);
        if (currMath.GetXY() == goalMath.GetXY() && currMath.GetBlockID() == goalMath.GetBlockID()) // checks if the current node is the goal
        {
            while (CurrNode != null)
            {
                Solution.Insert(0, CurrNode);
                CurrNode = CurrNode.prev;
            }
            break;
        }
        List<Waypoint> successors = CurrNode.GetSuccessors();
        print(successors.Count);
        foreach (Waypoint w in successors)
        {
            Waypoint OpenWp = null;
            Waypoint ClosedWp = null;
            if(OpenList.Contains(w))
            {
                IEnumerator<Waypoint> ie = OpenList.GetEnumerator();
                while((int)ie.Current.x != (int)w.x && (int)ie.Current.y != (int)w.y && ie.Current != null)
                {
                    ie.MoveNext();
                }
                if (ie.Current == null)
                    print("IE ERROR. CHECK CONTAINS IN OPENLIST.");
                else
                    OpenWp = ie.Current;

                if (OpenWp != null && w.totalCost > OpenWp.totalCost)
                    continue;

            }
            if(ClosedList.Contains(w))
            {
                IEnumerator<Waypoint> ie = ClosedList.GetEnumerator();
                while ((int)ie.Current.x != (int)w.x && (int)ie.Current.y != (int)w.y && ie.Current != null)
                {
                    ie.MoveNext();
                }
                if (ie.Current == null)
                    print("IE ERROR. CHECK CONTAINS IN CLOSEDLIST.");
                else
                    ClosedWp = ie.Current;

                if (ClosedWp != null && w.totalCost > ClosedWp.totalCost)
                    continue;
            }
            if (OpenWp != null)
                OpenList.Remove(OpenWp);
            if (ClosedWp != null)
                ClosedList.Remove(ClosedWp);
            OpenList.Enqueue(w, openPriority);
            openPriority--;
            ClosedList.Enqueue(w, closedPriority);
            closedPriority--;

        }
    }
    return Solution;
}

基本上，我有返回航点列表的方法（非常简单的类，如果您有兴趣，这里有一个粘贴：http://pastebin.com/Sch5vRY3），但是在我的函数中，列表在返回时计数为 0，因此（错误地）指示没有到达该点的路径。

我已经完成了通常的调试，但是 A* 对我来说有点混乱，所以如果有人可以帮助我解决常见的陷阱，我将不胜感激。

我绝对是人工智能和寻路的新手，所以如果你有任何其他杂项指示，我全听！

谢谢。