Good A star algorithm website!

Gamasutra has an article
http://www.gamasutra.com/features/19990212/sm_04.htm

Here's a cpp file I wrote.  It works fine.


// A Sample by D.S.Reynolds
//
// How It Works:
//
// The basic formula for the A* algorithm is f = g + h.  g represents the cost of
// distance travelled (or gone).  h represents the estimate (or heuristic) of distance
// from current node to the destination.  Some sort of contianers are needed.  Some
// chose stacks, I chose STL vectors and a heap.  I believe the most critical part
// of the formula is the calculation for the heuristic.  Also, Breadth-First and Depth-First
// algorithms are simplified subsets of the A*.
//
//  1.  Create OPEN, CLOSED and PATH containers.  
//  2.  Start with the OPEN container containing only the starting node.
//      Set that nodes g value to 0, it's h value to the euclidian difference
//      ((dx - sx)*(dx - sx)) + ((dy - sy)*(dy - sy)), calc the f value and
//      set the parent node to NULL.
//  3.  Until the goal node is found, repeat the following:
//        If no nodes found exit with failure.
//        Select Node on OPEN with the lowest f value.  This is the BestNode.
//        Push BestNode to Closed (This is the only place CLOSED is populated).
//        If BestNode is the destination, exit.  Otherwise, determine the
//        child nodes (adjacents) of BestNode.
//           For each child node do the following:
//           Set each child nodes Parent to BestNode (We'll use this later to
//             get the path.)
//           Calc the cost of g:  Child.g = BestNode.g + 1  (Use other than 1
//             for various terrain)
//           See if child node is already on OPEN.  If so, determine which has
//             the lower g value and use it's other corresponding values.
//           See if child node is already on CLOSED.  If so, determine which
//             has the lower g value and use it's other corresponding values.
//           If the child node was NOT on OPEN or CLOSED, add it to OPEN.
//
///////////////////////////////////////////////////////////
#ifndef ASTAR_HPP
#define ASTAR_HPP

#include <stdlib.h>
#include <iostream>
#include <conio.h>   // for getch
#include <vector>    // STL for Vector
#include <algorithm> // STL for Heap

using namespace std;

#define MAX_CLOSED_NODES 400
#define MIN_MAP_X        0
#define MIN_MAP_Y        0
#define MAX_MAP_X        9
#define MAX_MAP_Y        9


///////////////////////////////////////////////////////////
// Externally use a vecter of multiple Location objects
class CLocation{
public:
      int x;
      int y;
};


///////////////////////////////////////////////////////////
class CPathNode{
public:                    
    int f, gone, heuristic; // f = gone + heuristic
    int x, y;               // Location of node
    int px, py;             // Locaton of parent node

    void FindPath(int sx, int sy, int dx, int dy, char * pMap, vector<CLocation> *pVector);
};


