void DijkstraSP::construct() {
assert(spTreeVertices.empty());
assert(parentTo.empty());
auto const numVertices = g.adj.size();
std::vector<int> openNodes(numVertices, -1);
std::vector<int> nodeKeys(numVertices, LARGE_INT);
std::vector<int> currentParentTo(numVertices, -1);
std::iota(openNodes.begin(), openNodes.end(), 0);
nodeKeys[0] = -LARGE_INT;
auto comp = [&nodeKeys](const int v, const int w) {return nodeKeys[v] > nodeKeys[w]; };
while (!openNodes.empty()){
std::make_heap(openNodes.begin(), openNodes.end(), comp);
// pop min
auto minPQNode = openNodes.front();
std::pop_heap(openNodes.begin(), openNodes.end(), comp);
openNodes.pop_back();
spTreeVertices.push_back(minPQNode);
parentTo.push_back(currentParentTo[minPQNode]);
updatePQFromNode(minPQNode, openNodes, nodeKeys, currentParentTo);
}
}
bool AStar::find_path(const std::shared_ptr<const ego_mesh_t>& mesh, uint32_t stoppedby, const int src_ix, const int src_iy, int dst_ix, int dst_iy)
{
/// @author ZF
/// @details Explores up to MAX_ASTAR_NODES number of nodes to find a path between the source coordinates and destination coordinates.
// The result is stored in a node list and can be accessed through AStar_get_path(). Returns false if no path was found.
float weight;
// do not start if the initial point is off the mesh
if (Index1D::Invalid == mesh->getTileIndex(Index2D(src_ix, src_iy)))
{
#ifdef DEBUG_ASTAR
printf("AStar failed because source position is off the mesh.\n");
#endif
return false;
}
struct Offset
{
Offset(int setX, int setY) : x(setX), y(setY) {}
int x;
int y;
};
//Explore all nearby nodes, including diagonal ones
static const std::array<Offset, 8> EXPLORE_NODES = {
Offset(-1, -1), Offset(-1, 0), Offset(-1, 1),
Offset(0, -1), Offset(1, -1), Offset(1, 1),
Offset(1, 0), Offset(0, 1)
};
//Node sorting algorithm (lowest weight first)
auto comparator = [](const std::shared_ptr<Node> &first, const std::shared_ptr<Node> &second){
return first->weight < second->weight;
};
//Set of closed nodes
std::unordered_set<int> closedNodes;
std::priority_queue<std::shared_ptr<Node>,
std::vector<std::shared_ptr<Node>>,
decltype(comparator)> openNodes(comparator);
//be a bit flexible if the destination is inside a wall
if (mesh->tile_has_bits(Index2D(dst_ix, dst_iy), stoppedby))
{
bool foundOpenSpace = false;
//check all tiles edging to this one, including corners
for(const auto& offset: EXPLORE_NODES) {
//Did we find a free tile?
if (!mesh->tile_has_bits(Index2D(dst_ix + offset.x, dst_iy + offset.y), stoppedby))
{
dst_ix = dst_ix + offset.x;
dst_iy = dst_iy + offset.y;
foundOpenSpace = true;
break;
}
}
if(!foundOpenSpace) {
#ifdef DEBUG_ASTAR
printf("AStar failed because goal position is impassable (and no nearby non-impassable tile found).\n");
#endif
return false;
}
}
// restart the algorithm
reset();
// initialize the starting node
weight = Distance()(src_ix, src_iy, dst_ix, dst_iy);
start_node = std::make_shared<AStar::Node>(src_ix, src_iy, weight, nullptr);
openNodes.push(start_node);
// do the algorithm
while (!openNodes.empty())
{
// list is completely full... we failed
if (closedNodes.size() >= MAX_ASTAR_NODES) break;
//Get the cheapest open node
std::shared_ptr<Node> currentNode = openNodes.top();
openNodes.pop();
// find some child nodes
for(const auto& offset: EXPLORE_NODES) {
// do not check diagonals
if (offset.x != 0 && offset.y != 0) continue;
//The node to explore
int tmp_x = currentNode->ix + offset.x;
int tmp_y = currentNode->iy + offset.y;
//Do not explore any node more than once
if(closedNodes.find(tmp_x | tmp_y << 16) != closedNodes.end()) {
continue;
}
closedNodes.insert(tmp_x | tmp_y << 16);
// check for the simplest case, is this the destination node?
if (tmp_x == dst_ix && tmp_y == dst_iy)
{
weight = Distance()(tmp_x, tmp_y, currentNode->ix, currentNode->iy);
final_node = std::make_shared<AStar::Node>(tmp_x, tmp_y, weight, currentNode);
return true;
}
// is the test node on the mesh?
Index1D itile = mesh->getTileIndex(Index2D(tmp_x, tmp_y));
if (Index1D::Invalid == itile)
{
continue;
}
//Dont walk into pits
//@todo: might need to check tile Z level here instead
const ego_tile_info_t& ptile = mesh->getTileInfo(itile);
if (ptile.isFanOff())
{
// add the invalid tile to the list as a closed tile
continue;
}
// is this a wall or impassable?
if (mesh->tile_has_bits(Index2D(tmp_x, tmp_y), stoppedby))
{
// add the invalid tile to the list as a closed tile
continue;
}
///
/// @todo I need to check for collisions with static objects, like trees
// OK. determine the weight (F + H)
weight = Distance()(tmp_x, tmp_y, currentNode->ix, currentNode->iy)
+ Distance()(tmp_x, tmp_y, dst_ix, dst_iy);
openNodes.push(std::make_shared<AStar::Node>(tmp_x, tmp_y, weight, currentNode));
}
}
#ifdef DEBUG_ASTAR
if (closedNodes.size() >= MAX_ASTAR_NODES) printf("AStar failed because maximum number of nodes were explored (%d)\n", MAX_ASTAR_NODES);
#endif
return false;
}
