static State
astar(const State& currentState, Function expand)
{
typedef priority_queue <State, vector<State>, bool(*)(State,State)>
fringe_t;
unordered_set<State> closed_set;
fringe_t fringe(compare_cost);
fringe.push(currentState);
while (!fringe.empty())
{
State nextState = fringe.top();
fringe.pop();
// clog << nextState << endl;
if (nextState.is_terminal)
return nextState;
if (closed_set.find(nextState) != closed_set.end())
continue;
closed_set.insert(nextState);
auto children = expand(nextState);
for (auto& state : children)
fringe.push(state);
}
return {};
}
void mbp_dijkstra_heap(graph<key_type,graph_size> &g0, int s, int t, vector< edge<key_type> > &path)
{
struct adj_node<key_type> table[graph_size + 1];
short v_flag[graph_size + 1];
int v_parent[graph_size + 1];
key_type v_parent_weight[graph_size + 1];
heap<vertex<key_type>,graph_size,value_fun<key_type>,name_fun<key_type>,key_type> fringe(true);
vertex<key_type> max,v1;
struct adj_node<key_type>* temp;
g0.get_adj_table(table);
for (int i = 1; i <= graph_size ; i++) v_flag[i] = UNSEEN;
v1 = g0.get_v(s);
v1.set_key(MAX_WEIGHT + 1);
v_flag[s] = FRINGE;
fringe.insert(v1);
while(fringe.size())
{
max = fringe.max();
if (max.get_name() == t) break;
for(temp = table[max.get_name()].adj_v;temp!=NULL;temp = temp->adj_v)
{
if (v_flag[temp->name] != INTREE)
{
if (v_flag[temp->name] == UNSEEN) fringe.insert(g0.get_v(temp->name));
v_flag[temp->name] = FRINGE;
if ((temp->weight > (fringe.index(fringe.get_index(temp->name))).get_key() )
&& (max.get_key() > (fringe.index(fringe.get_index(temp->name))).get_key() ) )
{
v_parent[temp->name] = max.get_name();
v_parent_weight[temp->name] = temp->weight;
//Relax
v1 = fringe.index(fringe.get_index(temp->name));
v1.set_key(std::min(max.get_key(),temp->weight));
fringe.modify_at(fringe.get_index(temp->name),v1);
}
}
}
v_flag[max.get_name()] = INTREE;
fringe.del_max();
}
int i = t;
edge<key_type> edge_here;
while( i != s)
{
edge_here = edge<key_type>(v_parent[i],i,v_parent_weight[i]);
path.push_back(edge_here);
i = v_parent[i];
}
return;
}
std::list<Tile *> TileMap::findShortestPath(Vec3<int> origin, Vec3<int> destination,
unsigned int iterationLimit,
const CanEnterTileHelper &canEnterTile, float)
{
TRACE_FN;
PathNodeComparer c;
std::unordered_map<Tile *, PathNode> visitedTiles;
std::set<PathNode, PathNodeComparer> fringe(c);
Vec3<float> goalPosition;
unsigned int iterationCount = 0;
LogInfo("Trying to route from {%d,%d,%d} to {%d,%d,%d}", origin.x, origin.y, origin.z,
destination.x, destination.y, destination.z);
if (origin.x < 0 || origin.x >= this->size.x || origin.y < 0 || origin.y >= this->size.y ||
origin.z < 0 || origin.z >= this->size.z)
{
LogError("Bad origin {%d,%d,%d}", origin.x, origin.y, origin.z);
return {};
}
if (destination.x < 0 || destination.x >= this->size.x || destination.y < 0 ||
destination.y >= this->size.y || destination.z < 0 || destination.z >= this->size.z)
{
LogError("Bad destination {%d,%d,%d}", destination.x, destination.y, destination.z);
return {};
}
goalPosition = {destination.x, destination.y, destination.z};
Tile *goalTile = this->getTile(destination);
if (!goalTile)
{
LogError("Failed to get destination tile at {%d,%d,%d}", destination.x, destination.y,
destination.z);
return {};
}
Tile *startTile = this->getTile(origin);
if (!startTile)
{
LogError("Failed to get origin tile at {%d,%d,%d}", origin.x, origin.y, origin.z);
return {};
}
if (origin == destination)
{
LogInfo("Destination == origin {%d,%d,%d}", destination.x, destination.y, destination.z);
return {goalTile};
}
PathNode startNode(0.0f, nullptr, startTile, goalPosition);
fringe.emplace(startNode);
visitedTiles.emplace(startTile, startNode);
auto closestNodeSoFar = *fringe.begin();
while (iterationCount++ < iterationLimit)
{
auto first = fringe.begin();
if (first == fringe.end())
{
LogInfo("No more tiles to expand after %d iterations", iterationCount);
return {};
}
auto nodeToExpand = *first;
fringe.erase(first);
// Make it so we always try to move at least one tile
if (closestNodeSoFar.parentTile == nullptr)
closestNodeSoFar = nodeToExpand;
Vec3<int> currentPosition = nodeToExpand.thisTile->position;
if (currentPosition == destination)
return getPathToNode(visitedTiles, nodeToExpand);
if (nodeToExpand.distanceToGoal < closestNodeSoFar.distanceToGoal)
{
closestNodeSoFar = nodeToExpand;
}
for (int z = -1; z <= 1; z++)
{
for (int y = -1; y <= 1; y++)
{
for (int x = -1; x <= 1; x++)
{
if (x == 0 && y == 0 && z == 0)
{
continue;
}
auto nextPosition = currentPosition;
nextPosition.x += x;
nextPosition.y += y;
nextPosition.z += z;
if (!tileIsValid(nextPosition))
continue;
Tile *tile = this->getTile(nextPosition);
// If Skip if we've already expanded this, as in a 3d-grid we know the first
// expansion will be the shortest route
if (visitedTiles.find(tile) != visitedTiles.end())
continue;
// FIXME: Make 'blocked' tiles cleverer (e.g. don't plan around objects that
// will
// move anyway?)
if (!canEnterTile.canEnterTile(nodeToExpand.thisTile, tile))
continue;
// FIXME: The old code *tried* to disallow diagonal paths that would clip past
// scenery but it didn't seem to work, no we should re-add that here
float newNodeCost = nodeToExpand.costToGetHere;
newNodeCost +=
glm::length(Vec3<float>{nextPosition} - Vec3<float>{currentPosition});
// make pathfinder biased towards vehicle's altitude preference
newNodeCost += canEnterTile.adjustCost(nextPosition, z);
PathNode newNode(newNodeCost, nodeToExpand.thisTile, tile, goalPosition);
visitedTiles.emplace(tile, newNode);
fringe.emplace(newNode);
}
}
}
}
LogInfo("No route found after %d iterations, returning closest path {%d,%d,%d}", iterationCount,
closestNodeSoFar.thisTile->position.x, closestNodeSoFar.thisTile->position.y,
closestNodeSoFar.thisTile->position.z);
return getPathToNode(visitedTiles, closestNodeSoFar);
}
