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