From b2552cb662c77ccef9e979477b7739db00c3bfb1 Mon Sep 17 00:00:00 2001 From: Jeremiah Morgan Date: Mon, 14 Oct 2024 16:41:05 +0100 Subject: [PATCH] Pathfinding: Precomputeportal nodes for A Star --- libopenage/pathfinding/demo/demo_1.cpp | 1 + libopenage/pathfinding/grid.cpp | 28 +++++++++++++ libopenage/pathfinding/grid.h | 19 +++++++++ libopenage/pathfinding/pathfinder.cpp | 55 +++++++++++++++++++------- libopenage/pathfinding/pathfinder.h | 42 +++++++++++++++----- 5 files changed, 122 insertions(+), 23 deletions(-) diff --git a/libopenage/pathfinding/demo/demo_1.cpp b/libopenage/pathfinding/demo/demo_1.cpp index cd67e7b79f..1edeba52a1 100644 --- a/libopenage/pathfinding/demo/demo_1.cpp +++ b/libopenage/pathfinding/demo/demo_1.cpp @@ -92,6 +92,7 @@ void path_demo_1(const util::Path &path) { start, target, }; + grid->init_portal_nodes(); timer.start(); Path path_result = pathfinder->get_path(path_request); timer.stop(); diff --git a/libopenage/pathfinding/grid.cpp b/libopenage/pathfinding/grid.cpp index 62a2d3156f..502e86f521 100644 --- a/libopenage/pathfinding/grid.cpp +++ b/libopenage/pathfinding/grid.cpp @@ -98,4 +98,32 @@ void Grid::init_portals() { } } +const nodemap_t &Grid::get_portal_map() { + return portal_nodes; +} + +void Grid::init_portal_nodes() +{ + //create portal_nodes + for(auto& sector : this->sectors) + { + for(auto& portal : sector->get_portals()) + { + if(this->portal_nodes.find(portal->get_id()) == this->portal_nodes.end()) + { + this->portal_nodes[portal->get_id()] = std::make_shared(portal); + } + } + } + + //init portal_node exits + for(auto& sector : this->sectors) + { + for(auto& portal : sector->get_portals()) + { + this->portal_nodes[portal->get_id()]->init_exits(this->portal_nodes, sector->get_id()); + } + } +} + } // namespace openage::path diff --git a/libopenage/pathfinding/grid.h b/libopenage/pathfinding/grid.h index 8f24743433..4513bb7b99 100644 --- a/libopenage/pathfinding/grid.h +++ b/libopenage/pathfinding/grid.h @@ -7,8 +7,10 @@ #include #include +#include "pathfinding/pathfinder.h" #include "pathfinding/types.h" #include "util/vector.h" +#include "pathfinding/pathfinder.h" namespace openage::path { @@ -95,6 +97,16 @@ class Grid { */ void init_portals(); + /** + * returns map of portal ids to portal nodes + */ + const nodemap_t& get_portal_map(); + + /** + * Initialize the portal nodes of the grid with neigbouring nodes and distance costs. + */ + void init_portal_nodes(); + private: /** * ID of the grid. @@ -115,6 +127,13 @@ class Grid { * Sectors of the grid. */ std::vector> sectors; + + /** + * maps portal_ids to portal nodes, which store their neigbouring nodes and associated distance costs + * for pathfinding + */ + + nodemap_t portal_nodes; }; diff --git a/libopenage/pathfinding/pathfinder.cpp b/libopenage/pathfinding/pathfinder.cpp index 288f7875df..cf325791ff 100644 --- a/libopenage/pathfinding/pathfinder.cpp +++ b/libopenage/pathfinding/pathfinder.cpp @@ -201,6 +201,7 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req std::vector> result; auto grid = this->grids.at(request.grid_id); + auto& portal_map = grid->get_portal_map(); auto sector_size = grid->get_sector_size(); auto start_sector_x = request.start.ne / sector_size; @@ -211,7 +212,8 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req heap_t node_candidates; // list of known portals and corresponding node. - nodemap_t visited_portals; + std::unordered_set visited_portals; + std::unordered_set was_best; // TODO: Compute cost to travel from one portal to another when creating portals // const int distance_cost = 1; @@ -223,7 +225,9 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req continue; } - auto portal_node = std::make_shared(portal, start_sector->get_id(), nullptr); + auto portal_node = portal_map.at(portal->get_id()); + portal_node->prev_portal = nullptr; + portal_node->entry_sector = start_sector->get_id(); auto sector_pos = grid->get_sector(portal->get_exit_sector(start_sector->get_id()))->get_position().to_tile(sector_size); auto portal_pos = portal->get_exit_center(start_sector->get_id()); @@ -235,7 +239,7 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req portal_node->future_cost = portal_node->current_cost + heuristic_cost; portal_node->heap_node = node_candidates.push(portal_node); - visited_portals[portal->get_id()] = portal_node; + visited_portals.insert(portal->get_id()); } // track the closest we can get to the end position @@ -246,7 +250,7 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req while (not node_candidates.empty()) { auto current_node = node_candidates.pop(); - current_node->was_best = true; + was_best.insert(current_node->portal->get_id()); // check if the current node is a portal in the target sector that can // be reached from the target cell @@ -266,20 +270,18 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req } // get the exits of the current node - auto exits = current_node->get_exits(visited_portals, current_node->entry_sector); + auto& exits = current_node->exits[current_node->entry_sector]; // evaluate all neighbors of the current candidate for further progress - for (auto &exit : exits) { - if (exit->was_best) { + for (auto &[exit_id, distance_cost] : exits) { + auto& exit = portal_map.at(exit_id); + exit->entry_sector = current_node->portal->get_exit_sector(current_node->entry_sector); + if (was_best.find(exit->portal->get_id()) != was_best.end()) { continue; } - // Get distance cost (from current node to exit node) - auto distance_cost = Pathfinder::distance_cost( - current_node->portal->get_exit_center(current_node->entry_sector), - exit->portal->get_entry_center(exit->entry_sector)); - bool not_visited = !visited_portals.contains(exit->portal->get_id()); + bool not_visited = visited_portals.find(exit->portal->get_id()) == visited_portals.end(); auto tentative_cost = current_node->current_cost + distance_cost; if (not_visited or tentative_cost < exit->current_cost) { @@ -300,7 +302,7 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req if (not_visited) { exit->heap_node = node_candidates.push(exit); - visited_portals[exit->portal->get_id()] = exit; + visited_portals.insert(exit->portal->get_id()); } else { node_candidates.decrease(exit->heap_node); @@ -463,6 +465,16 @@ int Pathfinder::distance_cost(const coord::tile_delta &portal1_pos, } +PortalNode::PortalNode(const std::shared_ptr &portal) : + portal{portal}, + entry_sector{NULL}, + future_cost{std::numeric_limits::max()}, + current_cost{std::numeric_limits::max()}, + heuristic_cost{std::numeric_limits::max()}, + was_best{false}, + prev_portal{nullptr}, + heap_node{nullptr} {} + PortalNode::PortalNode(const std::shared_ptr &portal, sector_id_t entry_sector, const node_t &prev_portal) : @@ -517,7 +529,6 @@ std::vector PortalNode::get_exits(const nodemap_t &nodes, std::vector exit_nodes; exit_nodes.reserve(exits.size()); - auto exit_sector = this->portal->get_exit_sector(entry_sector); for (auto &exit : exits) { auto exit_id = exit->get_id(); @@ -526,6 +537,7 @@ std::vector PortalNode::get_exits(const nodemap_t &nodes, exit_nodes.push_back(exit_node->second); } else { + auto exit_sector = this->portal->get_exit_sector(entry_sector); exit_nodes.push_back(std::make_shared(exit, exit_sector, this->shared_from_this())); @@ -535,6 +547,21 @@ std::vector PortalNode::get_exits(const nodemap_t &nodes, } +void PortalNode::init_exits(const nodemap_t &nodes, + sector_id_t entry_sector) { + auto &exits = this->portal->get_exits(entry_sector); + sector_id_t exit_sector = this->portal->get_exit_sector(entry_sector); + + for (auto &exit : exits) { + int distance_cost = Pathfinder::distance_cost( + this->portal->get_exit_center(entry_sector), + exit->get_entry_center(exit_sector)); + + this->exits[entry_sector][exit->get_id()] = distance_cost; + } +} + + bool compare_node_cost::operator()(const node_t &lhs, const node_t &rhs) const { return *lhs < *rhs; } diff --git a/libopenage/pathfinding/pathfinder.h b/libopenage/pathfinding/pathfinder.h index 1809b7524d..dbe3d1e60f 100644 --- a/libopenage/pathfinding/pathfinder.h +++ b/libopenage/pathfinding/pathfinder.h @@ -62,6 +62,17 @@ class Pathfinder { */ const Path get_path(const PathRequest &request); + + /** + * Calculate the distance cost between two portals. + * + * @param portal1_pos Center of the first portal (relative to sector origin). + * @param portal2_pos Center of the second portal (relative to sector origin). + * + * @return Distance cost between the portal centers. + */ + static int distance_cost(const coord::tile_delta &portal1_pos, const coord::tile_delta &portal2_pos); + private: using portal_star_t = std::pair>>; @@ -100,15 +111,6 @@ class Pathfinder { */ static int heuristic_cost(const coord::tile &portal_pos, const coord::tile &target_pos); - /** - * Calculate the distance cost between two portals. - * - * @param portal1_pos Center of the first portal (relative to sector origin). - * @param portal2_pos Center of the second portal (relative to sector origin). - * - * @return Distance cost between the portal centers. - */ - static int distance_cost(const coord::tile_delta &portal1_pos, const coord::tile_delta &portal2_pos); /** * Grids managed by this pathfinder. @@ -147,6 +149,8 @@ using nodemap_t = std::unordered_map; */ class PortalNode : public std::enable_shared_from_this { public: + PortalNode(const std::shared_ptr &portal); + PortalNode(const std::shared_ptr &portal, sector_id_t entry_sector, const node_t &prev_portal); @@ -181,6 +185,12 @@ class PortalNode : public std::enable_shared_from_this { * Get all exits of a node. */ std::vector get_exits(const nodemap_t &nodes, sector_id_t entry_sector); + + + /** + * init PortalNode::exits. + */ + void init_exits(const nodemap_t &nodes, sector_id_t entry_sector); /** * The portal this node is associated to. @@ -226,6 +236,20 @@ class PortalNode : public std::enable_shared_from_this { * Priority queue node that contains this path node. */ heap_t::element_t heap_node; + + /** + * maps portal_id_t of a neigbhour portal to the distance cost to travel between the portals + */ + using neighbours_t = std::unordered_map; + + + /* + * maps a sector_id that the portal can be entered from + * to a set of portal_ids that connect to the sector the portal will be exited from and the cost to travel to them. + */ + std::unordered_map exits; + + };