84 lines
2.8 KiB
C++
84 lines
2.8 KiB
C++
#include "pathfinder.h"
|
|
|
|
#include <map>
|
|
#include <queue>
|
|
#include <algorithm>
|
|
#include <set>
|
|
|
|
//#include "fiboheap.h"
|
|
|
|
PathFinder::PathFinder(){
|
|
}
|
|
|
|
std::vector<GraphNode*> PathFinder::a_star(GraphNode* start, GraphNode* goal, bool debug)
|
|
{
|
|
// Check if priorityqueue sort value in right order.
|
|
std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority> frontier = std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority>();
|
|
std::map<GraphNode*, float> cost = std::map<GraphNode*, float>(); //cost of visited node
|
|
std::map<GraphNode*, GraphNode*> pred = std::map<GraphNode*, GraphNode*>(); //pred of visited node
|
|
|
|
std::set<GraphNode*> visited = std::set<GraphNode*>();
|
|
|
|
// init frontier, cost, and pred with value for start
|
|
frontier.push(start);
|
|
cost.insert(std::pair<GraphNode*,float>(start,0));
|
|
pred.insert(std::pair<GraphNode*,GraphNode*>(start,NULL));
|
|
|
|
GraphNode* current;
|
|
while(!frontier.empty()){
|
|
//pick best element from frontier (with priority queue the best is in front)
|
|
current = frontier.top();
|
|
//current = pn->node.first;
|
|
frontier.pop();
|
|
if (visited.count(current) == 0)
|
|
visited.insert(current);
|
|
else
|
|
continue;
|
|
|
|
if(debug) std::cout << "Exploring node " << current->getValue() << std::endl;
|
|
// goal reached, end of a-star
|
|
if (current == goal){
|
|
break;
|
|
}
|
|
|
|
// for all neighbours of current node
|
|
for (GraphNode* next : current->getNeighbours()){
|
|
float new_cost = cost[current] + current->cost(next);
|
|
if(debug) std::cout << "\tExploring neighbours node " << next->getValue() << " with cost " << new_cost << std::endl;
|
|
|
|
if ((cost.count(next) == 0) || (new_cost < cost[next])){
|
|
// affect processed cost to next node in cost_map
|
|
cost[next] = new_cost;
|
|
|
|
// calcul priority of node with heuristic,and add it to frontier
|
|
float priority = new_cost + next->heuristic(goal);
|
|
if(debug) std::cout << "\t\t Priority: " << priority << std::endl;
|
|
next->setPriority(priority);
|
|
visited.erase(next);
|
|
frontier.push(next);
|
|
// memorize predecessor for next
|
|
pred[next] = current;
|
|
}
|
|
}
|
|
}
|
|
|
|
// reconstruct path by backtracking from goal to start
|
|
std::vector<GraphNode*> path = std::vector<GraphNode*>();
|
|
while(current != start){
|
|
path.push_back(current);
|
|
current = pred[current];
|
|
}
|
|
path.push_back(start);
|
|
std::reverse(path.begin(),path.end());
|
|
if(debug) std::cout << "path cost :" << cost[goal] << std::endl;
|
|
return path;
|
|
}
|
|
|
|
|
|
bool ComparePriority::operator ()(GraphNode* a, GraphNode* b)
|
|
{
|
|
return a->getPriority() > b->getPriority();
|
|
}
|
|
|
|
|