void astar_callback(int f){
TangoData tmp=TangoData::GetInstance();
//if ( tmp.is_relocalized==false ) return;

      trace_astar_sol->ClearVertexArray();
		MapSearchNode nodeStart(getNearestPosToPosStar(position_adf));
		MapSearchNode nodeEnd(tmp.pos_astar[f]);
        PlaceMarker(tmp.pos_astar[f]+kHeightOffset);
		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
		unsigned int SearchState;
		do
		{
			SearchState = astarsearch.SearchStep();
            MapSearchNode *p = astarsearch.GetOpenListStart();
            while( p )p = astarsearch.GetOpenListNext();
            p = astarsearch.GetClosedListStart();
            while( p )p = astarsearch.GetClosedListNext();
		}
		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
		{		MapSearchNode *node = astarsearch.GetSolutionStart();
         		for( ;; )
				{
				    node = astarsearch.GetSolutionNext();
					if(!node)break;
                    trace_astar_sol->UpdateVertexArray(node->toVec3());
				};
				astarsearch.FreeSolutionNodes();
		}
		astarsearch.EnsureMemoryFreed();
}
Example #2
0
bool ComputeAStar( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode, Vector2List& path )
{
	AStarSearch<SearchInterface> search;
	SearchInterface pStart(pSourceNode);
	SearchInterface pEnd(pDestNode);
	
	search.SetStartAndGoalStates( pStart, pEnd );

	while( AStarSearch<SearchInterface>::SEARCH_STATE_SEARCHING == search.SearchStep() )
	{

	}

	int curState = search.GetState();
	if( curState == AStarSearch<SearchInterface>::SEARCH_STATE_SUCCEEDED )
	{
		//Get path
		for( SearchInterface* pCur = search.GetSolutionStart(); pCur != NULL; pCur = search.GetSolutionNext() )
		{
			path.push_back( pCur->pNode->BBox.Centroid() );
		}
		search.FreeSolutionNodes();
		return true;
	}

	return false;
}
Example #3
0
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;

}
Example #4
0
int Astar::findPath( int Ax, int Ay, int Bx, int By, int &Cx, int &Cy)
{
	int result = 1;
	AStarSearch<MapSearchNode> astarsearch;
	unsigned int SearchCount = 0;
	const unsigned int NumSearches = 1;
	while(SearchCount < NumSearches)
	{
		//Create a start state
		MapSearchNode nodeStart;
		nodeStart.x = Ax;
		nodeStart.y = Ay; 

		//Define the goal state
		MapSearchNode nodeEnd;
		nodeEnd.x = Bx;						
		nodeEnd.y = By; 
		
		//Set Start and goal states
		astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		do
		{
			SearchState = astarsearch.SearchStep();
			SearchSteps++;
		}
		while (SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING);

		if (SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED)
		{
			MapSearchNode *node = astarsearch.GetSolutionStart();

			int steps = 0;
			for(ever)
			{
				node = astarsearch.GetSolutionNext();
				if( !node )
					break;
				if (steps == 0)
				{
					Cx = node->x;
					Cy = node->y;
				}
				steps ++;
			}

			astarsearch.FreeSolutionNodes();
		}
		else if( SearchState == AStarSearch <MapSearchNode>::SEARCH_STATE_FAILED ) 
Example #5
0
int main(int argc, char* argv[]) {
    cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";

    // Our sample problem defines the world as a 2d array representing a terrain
    // Each element contains an integer from 0 to 5 which indicates the cost
    // of travel across the terrain. Zero means the least possible difficulty
    // in travelling (think ice rink if you can skate) whilst 5 represents the
    // most difficult. 9 indicates that we cannot pass.

    // Create an instance of the search class...

    AStarSearch<MapSearchNode> astarsearch;

    unsigned int SearchCount = 0;

    const unsigned int NumSearches = 1;

    while (SearchCount < NumSearches) {
        // Create a start state
        MapSearchNode nodeStart;
        nodeStart.x = rand() % MAP_WIDTH;
        nodeStart.y = rand() % MAP_HEIGHT;

        // Define the goal state
        MapSearchNode nodeEnd;
        nodeEnd.x = rand() % MAP_WIDTH;
        nodeEnd.y = rand() % MAP_HEIGHT;

        // Set Start and goal states

        astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);

        unsigned int SearchState;
        unsigned int SearchSteps = 0;

        do {
            SearchState = astarsearch.SearchStep();

            SearchSteps++;

#if DEBUG_LISTS

            cout << "Steps:" << SearchSteps << "\n";

            int len = 0;

            cout << "Open:\n";
            MapSearchNode* p = astarsearch.GetOpenListStart();
            while (p) {
                len++;
#if !DEBUG_LIST_LENGTHS_ONLY
                ((MapSearchNode*)p)->PrintNodeInfo();
#endif
                p = astarsearch.GetOpenListNext();
            }

            cout << "Open list has " << len << " nodes\n";

            len = 0;

            cout << "Closed:\n";
            p = astarsearch.GetClosedListStart();
            while (p) {
                len++;
#if !DEBUG_LIST_LENGTHS_ONLY
                p->PrintNodeInfo();
#endif
                p = astarsearch.GetClosedListNext();
            }

            cout << "Closed list has " << len << " nodes\n";
#endif

        } while (SearchState ==
                 AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING);

        if (SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED) {
            cout << "Search found goal state\n";

            MapSearchNode* node = astarsearch.GetSolutionStart();

#if DISPLAY_SOLUTION
            cout << "Displaying solution\n";
#endif
            int steps = 0;

            node->PrintNodeInfo();
            for (;;) {
                node = astarsearch.GetSolutionNext();

                if (!node) {
                    break;
                }

                node->PrintNodeInfo();
                steps++;
            };

            cout << "Solution steps " << steps << endl;

            // Once you're done with the solution you can free the nodes up
            astarsearch.FreeSolutionNodes();

        } else if (SearchState ==
                   AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED) {
            cout << "Search terminated. Did not find goal state\n";
        }

        // Display the number of loops the search went through
        cout << "SearchSteps : " << SearchSteps << "\n";

        SearchCount++;

        astarsearch.EnsureMemoryFreed();
    }

    return 0;
}
//------------------------------------------------------------------------------
std::deque<WaypointServer*> WaypointManagerServer::findPath(WaypointSearchNode * start, WaypointSearchNode * end)
{
    std::deque<WaypointServer*> result;

    // no waypoints loaded here -> bail
    if(open_wp_.empty() || wp_map_.empty()) return result;    

    AStarSearch<WaypointSearchNode> astarsearch;

    unsigned int SearchCount = 0;

		
    // Set Start and goal states
		
    astarsearch.SetStartAndGoalStates( *start, *end );

    unsigned int SearchState;
    unsigned int SearchSteps = 0;

    do
    {
        SearchState = astarsearch.SearchStep();

        SearchSteps++;
    }
    while( SearchState == AStarSearch<WaypointSearchNode>::SEARCH_STATE_SEARCHING );

    if( SearchState == AStarSearch<WaypointSearchNode>::SEARCH_STATE_SUCCEEDED )
    {
        // s_log << "Search found goal state\n";

        WaypointSearchNode *node = astarsearch.GetSolutionStart();
                
        result.push_back(&wp_map_[node->x_][node->z_]);

        int steps = 0;

        //node->PrintNodeInfo();
        for( ;; )
        {
            node = astarsearch.GetSolutionNext();

            if( !node )
            {
                break;
            }

            result.push_back(&wp_map_[node->x_][node->z_]);

            //node->PrintNodeInfo();
            steps ++;
				
        };

        //s_log << "Solution steps " << steps << "\n";

        // Once you're done with the solution you can free the nodes up
        astarsearch.FreeSolutionNodes();

	
    }
    else if( SearchState == AStarSearch<WaypointSearchNode>::SEARCH_STATE_FAILED ) 
    {
        //s_log << "Search terminated. Did not find goal state\n";		
    }

    // Display the number of loops the search went through
    // s_log << "SearchSteps : " << SearchSteps << "\n";

    SearchCount ++;

    astarsearch.EnsureMemoryFreed();
	

    return result;
}
Example #7
0
int Ghost::SetTarget(){
    // std::cout<<"target set"<<std::endl;
     
     
AStarSearch<MapSearchNode> astarsearch;

	unsigned int SearchCount = 0;

	const unsigned int NumSearches = 1;

	while(SearchCount < NumSearches)
	{

		// Create a start state
		MapSearchNode nodeStart;

		nodeStart.x = m_X/30;
		nodeStart.y = m_Y/30; 

		// Define the goal state
		MapSearchNode nodeEnd;
		if(pill==0&&!Dead){
		  nodeEnd.x = targetFx;						
		  nodeEnd.y = targetFy; 
        }
        if(pill==1&&!Dead){
		  nodeEnd.x = targetFy;						
		  nodeEnd.y = targetFx; 
        } 
        if(Dead){
                 
          nodeEnd.x = 12;
         // targetFx=12;
         // targetFy=9;						
          nodeEnd.y = 10; 
        }
		// Set Start and goal states
		
		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		do
		{
			SearchState = astarsearch.SearchStep();

			SearchSteps++;

	#if DEBUG_LISTS

			cout << "Steps:" << SearchSteps << "\n";

			int len = 0;

			cout << "Open:\n";
			MapSearchNode *p = astarsearch.GetOpenListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY			
			//	((MapSearchNode *)p)->PrintNodeInfo();
	#endif
				p = astarsearch.GetOpenListNext();
				
			}

			cout << "Open list has " << len << " nodes\n";

			len = 0;

			cout << "Closed:\n";
			p = astarsearch.GetClosedListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY			
			//	p->PrintNodeInfo();
	#endif			
				p = astarsearch.GetClosedListNext();
			}

			cout << "Closed list has " << len << " nodes\n";
	#endif

		}
		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
		{
		//	cout << "Search found goal state\n";
                
				MapSearchNode *node = astarsearch.GetSolutionStart();

	#if DISPLAY_SOLUTION
				cout << "Displaying solution\n";
	#endif

			//	node->PrintNodeInfo();
				node = astarsearch.GetSolutionNext();
				if(!node){
                          
                    return 1;
                }
                if(node){
 
                  targetNx=node->RetX();
			      targetNy=node->RetY();                        
            //    node->PrintNodeInfo();
                   if(Dead)
                      if(targetNx==12&&targetNy==10)
			               Reset();

			               
                }
/*			    cout << "for reference \n";
			    for( ;; )
				{
					node = astarsearch.GetSolutionNext();
					if( !node )
					{
						break;
					}

					node->PrintNodeInfo();
				};
	*/		    
			// Once you're done with the solution you can free the nodes up
				astarsearch.FreeSolutionNodes();

	
		}
		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ) 
		{
			cout << "Search terminated. Did not find goal state\n";
		
		}

		// Display the number of loops the search went through
	//	cout << "SearchSteps : " << SearchSteps << "\n";

		SearchCount ++;

		astarsearch.EnsureMemoryFreed();
	}

