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(); }
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; }
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; }
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; }
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; }