Informed A* algorithm to find the shortest path between two nodes

Written a couple of months ago  and now post up in case it might be helpful to someone interested.

/*--------------------------------------------------------------------------------------
 * 
 *  Informed search using A* algorithm to find the shortest path from Arad (any city)  to Bucharest.
 *  Written by Terry 15 Sep.2005  
 *
 *--------------------------------------------------------------------------------------*/
 
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <map>
#include <queue>
#include <stack>
#include <assert.h>

using namespace std;

class Node{
 public:
  int state;         // can be used as an index into node decor - eg names
  Node * parentptr;  // previous or parent node instance in the search tree

  int action;// not yet used - could be used as child index
  int depth;  //depth in the tree  root is 1
  int pathCost;  // same as  depth at present

////////////////////////////////////////////////////////////////param   "cost" added
 Node( int s, Node * p, int cost){  // normal constructor
    state = s;
    parentptr = p;
    if( parentptr == NULL ){
      depth = 1;
      pathCost = 0;
    }else{
      depth = parentptr->depth + 1;
      pathCost = cost;
    }
  }

  ~Node(){}  // destructor

  Node( const Node & node ){  // copy constructor
    state = node.state;
    parentptr = node.parentptr;
    depth = node.depth;
    pathCost = node.pathCost;
  }

  Node* parent(){  // return a pointer to this instance's parent instance
    if( parentptr != NULL )
      return parentptr;
    else
      return this;
  }

  static int count;  // count of all instances created by factory
  static vector < Node*> all;  // all factory-created instances

  static Node *factory() { return factory( -1, NULL, 0 ); }
  static Node *factory(int s, int cost) { return factory( s, NULL, cost ); }
 
 
////////////////////////////////////////////////////////////////param   "cost" added
    static Node *factory(int s, Node * p, int cost){
    Node * retval = new Node( s, p, cost );
    all.push_back( retval );
    count++;
    return retval;
  }

  static void freeall(){  // free up memory  from all factory-created instances
    for(int i=0;i<all.size();i++)
      delete all[i];
    all.clear();
  }

};

int Node::count = 0;
vector<Node*> Node::all;  // internal list of all factory-created instances

/*---------------------------------------------------------------------------------------*/
// Suitable for a graph like proble such as the Romanian road network
// problem such as in Russell and Norvig, 2nd Edition, Chapter 3.

class Problem{
 public:
  map< string, int > indices;       // map of string-names to integer indices  i=map[string]
  vector<string> names;             // names of the vertices (cities)        string=names[i]  
  vector<vector<int> >successors;  // k=successors[i][j] is jth successor of ith vertex
  vector< vector<int> >weights;     // w=weights[i][j] is jth weight of arc to ith vertex
 
  ///////////////////////////////////////////////////////////////////////////
  map< string, int > marks;                     // map of string name to marks
  ////////////////////////////////////////////////////////////////////////////

  int n()
  {return names.size(); } // number of vertices (cities) in the graph (roadnetwork)
 
  int nSuccessors( int i )
   { return successors[i].size(); }  // out-degree of ith vertex

  Problem(){}   // constructor
  ~Problem(){}  // destructor

  void addSuccessor( string src, string dst, int w ){
   successors[ indices[src] ].push_back( indices[dst] );
   weights   [ indices[src] ].push_back( w );
  }

  void addSuccessorPair( string a, string b, int w ){
    addSuccessor( a, b, w );
    addSuccessor( b, a, w);
  }
 
 