return 0;

                   
}
int main()
{
    MineMap map;
	//map.openFileForMap("C:\\Users\\Vlad\\Projects\\C++\\Lex200icfp\\Debug\\test.map");
    map.ReadMap();
    //map.ShortPrint();
    vector<Point> lam = map.GetLambdas();
    Point start = map.GetRobot();
    vector< list<Point> > paths;

    vector<char> res;

    vector< vector<Point> >  clstrs = GetCluster(lam);
    Point minp;
	//cout<<"We get index and route to nearest cluster "<<endl;
    int clastertomove = GetNearestAA(map, clstrs, start, &minp, 1);

    AStarSearch as;

	list<Point>* route = new list<Point>();
	bool claster_achieved = false;
    while (clastertomove != -1)
    {
		if(map.Waterproof() == 1)
		{
			res.push_back('A');
			printvect(res);
			cout<<'\n';
			return 0;
		}
		if(!claster_achieved)
		{
			route = new list<Point>();
			//MOVEROBOT here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			Point ppp = map.GetRobot();
			as.getRoute(&map, ppp, minp, *route, isSafeFromRocks, (char*)"*#W@");
			if(route->size() == 0)
			{
				clastertomove = GetNearestAA(map, clstrs, map.GetRobot(), &minp, 1);
				continue;
			}
		//	cout<<"//we get a route to the nearest point of the nearest claster"<<endl;
			do
			{
				MoveRobot(&res, map, route);
			}
			while((!map.isChanged()) && (route->size() != 0));
			if(map.isChanged())								//if map changed we get route to claster again
			{
				if(route->size() == 0)
					claster_achieved = true;
				continue;
			}
		}
		//cout<<"//when we achieved claster we achieved it's lambda"<<endl;
		if(find(clstrs[clastertomove].begin(), clstrs[clastertomove].end(), map.GetRobot()) != clstrs[clastertomove].end())
			clstrs[clastertomove].erase(find(clstrs[clastertomove].begin(), clstrs[clastertomove].end(), map.GetRobot()));
		list<Point> *route_to_nrp = new list<Point>();												//route to nearest point in claster
		//we running from lambda to lambda in claster
		Point p = GetNextLam(map, as, map.GetRobot(), clstrs[clastertomove], route_to_nrp);
		route_to_nrp->clear();												//f*****g kostyl
		PathSize(map, map.GetRobot(), p, route_to_nrp, as);
		while(route_to_nrp->size())//!(map.GetRobot() == p))
		{
			do
			{
				//if point to move is lambda then remove it from cluster
				if(find(clstrs[clastertomove].begin(), clstrs[clastertomove].end(), *(route_to_nrp->begin())) != clstrs[clastertomove].end())
					clstrs[clastertomove].erase(find(clstrs[clastertomove].begin(), clstrs[clastertomove].end(), *route_to_nrp->begin()));
				MoveRobot(&res, map, route_to_nrp);
			}
			while((route_to_nrp->size() != 0) && (!map.isChanged()));
			//don't have any thoughts about action when map changes. Now robot continues gather lambdas in claster
			route_to_nrp->clear();
			Point p = GetNextLam(map, as, map.GetRobot(), clstrs[clastertomove], route_to_nrp);
			route_to_nrp->clear();
			PathSize(map, map.GetRobot(), p, route_to_nrp, as);
		}
		clstrs.erase(clstrs.begin() + clastertomove);
		clastertomove = GetNearestAA(map, clstrs, map.GetRobot(), &minp, 1);				//we get new claster to move
		claster_achieved = false;
        //cout<<"//and repeat our routine"<<endl;
    }
//	cout<<"//when all what we can do is done, we find way to exit"<<endl;
    minp = map.GetLift();
    route = new list<Point>();
	while(!(map.GetRobot() == map.GetLift()))
	{
		route = new list<Point>();
		Point ppp = map.GetRobot();
		as.getRoute(&map, ppp, minp, *route, isSafeFromRocks, (char*)"*#W@");
		
		if(route->size() == 0)
		{
	//		cout<<"//if lift is unaccessible we send A"<<endl;
			res.push_back('A');
			printvect( res );
			cout <<"\n";
			return 0;
		}
		//cout<<"//we are going to lift"<<endl;
		do
		{
			MoveRobot(&res, map, route);
			if(route->size() == 0)
			{
				printvect( res );
				cout <<"\n";
				return 0;
			}
		}
		while(!map.isChanged());
	}
    
    return 0;
}
Example #9
0
bool ast::astar( uint8_t* map,
                 uint32_t width,
                 uint32_t height,
                 const point_t start,
                 const point_t goal,
                 std::vector<point_t>& path )
{
    //cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";

    // set the static vars
    _map = map;
    _map_width = width;
    _map_height = height;

    // Our sample problem defines the world as a 2d array representing a terrain
    // Each element contains an integer from 0 to 5 which indicates the cost
    // of travel across the terrain. Zero means the least possible difficulty
    // in travelling (think ice rink if you can skate) whilst 5 represents the
    // most difficult. 9 indicates that we cannot pass.

    // Create an instance of the search class...

    AStarSearch<MapSearchNode> astarsearch;

    unsigned int SearchCount = 0;

    const unsigned int NumSearches = 1;

    bool path_found = false;

    while(SearchCount < NumSearches)
    {

        // Create a start state
        MapSearchNode nodeStart;
        nodeStart.x = start.x;
        nodeStart.y = start.y;

        // Define the goal state
        MapSearchNode nodeEnd;
        nodeEnd.x = goal.x;
        nodeEnd.y = goal.y;

        // Set Start and goal states

        astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

        unsigned int SearchState;
        unsigned int SearchSteps = 0;

        do
        {
            SearchState = astarsearch.SearchStep();

            SearchSteps++;

#if DEBUG_LISTS

            cout << "Steps:" << SearchSteps << "\n";

            int len = 0;

            cout << "Open:\n";
            MapSearchNode *p = astarsearch.GetOpenListStart();
            while( p )
            {
                len++;
#if !DEBUG_LIST_LENGTHS_ONLY
                ((MapSearchNode *)p)->PrintNodeInfo();
#endif
                p = astarsearch.GetOpenListNext();

            }

            cout << "Open list has " << len << " nodes\n";

            len = 0;

            cout << "Closed:\n";
            p = astarsearch.GetClosedListStart();
            while( p )
            {
                len++;
#if !DEBUG_LIST_LENGTHS_ONLY
                p->PrintNodeInfo();
#endif
                p = astarsearch.GetClosedListNext();
            }

            cout << "Closed list has " << len << " nodes\n";
#endif

        }
        while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

        if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
        {
            //cout << "Search found goal state\n";

            MapSearchNode *node = astarsearch.GetSolutionStart();

#if DISPLAY_SOLUTION
            cout << "Displaying solution\n";
#endif
            int steps = 0;

            //node->PrintNodeInfo();

            path.push_back( point_t( node->x, node->y ) );

            for( ;; )
            {
                node = astarsearch.GetSolutionNext();

                if( !node )
                {
                    break;
                }

                //node->PrintNodeInfo();

                path.push_back( point_t( node->x, node->y ) );

                steps ++;

            };

            //cout << "Solution steps " << steps << endl;

            // Once you're done with the solution you can free the nodes up
            astarsearch.FreeSolutionNodes();

            path_found = true;

        }
        else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED )
        {
            cout << "Search terminated. Did not find goal state\n";

        }

        // Display the number of loops the search went through
        //cout << "SearchSteps : " << SearchSteps << "\n";

        SearchCount ++;

        astarsearch.EnsureMemoryFreed();
    }

    return path_found;
}
int main( int argc, char *argv[] )
{
  // creating map of Romania
  for(int i=0; i<MAX_CITIES; i++)
    for(int j=0; j<MAX_CITIES; j++)
      RomaniaMap[i][j]=-1.0;

  RomaniaMap[Arad][Sibiu]=140;
  RomaniaMap[Arad][Zerind]=75;
  RomaniaMap[Arad][Timisoara]=118;
  RomaniaMap[Bucharest][Giurgiu]=90;
  RomaniaMap[Bucharest][Urziceni]=85;
  RomaniaMap[Bucharest][Fagaras]=211;
  RomaniaMap[Bucharest][Pitesti]=101;
  RomaniaMap[Craiova][Drobeta]=120;
  RomaniaMap[Craiova][RimnicuVilcea]=146;
  RomaniaMap[Craiova][Pitesti]=138;
  RomaniaMap[Drobeta][Craiova]=120;
  RomaniaMap[Drobeta][Mehadia]=75;
  RomaniaMap[Eforie][Hirsova]=75;
  RomaniaMap[Fagaras][Bucharest]=211;
  RomaniaMap[Fagaras][Sibiu]=99;
  RomaniaMap[Giurgiu][Bucharest]=90;
  RomaniaMap[Hirsova][Eforie]=86;
  RomaniaMap[Hirsova][Urziceni]=98;
  RomaniaMap[Iasi][Vaslui]=92;
  RomaniaMap[Iasi][Neamt]=87;
  RomaniaMap[Lugoj][Timisoara]=111;
  RomaniaMap[Lugoj][Mehadia]=70;
  RomaniaMap[Mehadia][Lugoj]=70;
  RomaniaMap[Mehadia][Drobeta]=75;
  RomaniaMap[Neamt][Iasi]=87;
  RomaniaMap[Oradea][Zerind]=71;
  RomaniaMap[Oradea][Sibiu]=151;
  RomaniaMap[Pitesti][Bucharest]=101;
  RomaniaMap[Pitesti][RimnicuVilcea]=97;
  RomaniaMap[Pitesti][Craiova]=138;
  RomaniaMap[RimnicuVilcea][Pitesti]=97;
  RomaniaMap[RimnicuVilcea][Craiova]=146;
  RomaniaMap[RimnicuVilcea][Sibiu]=80;
  RomaniaMap[Sibiu][RimnicuVilcea]=80;
  RomaniaMap[Sibiu][Fagaras]=99;
  RomaniaMap[Sibiu][Oradea]=151;
  RomaniaMap[Sibiu][Arad]=140;
  RomaniaMap[Timisoara][Arad]=118;
  RomaniaMap[Timisoara][Lugoj]=111;
  RomaniaMap[Urziceni][Bucharest]=85;
  RomaniaMap[Urziceni][Hirsova]=98;
  RomaniaMap[Urziceni][Vaslui]=142;
  RomaniaMap[Vaslui][Urziceni]=142;
  RomaniaMap[Vaslui][Iasi]=92;
  RomaniaMap[Zerind][Arad]=75;
  RomaniaMap[Zerind][Oradea]=71;

  // City names
  CityNames[Arad].assign("Arad");
  CityNames[Bucharest].assign("Bucharest");
  CityNames[Craiova].assign("Craiova");
  CityNames[Drobeta].assign("Drobeta");
  CityNames[Eforie].assign("Eforie");
  CityNames[Fagaras].assign("Fagaras");
  CityNames[Giurgiu].assign("Giurgiu");
  CityNames[Hirsova].assign("Hirsova");
  CityNames[Iasi].assign("Iasi");
  CityNames[Lugoj].assign("Lugoj");
  CityNames[Mehadia].assign("Mehadia");
  CityNames[Neamt].assign("Neamt");
  CityNames[Oradea].assign("Oradea");
  CityNames[Pitesti].assign("Pitesti");
  CityNames[RimnicuVilcea].assign("RimnicuVilcea");
  CityNames[Sibiu].assign("Sibiu");
  CityNames[Timisoara].assign("Timisoara");
  CityNames[Urziceni].assign("Urziceni");
  CityNames[Vaslui].assign("Vaslui");
  CityNames[Zerind].assign("Zerind");

  ENUM_CITIES initCity = Arad;
  if(argc == 2)
  {
    bool found = false;
    for(size_t i=0; i<CityNames.size(); i++)
      if(CityNames[i].compare(argv[1])==0)
      {
        initCity = (ENUM_CITIES)i;
        found = true;
        break;
      }
    if(not found)
    {
      cout << "There is no city named "<<argv[1]<<" in the map!\n";
      return(1);
    }
  }

  // An instance of A* search class
	AStarSearch<PathSearchNode> astarsearch;

	unsigned int SearchCount = 0;
	const unsigned int NumSearches = 1;

	while(SearchCount < NumSearches)
	{
		// Create a start state
		PathSearchNode nodeStart;
		nodeStart.city = initCity;

		// Define the goal state, always Bucharest!
		PathSearchNode nodeEnd;
		nodeEnd.city = Bucharest;

		// Set Start and goal states
		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		do
		{
			SearchState = astarsearch.SearchStep();
			SearchSteps++;

	#if DEBUG_LISTS

			cout << "Steps:" << SearchSteps << "\n";

			int len = 0;

			cout << "Open:\n";
			PathSearchNode *p = astarsearch.GetOpenListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				((PathSearchNode *)p)->PrintNodeInfo();
	#endif
				p = astarsearch.GetOpenListNext();

			}
			cout << "Open list has " << len << " nodes\n";

			len = 0;

			cout << "Closed:\n";
			p = astarsearch.GetClosedListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				p->PrintNodeInfo();
	#endif
				p = astarsearch.GetClosedListNext();
			}

			cout << "Closed list has " << len << " nodes\n";
	#endif

		}
		while( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_SUCCEEDED )
		{
			cout << "Search found the goal state\n";
      PathSearchNode *node = astarsearch.GetSolutionStart();
      cout << "Displaying solution\n";
      int steps = 0;
      node->PrintNodeInfo();
      for( ;; )
      {
        node = astarsearch.GetSolutionNext();
        if( !node ) break;
        node->PrintNodeInfo();
        steps ++;
      };
      cout << "Solution steps " << steps << endl;
      // Once you're done with the solution you can free the nodes up
			astarsearch.FreeSolutionNodes();
		}
		else if( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_FAILED )
		{
			cout << "Search terminated. Did not find goal state\n";
		}
		// Display the number of loops the search went through
		cout << "SearchSteps : " << SearchSteps << "\n";
		SearchCount ++;
		astarsearch.EnsureMemoryFreed();
	}

	return 0;
}
Example #11
0
void Mystery::step() {

    if (ended) {
        return;
    }

    AStarSearch<MapSearchNode> astarsearch;

    std::vector<Character *>::iterator itChars;
    for (itChars = characters.begin(); itChars < characters.end(); ++itChars) {

        Character *character = (Character *) *itChars;

        if (character->dead) {
            continue;
        }

        if (character->idle) {

            int targetX = 0;
            int targetY = 0;

            // Any dead people in the same room?

            Character *corpseInRoom = NULL;

            std::vector<Character *>::iterator itOthers;
            for (itOthers = characters.begin(); itOthers < characters.end(); ++itOthers) {
                Character *other = (Character *) *itOthers;
                if (other->currentRoom == character->currentRoom && other->dead) {
                    corpseInRoom = other;
                }
            }

            // Does the character want to chat?
            bool wantsToTalk = rand() % 2 == 0;

            if (corpseFound && (character->murderTarget == NULL || (character->murderTarget != NULL && character->carryingWeapon == NULL))) {

                targetX = victim->position.x;
                targetY = victim->position.y;

                character->currentTarget = NULL;

            } else if (corpseInRoom != NULL && character->murderTarget == NULL) {

                // Go look at the corpse

                targetX = corpseInRoom->position.x;
                targetY = corpseInRoom->position.y;

                character->currentTarget = NULL;

            } else if (character->murderTarget != NULL && !character->murderTarget->dead && character->carryingWeapon != NULL && character->timeBeforeTryMurder == 0) {

                // Murderer goes after its target if he has a weapon and the target is
                // not dead yet

                targetX = character->murderTarget->position.x;
                targetY = character->murderTarget->position.y;

                character->currentTarget = NULL;

            } else if (wantsToTalk) {

                // Goes after another character

                int idxOption = rand() % characters.size();
                Character *option = characters[idxOption];
                targetX = option->position.x;
                targetY = option->position.y;

                character->currentTarget = NULL;

            } else {

                // Looks for a POI matching the characters interest in any room

                Room *room;
                std::vector<POI *> points;

                bool notSearchedOnly = false;

                Interest interest = character->interest;
                if (character->murderTarget != NULL && ((character->carryingWeapon == NULL && character->timeBeforeSearchWeapon == 0 && !character->murderTarget->dead) || (character->carryingWeapon != NULL && character->murderTarget->dead))) {

                    // If the character is a murderer and is looking for a weapon, or
                    // if he/she already killed the target, he/she goes to a container
                    // to grab/hide the weapon

                    if (character->carryingWeapon != NULL) {
                        interest = InterestContainerConceiled;
                    } else {
                        interest = InterestContainerVisible;
                    }

                    // If he has no weapon, searches only the containers not
                    // searched yet

                    notSearchedOnly = character->carryingWeapon == NULL;

                } else {

                    // There's a chance the character wants to see a container
                    int chance = rand() % 100;

                    if (chance < 25) {
                        interest = InterestContainerVisible;
                    } else if (chance < 50) {
                        interest = InterestContainerConceiled;
                    }
                }

                do {
                    int roomIdx = rand() % rooms.size();
                    room = rooms[roomIdx];

                    points = room->getPointsOfInterest(interest, notSearchedOnly);

                } while (points.size() == 0);

                // Goes after an interesting random POI

                int idx = rand() % points.size();
                POI *poi = points[idx];

                character->currentTarget = poi;

                targetX = poi->position.x;
                targetY = poi->position.y;
            }

            character->idle = false;

            MapSearchNode nodeStart;
            nodeStart.x = (int) character->position.x;
            nodeStart.y = (int) character->position.y;
            nodeStart.mystery = this;

            MapSearchNode nodeEnd;
            nodeEnd.x = targetX;
            nodeEnd.y = targetY;
            nodeEnd.mystery = this;

            astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);

            unsigned int SearchState;

            do {

                SearchState = astarsearch.SearchStep();

            } while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

            // Found a path to the target

            if (SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED) {

                MapSearchNode *node = astarsearch.GetSolutionStart();

                while (node) {

                    // Builds the steps for the character to reach its target

                    Step *step = new Step();
                    step->position = pointMake(node->x, node->y);
                    step->conversationWith = NULL;
                    step->type = StepTypeWalk;
                    step->duration = STEP_DURATION;

                    if (character->currentTarget != NULL && pointEqualsIntegral(step->position, character->currentTarget->position)) {

                        Step *step = new Step();
                        step->position = pointMake(node->x, node->y);
                        step->conversationWith = NULL;
                        step->type = StepTypeStartInteractPOI;
                        step->duration = 1;

                        character->addStep(step);

                        step = new Step();
                        step->position = pointMake(node->x, node->y);
                        step->conversationWith = NULL;
                        step->type = StepTypeInteractPOI;
                        step->duration = rand() % MAX_DURATION_POI_INTERACTION + MIN_DURATION_POI_INTERACTION;

                        character->addStep(step);

                        step = new Step();
                        step->position = pointMake(node->x, node->y);
                        step->conversationWith = NULL;
                        step->type = StepTypeEndInteractPOI;
                        step->duration = 1;

                        character->addStep(step);

                    } else {

                        Step *step = new Step();
                        step->position = pointMake(node->x, node->y);
                        step->conversationWith = NULL;
                        step->type = StepTypeWalk;
                        step->duration = STEP_DURATION;

                        character->addStep(step);
                    }

                    node = astarsearch.GetSolutionNext();
                }

                astarsearch.FreeSolutionNodes();
            }

        } else {

            // Updates the character's path by 1 unit of time

            character->updatePath();

            if (!character->isHavingConversation() && character->conversationInterval > 0) {
                character->conversationInterval--;
            }

            if (character->timeBeforeSearchWeapon > 0) {
                character->timeBeforeSearchWeapon--;
            } else {

                if (character->timeBeforeTryMurder > 0 && character->carryingWeapon != NULL) {
                    character->timeBeforeTryMurder--;
                }
            }

            Room *currentRoom = NULL;

            // Determines if the character entered or left a room

            std::vector<Room *>::iterator itRooms;
            for (itRooms = rooms.begin(); itRooms < rooms.end(); ++itRooms) {
                Room *room = (Room *) *itRooms;

                if (rectContainsPoint(room->bounds, character->position)) {
                    currentRoom = room;
                }
            }

            if (currentRoom != character->currentRoom) {

                if (character->currentRoom != NULL) {
                    printf("%s left %s\n", character->name.c_str(), character->currentRoom->name.c_str());
                    registerEventForAllInRoom(EventLeftRoom, character, NULL, character->currentRoom, NULL, NULL);
                }

                character->currentRoom = currentRoom;

                if (character->currentRoom != NULL) {
                    printf("%s entered %s\n", character->name.c_str(), character->currentRoom->name.c_str());
                    registerEventForAllInRoom(EventEnteredRoom, character, NULL, character->currentRoom, NULL, NULL);

                    if (character != murderer) {
                        // Register any weapons saw before but missing
                        std::vector<Memory *>::iterator it;
                        std::vector<Memory *> memories = character->getMemories();
                        std::vector<Memory *> considered;

                        for (it = memories.begin(); it < memories.end(); ++it) {

                            Memory *memory = (Memory *) *it;

                            if (memory->event == EventSawWeapon && memory->where == character->currentRoom && memory->when < time && !pointEqualsIntegral(memory->whatInside->position, memory->what->visualPosition)) {

                                std::vector<Memory *>::iterator itOther;

                                bool found = false;

                                for (itOther = considered.begin(); itOther < considered.end(); ++itOther) {

                                    Memory *other = (Memory *) *itOther;

                                    if (other->where == memory->where && other->whatInside == memory->whatInside) {
                                        found = true;
                                        break;
                                    }
                                }

                                if (!found) {
                                    registerEventFor(character, EventWeaponMissing, NULL, NULL, character->currentRoom, memory->what, memory->whatInside);
                                    considered.push_back(memory);
                                }
                            }
                        }
                    }

                    // Register visible weapons the character sees in the room
                    std::vector<POI *>::iterator itPOI;
                    for (itPOI = character->currentRoom->pointsOfInterest.begin(); itPOI < character->currentRoom->pointsOfInterest.end(); ++itPOI) {

                        POI *poi = (POI *) *itPOI;

                        if (poi->interest == InterestContainerVisible && poi->contents != NULL && poi->contents->isWeapon()) {
                            registerEventFor(character, EventSawWeapon, NULL, NULL, character->currentRoom, poi, poi->contents);
                        }
                    }

                    std::vector<Character *>::iterator itOthers;
                    for (itOthers = characters.begin(); itOthers < characters.end(); ++itOthers) {
                        Character *other = (Character *) *itOthers;

                        if (other != character && other->currentRoom == character->currentRoom) {

                            registerEventFor(character, EventWasInRoom, other, NULL, character->currentRoom, NULL, NULL);

                            // Register others having conversations
                            if (other->isHavingConversation()) {
                                Character *another = other->getCurrentStep()->conversationWith;
                                registerEventFor(character, EventWasHavingConversation, other, another, character->currentRoom, NULL, NULL);
                            }

                            // Register others interacting with POIs
                            if (other->isInteractingWithPOI()) {
                                registerEventFor(character, EventWasInteractingPOI, other, NULL, character->currentRoom, other->currentTarget, NULL);
                            }
                        }
                    }
                }
            }

            if (character->getCurrentStep() != NULL) {

                Character *another;

                switch (character->getCurrentStep()->type) {
                case StepTypeStartInteractPOI:
                    registerEventForAllInRoom(EventStartInteractPOI, character, NULL, character->currentRoom, character->currentTarget, NULL);
                    break;
                case StepTypeEndInteractPOI:
                    another = character->getCurrentStep()->conversationWith;
                    registerEventForAllInRoom(EventEndInteractPOI, character, another, character->currentRoom, character->currentTarget, NULL);
                    break;
                case StepTypeEndConversation:
                    another = character->getCurrentStep()->conversationWith;
                    registerEventForAllInRoom(EventEndConversation, character, another, character->currentRoom, NULL, NULL);
                    break;
                default:
                    break;
                }
            }

            bool aloneInRoom = true;
            bool aloneInRoomWithVictim = true;
            bool allNearCorpse = pointEqualsIntegral(character->position, victim->position);

            std::vector<Character *>::iterator itOthers;
            for (itOthers = characters.begin(); itOthers < characters.end(); ++itOthers) {
                Character *other = (Character *) *itOthers;

                allNearCorpse = allNearCorpse && pointEqualsIntegral(other->position, victim->position);

                if (other != character) {
                    if (character->currentRoom == other->currentRoom) {
                        aloneInRoom = false;

                        if (other != character->murderTarget) {
                            aloneInRoomWithVictim = false;
                        }
                    }

                    if (victim->dead && other == victim && pointEqualsIntegral(character->position, other->position) && !corpseFound) {

                        printf("*** %s found %s's body in the %s ***\n", character->name.c_str(), victim->name.c_str(), character->currentRoom->name.c_str());
                        corpseFound = true;
                        corpseFoundTime = time;
                        corpseFoundRoom = character->currentRoom;
                        character->clearPath();

                        std::vector<Character *>::iterator itAll;
                        for (itAll = characters.begin(); itAll < characters.end(); ++itAll) {
                            registerEventFor(*itAll, EventFoundBody, character, NULL, corpseFoundRoom, NULL, NULL);
                        }
                    }

                    // Looks for:
                    // - adjacent characters
                    // - one of them without a POI in mind
                    // - both not already in a conversation
                    // - both conversation intervals expired

                    if (pointAdjacentIntegral(character->position, other->position) &&
                            (character->currentTarget == NULL || other->currentTarget == NULL) &&
                            (!character->isHavingConversation() && !other->isHavingConversation()) &&
                            (character->conversationInterval == 0 && other->conversationInterval == 0) && !corpseFound) {

                        int duration = rand() % MAX_DURATION_CONVERSATION + MIN_DURATION_CONVERSATION;

                        // If both characters have matching interests, the talk is longer

                        if (character->interest == other->interest) {
                            duration *= CONVERSATION_INTEREST_FACTOR;
                        }

                        character->clearPath();
                        other->clearPath();

                        Step *step = new Step();
                        step->position = character->position;
                        step->duration = duration;
                        step->conversationWith = other;
                        step->type = StepTypeConversation;
                        character->addStep(step);

                        step = new Step();
                        step->position = character->position;
                        step->duration = 1;
                        step->conversationWith = other;
                        step->type = StepTypeEndConversation;
                        character->addStep(step);

                        step = new Step();
                        step->position = other->position;
                        step->duration = duration;
                        step->conversationWith = character;
                        step->type = StepTypeConversation;
                        other->addStep(step);

                        step = new Step();
                        step->position = other->position;
                        step->duration = 1;
                        step->conversationWith = character;
                        step->type = StepTypeEndConversation;
                        other->addStep(step);

                        character->conversationInterval = CONVERSATION_INTERVAL;
                        other->conversationInterval = CONVERSATION_INTERVAL;

                        registerEventForAllInRoom(EventStartConversation, character, other, character->currentRoom, NULL, NULL);
                        registerEventForAllInRoom(EventStartConversation, other, character, character->currentRoom, NULL, NULL);

                        printf("%s and %s are having a conversation\n", character->name.c_str(), other->name.c_str());

                    }
                }
            }

            if (allNearCorpse && corpseFound) {
                ended = true;
                printf("*** Mystery finished ***\n");
            }

            // Murderer-specific actions

            if (character->murderTarget != NULL) {

                // Grabs a weapon if:
                // - interval elapsed
                // - not carrying a weapon already
                // - murder target is not dead
                // - has a target POI
                // - reached the target POI
                // - POI has contents
                // - POI's content is a weapon matching interest
                // - alone in the room

                if (character->timeBeforeSearchWeapon == 0 &&
                        character->carryingWeapon == NULL &&
                        !victim->dead &&
                        character->currentTarget != NULL &&
                        pointEqualsIntegral(character->position, character->currentTarget->position) &&
                        character->currentTarget->contents != NULL &&
                        character->currentTarget->contents->interest == character->weaponInterest &&
                        aloneInRoom) {

                    character->carryingWeapon = character->currentTarget->contents;
                    character->carryingWeapon->position = pointMake(-20, -20);
                    character->currentTarget->contents = NULL;

                    printf("*** %s got a %s! ***\n", character->name.c_str(), character->carryingWeapon->description.c_str());
                }

                // Kills the victim if:
                // - interval elapsed
                // - already got a weapon
                // - near the victim
                // - murder target is not dead
                // - alone in the room with victim

                if (character->timeBeforeTryMurder == 0 &&
                        character->carryingWeapon != NULL &&
                        pointAdjacentIntegral(character->position, victim->position) &&
                        !victim->dead &&
                        aloneInRoomWithVictim) {

                    printf("*** %s murdered %s! ***\n", character->name.c_str(), victim->name.c_str());
                    victim->dead = true;
                    crimeWeapon = character->carryingWeapon;

                    character->clearPath();

                    // Murderer will lie

                    int idx = rand() % character->currentRoom->pointsOfInterest.size();
                    POI *poi = character->currentRoom->pointsOfInterest[idx];

                    registerEventFor(character, EventStartInteractPOI, character, NULL, character->currentRoom, poi, NULL);
                    registerEventFor(character, EventEndInteractPOI, character, NULL, character->currentRoom, poi, NULL);
                }

                // Hides the weapon if:
                // - carrying a weapon
                // - murder target is dead
                // - has a target POI
                // - reached the target POI
                // - POI has no contents
                // - alone in the room

                if (character->carryingWeapon != NULL &&
                        victim->dead &&
                        character->currentTarget != NULL &&
                        pointEqualsIntegral(character->position, character->currentTarget->position) &&
                        character->currentTarget->contents == NULL &&
                        aloneInRoom) {

                    character->carryingWeapon->position = character->currentTarget->visualPosition;

                    if (character->currentTarget->interest == InterestContainerConceiled) {
                        character->carryingWeapon->position = pointMake(-20, -20);
                    }

                    character->currentTarget->contents = character->carryingWeapon;
                    character->carryingWeapon = NULL;

                    printf("*** %s hid the %s in the %s, in the %s! ***\n", character->name.c_str(), character->currentTarget->contents->description.c_str(), character->currentTarget->description.c_str(), currentRoom->name.c_str());
                }
            }
        }
    }

    astarsearch.EnsureMemoryFreed();

    time++;
}
Example #12
0
double Navigation::getPath(RobotCommand rc, QList<Vector2D> *points)
{
    Q_UNUSED(points);
    if(rc.useNav == false)
        return wm->ourRobot[id].pos.loc.dist(rc.fin_pos.loc);

    AStarSearch<MapSearchNode> astarsearch;
    MapSearchNode::wm = wm;
    MapSearchNode::isBallObs = rc.isBallObs;
    MapSearchNode::isKickObs = rc.isKickObs;
    MapSearchNode::selfRobot = id;

    MapSearchNode nodeStart;
    MapSearchNode nodeEnd;

    nodeStart.vec = wm->ourRobot[id].pos.loc;

    auto obs = MapSearchNode::getObsCircle();
    bool is_fin_obs = false;
    for(int i=0; i<obs.size(); i++)
    {
        if(obs[i].contains(rc.fin_pos.loc))
        {
            Circle2D c(obs[i].center(), obs[i].radius() + BALL_RADIUS*3);
            if(rc.fin_pos.loc == obs[i].center()) rc.fin_pos.loc += Vector2D(-1,0);
            Line2D l(obs[i].center(), rc.fin_pos.loc);
            Vector2D ans1, ans2;
            int ans = c.intersection(l, &ans1, &ans2);
            switch (ans)
            {
            case 0:
                qDebug() << "NAV 0 !!!";
                //qDebug() << c.center().x << c.center().y << c.radius();
                //qDebug() << l.a() << l.b() << l.c();
                //qDebug() << wm->ball.pos.loc.x << wm->ball.pos.loc.y;
                //qDebug() << rc.fin_pos.loc.x << rc.fin_pos.loc.y;
                break;
            case 1:
                //qDebug() << "1";
                nodeEnd.vec = ans1;
                is_fin_obs = true;
                break;
            case 2:
                //qDebug() << "2";
                if(ans1.dist2(rc.fin_pos.loc) < ans2.dist2(rc.fin_pos.loc))
                    nodeEnd.vec = ans1;
                else
                    nodeEnd.vec = ans2;
                is_fin_obs = true;
                break;
            default:
                qDebug() << "DEF";
                break;
            }

            //qDebug() << "FIN_POS chaned" << rc.fin_pos.loc.x << rc.fin_pos.loc.y << nodeEnd.vec.x << nodeEnd.vec.y;
            break;
        }
    }
    if(!is_fin_obs) nodeEnd.vec = rc.fin_pos.loc;

    //qDebug()<< "----- NAV START -----";
    //qDebug()<< "start" << nodeStart.vec.x << nodeStart.vec.y;
    //qDebug()<< "end  " << nodeEnd.vec.x << nodeEnd.vec.y;

    astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);

    unsigned int SearchState;
    unsigned int SearchSteps = 0;

    do
    {
        //qDebug() << "SearchStep" << SearchSteps;
        SearchState = astarsearch.SearchStep();
        SearchSteps++;
    }
    while(SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING);

    double pathL = 0;
    if(SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED)
    {
        //qDebug() << "AStarSearch SUCCEEDED";
        MapSearchNode *node = astarsearch.GetSolutionStart();
        //qDebug() << "SolutionStart" << node->vec.x << node->vec.y;
        int steps = 0;
        if(points) points->append(node->vec); //nodeStart
        Vector2D lastNode = node->vec;
        for(;;)
        {
            node = astarsearch.GetSolutionNext();
            if(!node) break;
            //qDebug() << "SolutionNext" << node->vec.x << node->vec.y;
            if(points) points->append(node->vec);
            steps++;
            pathL += (lastNode - node->vec).length();
            lastNode = node->vec;
        }
        astarsearch.FreeSolutionNodes();
    }
    else if(SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED)
    {
        qDebug() << "AStarSearch FAILED" << id;
    }
    astarsearch.EnsureMemoryFreed();

    //qDebug()<< "----- NAV END -----";
    return pathL; //path length
}
Example #13
0
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;
}