bool fmpAStarPathFinder::findPath(const Hexagon& start, const Hexagon& goal, std::vector<const Hexagon*>& path) { // A* implementation // assert path.clear(); // Trivial early out if (&start == &goal) { path.push_back(&start); return true; } OpenSet open; std::set<const Hexagon*> closed; // Allows for reconstruction of the path at the end. std::unordered_map<const Hexagon*, const Hexagon*> parents; OpenSetEntry startEntry = {&start, 0, heuristics(start, goal)}; open.push(startEntry); while( !open.empty() ) { OpenSetEntry currentEntry = open.top(); if (currentEntry.m_hex == &goal) { reconstructPath(start, goal, parents, path); return true; } open.pop(); closed.insert(currentEntry.m_hex); for (int i = 0 ; i < 6 ; ++i) { const Hexagon* neighbor = currentEntry.m_hex->m_neighbors[i]; if (neighbor) { auto neighborPosClosed = closed.find(neighbor); // If neighbor is not in closed if (neighborPosClosed == closed.end()) { OpenSetEntry neighborEntry = { neighbor, currentEntry.m_pastCost + 1, heuristics(*neighbor,goal) }; if (open.pushOrReplace(neighborEntry)) { parents.insert(std::pair<const Hexagon*, const Hexagon*>(neighbor, currentEntry.m_hex)); } } } } } return false; }
/** * This is a straight forward C++ implementation of Wikipedia's A* pseudo code. * As proof of this the pseudo code is embedded in the source, just above the * C++ statements. * * See http://en.wikipedia.org/wiki/A*#Pseudocode for the complete pseudo code * * But also see the discussion page: it explains why the algorithm is only * correct if the cost function is monotone.This algorithm does not give the * correct solution in our case. The routes and hence costs differ whether we * go from (0,0) to (10,10) or vice versa: * * 0-0 -> 3-2 -> 4-5 -> 4-8 -> 6-9 -> 10-10, cost = 15 * * 10-10 -> 10-7 -> 7-5 -> 6-3 -> 3-2 -> 0-0, cost = 14 * * Use this piece of code as inspiration only. * */ std::vector<Vertex> WikiAStar( Vertex start, Vertex goal) { ClosedSet closedSet; // The set of nodes already evaluated. OpenSet openSet; // The set of tentative nodes to be evaluated, initially containing the start node VertexMap predecessorMap; // The map of navigated nodes. start.actualCost = 0.0; // Cost from start along best known path. start.heuristicCost = start.actualCost + HeuristicCost(start, goal); // Estimated total cost from start to goal through y. openSet.insert(start); //while openset is not empty while(!openSet.empty()) { // current := the node in openset having the lowest f_score[] value Vertex current = *openSet.begin(); // if current = goal if(current == goal) { // return reconstruct_path(came_from, goal) return ConstructPath(predecessorMap,current); } // remove current from openset openSet.erase(current); // add current to closedset closedSet.insert(current); //for each neighbour in neighbour_nodes(current) std::vector< Vertex > neighbours = GetNeighbours(current); for(Vertex neighbour : neighbours) { // if neighbor in closedset continue (with next neighbour) if(closedSet.find(neighbour) != closedSet.end()) { continue; } // tentative_g_score := g_score[current] + dist_between(current,neighbour) double calculatedActualNeighbourCost = current.actualCost + ActualCost(current,neighbour); // if neighbour not in openset or tentative_g_score < g_score[neighbour] if(openSet.find(neighbour) == openSet.end() || calculatedActualNeighbourCost < neighbour.actualCost) { // Here we deviate from the Wikipedia article, because of the map semantics: // we cannot change the object's key values once it in the map so we first // set the vales and then put it into the map. // g_score[neighbor] := tentative_g_score neighbour.actualCost = calculatedActualNeighbourCost; // f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal) neighbour.heuristicCost = neighbour.actualCost + HeuristicCost(neighbour, goal); // came_from[neighbor] := current std::pair<VertexMap::iterator,bool> result = predecessorMap.insert(std::make_pair(neighbour,current)); // The following if-statement is not part of the pseudo code but a partial fix for a semantic difference // in the map of the pseudo code and the c++ std::map if(result.second==false) (*result.first).second = current; // if neighbor not in openset if(openSet.find(neighbour) == openSet.end()) { // add neighbor to openset openSet.insert(neighbour); } } } } //return failure return std::vector<Vertex>(); }