From 7cc49be72d5838bef1780cf14857b55bde98d14c 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 | 60 +++++++++++++------------- libopenage/pathfinding/pathfinder.h | 42 +++++++++++++----- 5 files changed, 108 insertions(+), 42 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..98f9713436 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,18 +270,16 @@ 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); + const auto& exits = current_node->exits.at(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.contains(exit->portal->get_id())) { 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()); auto tentative_cost = current_node->current_cost + distance_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,15 @@ 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()}, + prev_portal{nullptr}, + heap_node{nullptr} {} + PortalNode::PortalNode(const std::shared_ptr &portal, sector_id_t entry_sector, const node_t &prev_portal) : @@ -471,7 +482,6 @@ PortalNode::PortalNode(const std::shared_ptr &portal, future_cost{std::numeric_limits::max()}, current_cost{std::numeric_limits::max()}, heuristic_cost{std::numeric_limits::max()}, - was_best{false}, prev_portal{prev_portal}, heap_node{nullptr} {} @@ -485,7 +495,6 @@ PortalNode::PortalNode(const std::shared_ptr &portal, future_cost{past_cost + heuristic_cost}, current_cost{past_cost}, heuristic_cost{heuristic_cost}, - was_best{false}, prev_portal{prev_portal}, heap_node{nullptr} { } @@ -511,27 +520,16 @@ std::vector PortalNode::generate_backtrace() { return waypoints; } -std::vector PortalNode::get_exits(const nodemap_t &nodes, - sector_id_t entry_sector) { +void PortalNode::init_exits(const nodemap_t &nodes, sector_id_t entry_sector) { auto &exits = this->portal->get_exits(entry_sector); - std::vector exit_nodes; - exit_nodes.reserve(exits.size()); - - auto exit_sector = this->portal->get_exit_sector(entry_sector); + sector_id_t exit_sector = this->portal->get_exit_sector(entry_sector); for (auto &exit : exits) { - auto exit_id = exit->get_id(); + int distance_cost = Pathfinder::distance_cost( + this->portal->get_exit_center(entry_sector), + exit->get_entry_center(exit_sector)); - auto exit_node = nodes.find(exit_id); - if (exit_node != nodes.end()) { - exit_nodes.push_back(exit_node->second); - } - else { - exit_nodes.push_back(std::make_shared(exit, - exit_sector, - this->shared_from_this())); - } + this->exits[entry_sector][exit->get_id()] = distance_cost; } - return exit_nodes; } diff --git a/libopenage/pathfinding/pathfinder.h b/libopenage/pathfinding/pathfinder.h index 1809b7524d..9b5aeb31ed 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); @@ -177,10 +181,12 @@ class PortalNode : public std::enable_shared_from_this { */ std::vector generate_backtrace(); + + /** - * Get all exits of a node. + * init PortalNode::exits. */ - std::vector get_exits(const nodemap_t &nodes, sector_id_t entry_sector); + void init_exits(const nodemap_t &nodes, sector_id_t entry_sector); /** * The portal this node is associated to. @@ -226,6 +232,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; + + };