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