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