///////////////////////////////////////////////////////////
void CPathNode::FindPath(int sx, int sy, int dx, int dy, char * pMap, vector<CLocation> *pVector)
{
      vector<CPathNode> OPEN;                 // STL Vectors chosen because of rapid
    vector<CPathNode> CLOSED;               // insertions/deletions at back,
    vector<CPathNode> PATH;                 // and Direct access to any element.
    CPathNode Node, BestNode;               // Temporary Node and BestNode
    bool bNodeFound = false;                // Flag if node is found in container


    Node.x = sx;                            // Create the start node
    Node.y = sy;
    Node.gone = 0;
    Node.heuristic = ((dx - sx)*(dx - sx)) + ((dy - sy)*(dy - sy));
    Node.f = Node.gone + Node.heuristic;    // The classic formula f = g + h
    Node.px = NULL;                         // No parent for start location
    Node.py = NULL;                         // No parent for start location
    OPEN.push_back(Node);                   // Populate the OPEN container with the first location
      make_heap( OPEN.begin(), OPEN.end() );  // Create a heap from OPEN for sorting


    while (!OPEN.empty())
    {
          sort_heap(OPEN.begin(), OPEN.end());// Ascending sort based on overloaded operators below
            BestNode = OPEN.front();            // Set the Node with lowest f value to BESTNODE
            pop_heap(OPEN.begin(), OPEN.end()); // Pop off the heap.  Actually this moves the
                                            // far left value to the far right.  The node
                                            // is not actually removed since the pop is to
                                            // the heap and not the container.
        OPEN.pop_back();                    // Remove node from right (the value we pop_heap'd)
        CLOSED.push_back(BestNode);         // Push the BestNode onto CLOSED

        // If at destination, break and create path below
        if ((BestNode.x == dx) && (BestNode.y == dy))
        {
            //bPathFound = true; // arrived at destination...
            break;
        }

        // Set limit to break if looking too long
        if ( CLOSED.size() > MAX_CLOSED_NODES )
            break;

        // Check adjacent locations (This is done in a clockwise order to lessen jaggies)
        for (int i=1; i<9; i++)
        {
            switch(i)
            {
            case 1:
                Node.x = BestNode.x;
                Node.y = BestNode.y - 1;
                break;
            case 2:
                Node.x = BestNode.x + 1;
                Node.y = BestNode.y - 1;
                break;
            case 3:
                Node.x = BestNode.x + 1;
                Node.y = BestNode.y;
                break;
            case 4:
                Node.x = BestNode.x + 1;
                Node.y = BestNode.y + 1;
                break;
            case 5:
                Node.x = BestNode.x;
                Node.y = BestNode.y + 1;
                break;
            case 6:
                Node.x = BestNode.x - 1;
                Node.y = BestNode.y + 1;
                break;
            case 7:
                Node.x = BestNode.x - 1;
                Node.y = BestNode.y;
                break;
            case 8:
                Node.x = BestNode.x - 1;
                Node.y = BestNode.y - 1;
                break;
            }

            if ((Node.x >= MIN_MAP_X) && (Node.x <= MAX_MAP_X) &&
                (Node.y >= MIN_MAP_Y) && (Node.y <= MAX_MAP_Y))
            {
                // Determine cost of distance travelled
                if ( pMap[Node.x + (Node.y*10)] == 'X' ) // If location is obstruction
                    Node.gone = 1000;
                else
                    Node.gone = BestNode.gone + 1;

                // Determine the Heuristic.  Probably the most crucial aspect
                // Heuristic by Simple Euclidian method
                // Node.heuristic = ((dx - Node.x)*(dx - Node.x)) + ((dy - Node.y)*(dy - Node.y));
                // Heuristic by my own Orthogonal/Diagonal + Euclidian modifier
                int DX = abs(dx - Node.x);
                int DY = abs(dy - Node.y);
                int Orthogonal = abs(DX - DY);
                int Diagonal = abs(((DX + DY) - Orthogonal)/2);
                Node.heuristic = Diagonal + Orthogonal + DX + DY;

                // The A* formula
                Node.f = Node.gone + Node.heuristic;
                Node.px = BestNode.x; // Point parent to last BestNode (pushed onto CLOSED)
                Node.py = BestNode.y; // Point parent to last BestNode (pushed onto CLOSED)


                bNodeFound = false;

                // Check to see if already on OPEN
                for (int i=0; i<OPEN.size(); i++)
                {
                    if ((Node.x == OPEN.at(i).x) &&
                        (Node.y == OPEN.at(i).y))
                    {   // If already on OPEN
                        if (Node.gone < OPEN.at(i).gone)
                        {
                            OPEN.at(i).gone = Node.gone;
                            OPEN.at(i).f = Node.gone + OPEN.at(i).heuristic;
                            OPEN.at(i).px = Node.px;
                            OPEN.at(i).py = Node.py;
                        }
                        bNodeFound = true;
                        break;
                    }
                }
                if (!bNodeFound ) // If Node NOT found on OPEN
                {
                    // Check to see if already on CLOSED
                    for (i=0; i<CLOSED.size(); i++)
                    {
                        if ((Node.x == CLOSED.at(i).x) &&
                            (Node.y == CLOSED.at(i).y))
                        {   // If on CLOSED, Which has lower gone?
                            if (Node.gone < CLOSED.at(i).gone)
                            {
                                CLOSED.at(i).gone = Node.gone;
                                CLOSED.at(i).f = Node.gone + CLOSED.at(i).heuristic;
                                CLOSED.at(i).px = Node.px;
                                CLOSED.at(i).py = Node.py;
                            }
                            bNodeFound = true;
                            break;
                        }
                    }
                }

                if (!bNodeFound ) // If Node NOT found on OPEN or CLOSED
                {
                    OPEN.push_back(Node);                  // Push NewNode onto OPEN
                    push_heap( OPEN.begin(), OPEN.end() ); // Push NewNode onto heap
                    make_heap( OPEN.begin(), OPEN.end() ); // Re-Assert heap, or will be short by one
             
                    /*
                    // Display OPEN and CLOSED containers (For Debugging)
                    int i;
                    cout << "OPEN:   ";
                    for (i=0; i<OPEN.size(); i++)
                    {
                        cout << OPEN.at(i).x << "," << OPEN.at(i).y << ",";
                        cout << OPEN.at(i).gone << "," << OPEN.at(i).heuristic << "  ";
                    }
                    cout << endl;
                    cout << "CLOSED:   ";
                    for (i=0; i<CLOSED.size(); i++)
                    {
                        cout << CLOSED.at(i).x << "," << CLOSED.at(i).y << ",";
                        cout << CLOSED.at(i).gone << "," << CLOSED.at(i).heuristic << "  ";
                    }
                    cout << endl << endl;
                    int ch = _getch();
                    //*/
                }
            }
        }
    }

    if (CLOSED.size() > 0)
    {
        // Create the path from elements of the CLOSED container
        PATH.clear();
        PATH.push_back(CLOSED.back());
        CLOSED.pop_back();

        while (!CLOSED.empty())
        {
            if ((CLOSED.back().x == PATH.back().px) &&
                (CLOSED.back().y == PATH.back().py))
                PATH.push_back(CLOSED.back());

            CLOSED.pop_back();
        }

            // Populate the vector that was passed in by reference
        CLocation Fix;
        pVector->clear();
        for (int i=(PATH.size()-1); i>=0; i--)
        //for (container::iterator i=PATH.begin(); i!= PATH.end(); ++i)
        {
                  Fix.x = PATH.at(i).x;
                  Fix.y = PATH.at(i).y;
                  pVector->push_back(Fix);
            }
    }
}


///////////////////////////////////////////////////////////
// Needed because the template doesn't know what a PathNode is
bool operator<(const CPathNode &a, const CPathNode &b)
{
    return a.f < b.f;
}


///////////////////////////////////////////////////////////
// Needed because the template doesn't know what a PathNode is
bool operator>(const CPathNode &a, const CPathNode &b)
{
    return a.f > b.f;
}


#endif