PathPtr WaypointManager::getPath(WaypointPtr source, WaypointPtr destination)
{
PathPtr path(new Path());
int state;
Node s(source,0);
Node d(destination,0);
AStarSearch<Node> astarsearch;
astarsearch.SetStartAndGoalStates(s,d);
while((state=astarsearch.SearchStep()) == AStarSearch<Node>::SEARCH_STATE_SEARCHING)
{
log("Astar search stepped");
}
if(state == AStarSearch<Node>::SEARCH_STATE_SUCCEEDED)
{
log("AStar found path");
Node * n = astarsearch.GetSolutionStart();
while(( n = astarsearch.GetSolutionNext() )!= NULL)
path->addWaypoint(n->getWP());
}
astarsearch.CancelSearch();
return path;
}
int main( int argc, char *argv[] )
{
cout << "STL A* 8-puzzle solver implementation\n(C)2001 Justin Heyes-Jones\n";
bool bUserBoard = false;
if( argc > 1 )
{
char *userboard = argv[1];
int i = 0;
int c;
while( c = argv[1][i] )
{
if( isdigit( c ) )
{
int num = (c - '0');
PuzzleState::g_start[i] = static_cast<PuzzleState::TILE>(num);
}
i++;
}
}
// Create an instance of the search class...
AStarSearch<PuzzleState> astarsearch;
int NumTimesToSearch = NUM_TIMES_TO_RUN_SEARCH;
while( NumTimesToSearch-- )
{
// Create a start state
PuzzleState nodeStart( PuzzleState::g_start );
// Define the goal state
PuzzleState nodeEnd( PuzzleState::g_goal );
// Set Start and goal states
astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
unsigned int SearchState;
unsigned int SearchSteps = 0;
do
{
SearchState = astarsearch.SearchStep();
#if DEBUG_LISTS
float f,g,h;
cout << "Search step " << SearchSteps << endl;
cout << "Open:\n";
PuzzleState *p = astarsearch.GetOpenListStart( f,g,h );
while( p )
{
((PuzzleState *)p)->PrintNodeInfo();
cout << "f: " << f << " g: " << g << " h: " << h << "\n\n";
p = astarsearch.GetOpenListNext( f,g,h );
}
cout << "Closed:\n";
p = astarsearch.GetClosedListStart( f,g,h );
while( p )
{
p->PrintNodeInfo();
cout << "f: " << f << " g: " << g << " h: " << h << "\n\n";
p = astarsearch.GetClosedListNext( f,g,h );
}
#endif
// Test cancel search
#if 0
int StepCount = astarsearch.GetStepCount();
if( StepCount == 10 )
{
astarsearch.CancelSearch();
}
#endif
SearchSteps++;
}
while( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_SEARCHING );
if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_SUCCEEDED )
{
#if DISPLAY_SOLUTION_FORWARDS
cout << "Search found goal state\n";
#endif
PuzzleState *node = astarsearch.GetSolutionStart();
#if DISPLAY_SOLUTION_FORWARDS
cout << "Displaying solution\n";
#endif
int steps = 0;
#if DISPLAY_SOLUTION_FORWARDS
node->PrintNodeInfo();
cout << endl;
#endif
for( ;; )
{
node = astarsearch.GetSolutionNext();
if( !node )
{
break;
}
#if DISPLAY_SOLUTION_FORWARDS
node->PrintNodeInfo();
cout << endl;
#endif
steps ++;
};
#if DISPLAY_SOLUTION_FORWARDS
// todo move step count into main algorithm
cout << "Solution steps " << steps << endl;
#endif
////////////
node = astarsearch.GetSolutionEnd();
#if DISPLAY_SOLUTION_BACKWARDS
cout << "Displaying reverse solution\n";
#endif
steps = 0;
node->PrintNodeInfo();
cout << endl;
for( ;; )
{
node = astarsearch.GetSolutionPrev();
if( !node )
{
break;
}
#if DISPLAY_SOLUTION_BACKWARDS
node->PrintNodeInfo();
cout << endl;
#endif
steps ++;
};
#if DISPLAY_SOLUTION_BACKWARDS
cout << "Solution steps " << steps << endl;
#endif
//////////////
// Once you're done with the solution you can free the nodes up
astarsearch.FreeSolutionNodes();
}
else if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_FAILED )
{
#if DISPLAY_SOLUTION_INFO
cout << "Search terminated. Did not find goal state\n";
#endif
}
else if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_OUT_OF_MEMORY )
{
#if DISPLAY_SOLUTION_INFO
cout << "Search terminated. Out of memory\n";
#endif
}
// Display the number of loops the search went through
#if DISPLAY_SOLUTION_INFO
cout << "SearchSteps : " << astarsearch.GetStepCount() << endl;
#endif
}
return 0;
}