Cannot figure out what's wrong with my A* Algorithm

Question

I'm currently implementing an A* pathfinding algorithm in my 2D side-scroller, but I'm having some difficulty.

I thought I implemented it correctly, but obviously not, because it doesn't work.

I'm using a priority queue for efficiency, and followed the algorithm discussed here:

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

I did not use their priority queue, however, as many of the comments pointed out that it has some issues. Instead, I use the following priority queue:

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

Here's my function, and then I'll discuss the issue:

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;

}

Basically, I have the method returning a List of Waypoints (very simple class, here's a pastebin of it if you're interested: http://pastebin.com/Sch5vRY3), however in my function the List has a Count of 0 upon returning, thus (incorrectly) indicating there's no path to the point.

I've done the usual debugging, but A* is a bit confusing to me, so if anyone could help me with common pitfalls, I'd appreciate it.

I'm definitely a novice at Artificial Intelligence and Path-finding, so if you have any other miscellaneous pointers, I'm all ears!

Thanks.

Answer 1

Well I won't tell the exact code but I'll tell what is an A* algorithm and what is it in your case:
In A* we define two functions of state: the cost and the heuristic, the cost means "what is the cost of transferring from the current state to another state", in your case it should be the distance of the path between the current point and the point I'll try.
The heuristic means "How far am I from the goal", in your case maybe the distance from my location to the goal would be great.
After writing these two functions, you write a function let's call it "f" which sums the two.
The A* algorithm is actually a greedy backtracking algorithm in which you try the least "f".