int ExeTask::execute()
{
m_stop = false;
m_running = true;
m_joining = false;
nodeStart();
this->create_thr(RThread::ATTACH, getNodeName());
if(m_runner->isDebugMode()||m_runner->isMonitorMode()){
this->sendThreadInfo("new");
m_runner->addCondMu4BP(getThreadNum());
m_isDbgCondCreated = true;
}
return 0;
}
void main()
{
pf::Map map = generateMap();
pf::LPNode nodeStart(new pf::Node(map.getField(0, 3)));
pf::LPNode nodeStop (new pf::Node(map.getField(9, 3)));
std::cout << "before" << std::endl;
pf::PathFinding::print(map, nodeStart, nodeStop, pf::NodeList());
pf::NodeList path = pf::PathFinding::search(map, nodeStart, nodeStop);
std::cout << std::endl << "after" << std::endl;
pf::PathFinding::print(map, nodeStart, nodeStop, path);
std::cout << std::endl;
}
int PathSearch(void* data)
{
std::vector<Vector2D> result;
auto enemy = static_cast<Enemy*>(data);
auto params = enemy->GetPathSearchParams();
auto mapData = enemy->GetMap()->GetMapForAStarSearch();
unsigned int SearchCount = 0;
const unsigned int NumSearches = 1;
AStarSearch<MapSearchNode> m_astarsearch;
///////////////////////////////////////////
/*ofstream outputFile;
outputFile.open("mapData.txt");
int idx = 0;
for (int i=0; i<params.mapWidth*params.mapHeight; i++)
{
outputFile << mapData[i];
if((i+1)%params.mapWidth == 0)
{
outputFile << endl;
}
}
outputFile.close();*/
///////////////////////////////////////////
while(SearchCount < NumSearches)
{
// Create a start state
MapSearchNode nodeStart(params.start.x, params.start.y, params.mapWidth, params.mapHeight, mapData);
//nodeStart.SetMap(params.mapWidth, params.mapHeight, mapData);
//nodeStart.x = params.start.x;
//nodeStart.y = params.start.y;
// Define the goal state
MapSearchNode nodeEnd(params.end.x, params.end.y, params.mapWidth, params.mapHeight, mapData);
//nodeEnd.SetMap(params.mapWidth, params.mapHeight, mapData);
//nodeEnd.x = params.end.x;
//nodeEnd.y = params.end.y;
// Set Start and goal states
m_astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);
unsigned int SearchState;
unsigned int SearchSteps = 0;
do
{
SearchState = m_astarsearch.SearchStep();
SearchSteps++;
}
while(SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING);
if(SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED)
{
MapSearchNode* node = m_astarsearch.GetSolutionStart();
int steps = 0;
//node->PrintNodeInfo();
result.push_back(node->GetNodeInfo());
for(;;)
{
node = m_astarsearch.GetSolutionNext();
if(!node)
{
break;
}
//node->PrintNodeInfo();
result.push_back(node->GetNodeInfo());
steps ++;
};
// Once you're done with the solution you can free the nodes up
m_astarsearch.FreeSolutionNodes();
}
SearchCount++;
m_astarsearch.EnsureMemoryFreed();
}
enemy->SetPathSearchResult(result);
return 0;
}
void calculateAStar(int x1, int y1, int x2, int y2, int xMin, int yMin, int xMax, int yMax, int map[][9], std::list<MGAStarNode> &path)
{
std::list<MGAStarNode> open;
std::list<MGAStarNode> closed;
MGAStarNode nodeGoal(x2, y2, 0.0);
MGAStarNode nodeStart(x1, y1, nodeGoal, 0.0);
open.push_back(nodeStart);
int passes = 0;
int openDeletions = 0;
int closedDeletions = 0;
int openAdditions = 0;
int successorDeletions = 0;
int closedAdditions = 0;
int successorAdditions = 0;
int openLookups = 0;
int closedLookups = 0;
int successorLookups = 0;
bool pathWasFound = false;
while(!open.empty())
{
passes++;
// Take the node with smallest F from open list and call it nodeCurrent
std::list<MGAStarNode>::iterator nodeCurrentIt = open.begin();
openLookups++;
for(std::list<MGAStarNode>::iterator nodeIt = open.begin(); nodeIt != open.end(); nodeIt++)
{
if((*nodeIt).getF() < (*nodeCurrentIt).getF())
{
nodeCurrentIt = nodeIt;
}
}
printf("Best open = (%d, %d): %f\n", (*nodeCurrentIt).getX(), (*nodeCurrentIt).getY(), (*nodeCurrentIt).getF());
MGAStarNode nodeCurrent(*nodeCurrentIt);
openDeletions++;
open.erase(nodeCurrentIt);
if(nodeCurrent == nodeGoal)
{
pathWasFound = true;
closedAdditions++;
closed.push_back(nodeCurrent);
printf( "Path found (%d,%d) -> (%d,%d) in %d passes\n"
"%d open deletions, %d closed deletions, %d successor deletions, "
"%d open additions, %d closed additions, %d successor additions, "
"%d open lookups, %d closed lookups, %d successor lookups\n",
nodeStart.getX(), nodeStart.getY(), nodeGoal.getX(), nodeGoal.getY(), passes,
openDeletions, closedDeletions, successorDeletions,
openAdditions, closedAdditions, successorAdditions,
openLookups, closedLookups, successorLookups);
for(std::list<MGAStarNode>::iterator nodeIt = closed.begin(); nodeIt != closed.end(); nodeIt++)
{
printf("(%d,%d) <- (%d,%d), ", (*nodeIt).getX(), (*nodeIt).getY(), (*nodeIt).getParentX(), (*nodeIt).getParentY());
}
printf("DONE\n");
//Now, save the actual path in @path. Start from $closed(last) and follow it to @nodeStart
if(!closed.empty())
{
MGAStarNode pathEnt = closed.back();
int lastParentX = closed.back().getParentX();
int lastParentY = closed.back().getParentY();
path.push_front(MGAStarNode(pathEnt));
while(!(pathEnt == nodeStart))
{
if(pathEnt.getX() == lastParentX && pathEnt.getY() == lastParentY)
{
path.push_front(MGAStarNode(pathEnt));
lastParentX = closed.back().getParentX();
lastParentY = closed.back().getParentY();
}
closed.pop_back();
pathEnt = closed.back();
}
}
// Handle the special case where nodeStart equals nodeGoal
if(!path.empty() && (*path.begin()) == nodeStart)
{
path.clear();
}
break;
}
// Generate successors (neighbors)
std::list<MGAStarNode> potentialSuccessors;
std::list<MGAStarNode> successors;
int x = nodeCurrent.getX();
int y = nodeCurrent.getY();
potentialSuccessors.push_back(MGAStarNode(x + 1, y + 1, nodeGoal, nodeCurrent.getG() + sqrt_2));
potentialSuccessors.push_back(MGAStarNode(x + 1, y, nodeGoal, nodeCurrent.getG() + 1.0));
potentialSuccessors.push_back(MGAStarNode(x, y + 1, nodeGoal, nodeCurrent.getG() + 1.0));
potentialSuccessors.push_back(MGAStarNode(x - 1, y - 1, nodeGoal, nodeCurrent.getG() + sqrt_2));
potentialSuccessors.push_back(MGAStarNode(x - 1, y, nodeGoal, nodeCurrent.getG() + 1.0));
potentialSuccessors.push_back(MGAStarNode(x, y - 1, nodeGoal, nodeCurrent.getG() + 1.0));
potentialSuccessors.push_back(MGAStarNode(x + 1, y - 1, nodeGoal, nodeCurrent.getG() + sqrt_2));
potentialSuccessors.push_back(MGAStarNode(x - 1, y + 1, nodeGoal, nodeCurrent.getG() + sqrt_2));
for(std::list<MGAStarNode>::iterator potSuccIt = potentialSuccessors.begin(); potSuccIt != potentialSuccessors.end(); potSuccIt++)
{
if( (*potSuccIt).getX() >= xMin &&
(*potSuccIt).getX() <= xMax &&
(*potSuccIt).getY() >= yMin &&
(*potSuccIt).getY() <= yMax &&
map[(*potSuccIt).getY()][(*potSuccIt).getX()] == 0 &&
((*potSuccIt).getX() != nodeCurrent.getParentX() || (*potSuccIt).getY() != nodeCurrent.getParentY()))
{
(*potSuccIt).setH((*potSuccIt).heuristic(nodeGoal));
successors.push_back((*potSuccIt));
successorAdditions++;
}
}
for(std::list<MGAStarNode>::iterator nodeSuccessor = successors.begin(); nodeSuccessor != successors.end(); )
{
// Find the successor in the open list
openLookups++;
std::list<MGAStarNode>::iterator evaluateOpenIt = find(open.begin(), open.end(), *nodeSuccessor);
if(evaluateOpenIt != open.end())
{
if((*evaluateOpenIt).getF() <= (*nodeSuccessor).getF())
{
printf("\t\tKeeping (%d, %d):%f and skipping successor (%d, %d):%f\n",
(*evaluateOpenIt).getX(), (*evaluateOpenIt).getY(), (*evaluateOpenIt).getF(),
(*nodeSuccessor).getX(), (*nodeSuccessor).getY(), (*nodeSuccessor).getF());
nodeSuccessor = successors.erase(nodeSuccessor);
successorDeletions++;
continue;
}
else
{
open.erase(evaluateOpenIt);
openDeletions++;
}
}
// Find the successor in the closed list
closedLookups++;
std::list<MGAStarNode>::iterator evaluateClosedIt = find(closed.begin(), closed.end(), *nodeSuccessor);
if(evaluateClosedIt != closed.end())
{
if((*evaluateClosedIt).getF() <= (*nodeSuccessor).getF())
{
nodeSuccessor = successors.erase(nodeSuccessor);
successorDeletions++;
continue;
}
else
{
closed.erase(evaluateClosedIt);
closedDeletions++;
}
}
(*nodeSuccessor).setParent(nodeCurrent);
printf("\tAdding successor (%d, %d): %f\n", (*nodeSuccessor).getX(), (*nodeSuccessor).getY(), (*nodeSuccessor).getF());
open.push_front(*nodeSuccessor); // push_front allows the algorithm to find the most recently added neighbor first
openAdditions++;
// Step to next successor
nodeSuccessor++;
}
closed.push_back(nodeCurrent);
closedAdditions++;
}
if(!pathWasFound)
{
printf( "Path not found (%d,%d) -> (%d,%d) in %d passes\n"
"%d open deletions, %d closed deletions, %d successor deletions, "
"%d open additions, %d closed additions, %d successor additions, "
"%d open lookups, %d closed lookups, %d successor lookups\n",
nodeStart.getX(), nodeStart.getY(), nodeGoal.getX(), nodeGoal.getY(), passes,
openDeletions, closedDeletions, successorDeletions,
openAdditions, closedAdditions, successorAdditions,
openLookups, closedLookups, successorLookups);
path.clear();
}
}