  ////////////////////////////////////////////////////////////////////////////
  void readFile(string filename ){        // currently wired with test data

    // first pass to get city names only:
    ifstream in( filename.c_str() );  
    assert( in );  
    names.clear();
    marks.clear();
   
    int ncities=0;
     int cityindex = 0;
  
    char line[1024];
   
     
    cout << "/nReading cities...   " ;
   
   
 ///////////////////////////////////////////////////////////// keeping reading till get "20" -- number of cities  
   while( in.getline( line, sizeof( line ), '/n' ) ){
      if( line[0] != '#' ){  // ignore  comment lines in the file     
      char *delimiters = " /t/n";   // whitespace possible delimiters
   char *wordptr;
   char *numstr;     
      numstr = strtok (line, delimiters);
      ncities = 1000;
      ncities = atoi(numstr);
     
      if (ncities ==1000)
 //    if ( EOF ==sscanf(numstr, "%d", &ncities) )
            cout << "Error reading number of cities!" <<endl;
      else{
            cout << "Number of cities: " <<ncities <<endl;
           break;
      }  
      }                         
   }

   
///////////////////////////////////////////////////////////////////// get all cities   
    while(  (cityindex < ncities) && in.getline( line, sizeof( line ), '/n' )   ){
 
      if( line[0] != '#' ){  // ignore  comment lines in the file
 char *delimiters = " /t/n";   // whitespace possible delimiters
 int wordcount = 0;
 char *wordptr;
 char *numstr;
  
 wordptr = strtok( line, delimiters ); // tokenize string 
    int mark = 0;
    
 while( (wordptr = strtok( NULL, delimiters )) != NULL ){
   wordcount++;
   if (wordcount == 3) mark = atoi (wordptr); //sscanf(wordptr, "%d", &mark);  
   if (wordcount == 4) {
   string cityname(wordptr);
   addName( cityname );  // add if it exists
   marks[ names[cityindex] ] = mark;
   indices[ names[cityindex] ] = cityindex;
   cout << "index: "<< indices[ names[cityindex] ] << "     cityname: " << names[cityindex] <<"         mark: "<< mark<< endl;
       }  
 }  
 cityindex =cityindex+1;
    }     
    }
   
      cout << "Reading cities finished!/n" << endl;
       
    vector<int> dummy;  // an empty vector<int>
    successors.assign( n(), dummy );  // sets up 1st dimension  with n() copies of dummy
    weights.assign( n(), dummy );  // sets up 1st dimension  with n() copies of dummy

     cout << "Reading roads...   " ;
    
    
 /////////////////////////////////////////// keep reading till get "23' -- number of roads
 int nroads = 0, roadnum = 0; 
    if( in.getline( line, sizeof( line ), '/n' )){
      if( line[0] != '#' ){  // ignore  comment lines in the file     
      char *delimiters = " /t/n";   // whitespace possible delimiters
   char *wordptr;
   char *numstr;     
      numstr = strtok (line, delimiters);
      nroads = 1000;
  nroads =  atoi(numstr);
 
 
  if (nroads ==1000 )
 //     if (EOF ==sscanf(numstr, "%d", &nroads))
            cout << "Error reading number of roads!" <<endl;
      else
            cout << "Number of roads: " <<nroads <<endl;                       
      }                         
   }  
 
///////////////////////////////////////////////////////////////////    get all roads 
 
 
    while( in.getline( line, sizeof( line ), '/n' ) ){ 
      if( line[0] != '#' ){  // ignore  comment lines in the file
     
      char *delimiters = " /t/n";   // whitespace possible delimiters
      int wordcount = 0;
   char *wordptr;
   char *numstr;
        roadnum++;
        cout << " Road:" << roadnum;      
     
     
 int srcidx=0, dstidx=0, length=0; 
 wordptr = strtok( line, delimiters ); // tokenize string
 
 srcidx = atoi(wordptr);
                                                           // the first number, should be the source
 
 string source(names[srcidx] );
 assert( nameExists( source ) );  // should already exist

 string destination;
 while( (wordptr = strtok( NULL, delimiters )) != NULL ){
   wordcount++;

   if (wordcount==1){   
                             // tht second number, should be the destination
   dstidx = atoi(wordptr); //sscanf(wordptr, "%d", &dstidx);
   destination = (names[dstidx]);
   assert( nameExists( destination ) );  // should already exist
   }  
  
   if (wordcount==2){                            //  the thrid number, should be the length
  length = atoi(wordptr); // sscanf(wordptr, "%d", &length); 
   addSuccessorPair(source, destination, length );  
 cout << "         Source: "<< srcidx << "  --->   Destination: " << dstidx <<"     length: "<< length<<""<< endl;
  } 
 }
      } //end if
      
    }
   
    in.close();   
    cout << "Reading roads finished!/n" << endl;
  }
 
///////////////////////////////////////////////////////////////////////////////

