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