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