 bool nameExists( string name ){  // does a name alredy exist in vocabulary of names?
    for( vector<string>:: iterator it= names.begin() ; it!=names.end() ; it++){
      if( name == *it ) return true;
    }
    return false;
  }

  void addName( string name ){   // add name to names only if it is
 
     // not already in vocabulary
    if( ! nameExists( name ) ){
      names.push_back( name );
    }
  }
 
 // printout problem graph for check purposes - writes out each
  // vertex, its out-degree, and its list of successor vertices.
  void print(){
    int bmax = 0;
    for(int i=0;i<n();i++){
      cout << names[i] << " ("<<nSuccessors(i)<<")::";
      bmax = bmax > nSuccessors(i) ? bmax : nSuccessors(i);
      for(int j=0;j<nSuccessors(i);j++){
 cout << names[ successors[i][j] ] << " ";
      }
      cout << endl;
    }
    cout << "Max breadth b " << bmax << endl;
  }

};

/*-----------------------------------------------------------------------------------*/

class Queue : public stack<Node *> {  // a LIFO Queue aka a Stack
  // push() and top() are already defined
};

/*-----------------------------------------------------------------------------------*/

int main( int argc, char *argv[] ){  
  Problem problem; 
  problem.readFile( string("romania2.dat") );
// problem.testProblem();
//  problem.print();  
 cout << endl;
 
  string goalName("Bucharest");
  string startName("Arad");
 
// string startName("Sibiu");         //////   tested, no problem
//string startName("Craiova");         //////   tested, no problem
//    string startName("Zerind");      ///////   tested, no problem

  Node *nodeptr = Node::factory( problem.indices[startName], 0 );
 
 cout << "<==================  start considering ====================>" << endl;

  Queue fringe;  // the successor set to the node under scrutiny
  fringe.push( nodeptr );

  bool success = false;
  
  while(!success){
 
    if( fringe.empty() ) {   
  break;    // IF-EMPTY -> FAILURE
 }
 

   nodeptr = fringe.top();
// fringe.pop();

 
  int from;
  if (nodeptr->parentptr == NULL)
    from = 0;
  else
    from = nodeptr->parentptr->state;     
       
    cout << "/n/n( from " <<problem.names[from]<< " ) considering at " << problem.names[ nodeptr->state ] << " ( depth:"<<nodeptr->depth << ")"<< endl;

    int i = nodeptr->state;
    if( problem.names[ i ] == goalName ) {
        success = true;
  cout << " ************* " << endl;
        cout << " * Arriving! * "<< endl;
        cout << " ************* " << endl;
          break;
    }    
   
     int  hFun_RemainingCost;        //    h function storing estimated cost from n to d
    vector <int> fFun_TotalCost;      //    f function storing sum of costs =  cost(n's previous node->n) + esti_cost(n->d)
     int totalcost=0;
   
    for( int j=0;j< problem.nSuccessors( i );j++){   // EXPAND   
        cout << "  " << problem.names[i] << " --" << problem.weights[i][ j ] << "--> " << problem.names[  problem.successors[i][j]  ]  <<  " + ";
 
      hFun_RemainingCost = problem.marks[ problem.names[ problem.successors[i][j] ] ] ;         //  n' mark
     cout << "  from " << problem.names[  problem.successors[i][j]] << " to " <<goalName << " ( " << hFun_RemainingCost << " ) " ;
   
     totalcost = nodeptr->pathCost +  problem.weights[i][j] + hFun_RemainingCost ;     //  cost_so_far + cost (present node -> n)  + n' mark
      
       fFun_TotalCost.push_back( totalcost);                                                // storing into a temp vector
       cout << " = " << totalcost << endl;     
    }      
   
    int number = fFun_TotalCost.size();
    cout << "Number of successors :" << number << endl;
    cout << "Pushing successors at cost decreasing order..."<<endl;
     for( int j=0;j<number ;j++){   
     vector <int>::iterator itt;
    
     int maxcost = -10;
      for ( itt=fFun_TotalCost.begin(); itt !=  fFun_TotalCost.end(); itt++)
       if (*itt > maxcost)     { maxcost = *itt; }
 
       vector <int>::iterator it ;       
       int idx = 0;                                                         // which successor in the temp vector has the max estimated cost
       for (it = fFun_TotalCost.begin(); it != fFun_TotalCost.end(); it++)   {
              if ( *it == maxcost )          break;
              idx ++;                          
       }                            
 
      fringe.push(  Node::factory( problem.successors[i][ idx], nodeptr, nodeptr->pathCost+ problem.weights[i][idx])  );  
      cout << " Pushing node: " << problem.names[ problem.successors[i][ idx] ] << "(" <<*it<< ")" <<  endl;
      *it = -1000;                       // change the max cost to a nagivate value so that it can't affect the next turn of selection
     }  

  }
 
 
   
// report on the solution path found by back tracing parent pointers
  if( ! success ){
    cout << "Failure!" << endl;
  }
  else{
    cout << endl << "Solution: " << endl;
 
   cout <<"  Total distance of the route found: " << nodeptr->pathCost << " " << endl;  
   cout << "  Route:   " ;
   
     
    while( problem.names[ nodeptr->state ] != startName ){
      cout << problem.names[ nodeptr->state ] << " <--"<< nodeptr->pathCost - nodeptr->parentptr->pathCost << "-- " ;
      nodeptr = nodeptr->parent();
    }
 cout << problem.names[ nodeptr->state ] << endl;
   
  }

  Node::freeall();  // free up space
  cout << endl << Node::all.size() << " Nodes left" << "       " << Node::count << " Nodes created" << endl;
 
 
 
 
}

