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