/*---------------------------------------------------------------------------------------
 Development Notes:
 
The C++ STL queue with pop and front implements a FIFO queue.  This gives a BFS tree search.

The C++ Stack is a LIFO  - use  top and pop.

Both implement  push  which  pushes  onto end of the Queue.

@Terry 15 Sep. 2005-------------------------------------------------------------------------------*/

Map file as follows:

# Romanian Road Network, From Russell & Norvig Chapters 3 and 4
20
0   42   173   366    Arad
1   80   128   374    Zerind
2   129   64   380    Oradea
3   238   199   253    Sibiu
4   60   284   329    Timisoara
5   159   320   244    Lugoj
6   160   368   241    Mehadia
7   150   416   242    Dobreta
8   280   418   160    Craiova
9   281   277   193    Rimnicu-Vilcea
10   384   238   176    Fagaras
11   359   353   100    Pitesti
12   453   328   0    Bucharest
13   448   401   77    Giugiu
14   537   307   80    Urziceni
15   630   308   151    Hirsova
16   656   367   161    Eforie
17   591   159   199    Vaslui
18   522   104   226    Iasi
19   463   82   234    Neamt
23
0   1   75  
1   2   71  
2   3   151  
3   0   140  
0   4   118  
4   5   111  
6   5   70  
6   7   75  
7   8   120  
8   9   146  
9   3   80  
3   10   99  
10   12   211  
12   13   90  
11   8   138  
11   12   101  
14   12   85  
14   15   98  
15   16   86  
14   17   142  
17   18   92  
18   19   87  
11   9   97