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..45427b009d 100644 --- a/libopenage/pathfinding/grid.cpp +++ b/libopenage/pathfinding/grid.cpp @@ -98,4 +98,27 @@ void Grid::init_portals() { } } +const nodemap_t &Grid::get_portal_map() { + return portal_nodes; +} + +void Grid::init_portal_nodes() { + // create portal_nodes + for (auto §or : this->sectors) { + for (auto &portal : sector->get_portals()) { + if (!this->portal_nodes.contains(portal->get_id())) { + auto portal_node = std::make_shared(portal); + portal_node->node_sector_0 = sector->get_id(); + portal_node->node_sector_1 = portal_node->portal->get_exit_sector(sector->get_id()); + this->portal_nodes[portal->get_id()] = portal_node; + } + } + } + + // init portal_node exits + for (auto &[id, node] : this->portal_nodes) { + node->init_exits(this->portal_nodes); + } +} + } // namespace openage::path diff --git a/libopenage/pathfinding/grid.h b/libopenage/pathfinding/grid.h index 8f24743433..314d107a2e 100644 --- a/libopenage/pathfinding/grid.h +++ b/libopenage/pathfinding/grid.h @@ -7,6 +7,7 @@ #include #include +#include "pathfinding/pathfinder.h" #include "pathfinding/types.h" #include "util/vector.h" @@ -95,6 +96,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 +126,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..e46919e62b 100644 --- a/libopenage/pathfinding/pathfinder.cpp +++ b/libopenage/pathfinding/pathfinder.cpp @@ -4,6 +4,7 @@ #include "coord/chunk.h" #include "coord/phys.h" +#include "error/error.h" #include "pathfinding/cost_field.h" #include "pathfinding/flow_field.h" #include "pathfinding/grid.h" @@ -201,6 +202,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; @@ -210,8 +212,7 @@ const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &req // path node storage, always provides cheapest next node. heap_t node_candidates; - // list of known portals and corresponding node. - nodemap_t visited_portals; + std::unordered_set visited_portals; // TODO: Compute cost to travel from one portal to another when creating portals // const int distance_cost = 1; @@ -223,7 +224,8 @@ 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->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 +237,9 @@ 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; + portal_node->prev_portal = nullptr; + portal_node->was_best = false; + visited_portals.insert(portal->get_id()); } // track the closest we can get to the end position @@ -266,20 +270,21 @@ 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->get_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, distance_cost] : exits) { + exit->entry_sector = current_node->portal->get_exit_sector(current_node->entry_sector); + bool not_visited = !visited_portals.contains(exit->portal->get_id()); + + if (not_visited) { + exit->was_best = false; + } + else if (exit->was_best) { 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; if (not_visited or tentative_cost < exit->current_cost) { @@ -300,7 +305,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 +468,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) : @@ -511,27 +526,37 @@ std::vector PortalNode::generate_backtrace() { return waypoints; } -std::vector PortalNode::get_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()); +void PortalNode::init_exits(const nodemap_t &node_map) { + auto exits = this->portal->get_exits(this->node_sector_0); + for (auto &exit : exits) { + int distance_cost = Pathfinder::distance_cost( + this->portal->get_exit_center(this->node_sector_0), + exit->get_entry_center(this->node_sector_1)); + + auto exit_node = node_map.at(exit->get_id()); + this->exits_1[exit_node] = distance_cost; + } - auto exit_sector = this->portal->get_exit_sector(entry_sector); + exits = this->portal->get_exits(this->node_sector_1); for (auto &exit : exits) { - auto exit_id = exit->get_id(); + int distance_cost = Pathfinder::distance_cost( + this->portal->get_exit_center(this->node_sector_1), + exit->get_entry_center(this->node_sector_0)); - 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())); - } + auto exit_node = node_map.at(exit->get_id()); + this->exits_0[exit_node] = distance_cost; + } +} + +const PortalNode::exits_t &PortalNode::get_exits(sector_id_t entry_sector) { + ENSURE(entry_sector == this->node_sector_0 || entry_sector == this->node_sector_1, "Invalid entry sector"); + + if (this->node_sector_0 == entry_sector) { + return exits_1; + } + else { + return exits_0; } - return exit_nodes; } diff --git a/libopenage/pathfinding/pathfinder.h b/libopenage/pathfinding/pathfinder.h index 1809b7524d..e2529987b7 100644 --- a/libopenage/pathfinding/pathfinder.h +++ b/libopenage/pathfinding/pathfinder.h @@ -2,9 +2,9 @@ #pragma once +#include #include #include -#include #include "coord/tile.h" #include "datastructure/pairing_heap.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,7 @@ 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); @@ -178,9 +181,25 @@ class PortalNode : public std::enable_shared_from_this { std::vector generate_backtrace(); /** - * Get all exits of a node. + * init PortalNode::exits. + */ + void init_exits(const nodemap_t &node_map); + + + /** + * maps node_t of a neigbhour portal to the distance cost to travel between the portals */ - std::vector get_exits(const nodemap_t &nodes, sector_id_t entry_sector); + using exits_t = std::map; + + + /** + * Get the exit portals reachable via the portal when entering from a specified sector. + * + * @param entry_sector Sector from which the portal is entered. + * + * @return Exit portals nodes reachable from the portal. + */ + const exits_t &get_exits(sector_id_t entry_sector); /** * The portal this node is associated to. @@ -226,6 +245,26 @@ class PortalNode : public std::enable_shared_from_this { * Priority queue node that contains this path node. */ heap_t::element_t heap_node; + + /** + * First sector connected by the portal. + */ + sector_id_t node_sector_0; + + /** + * Second sector connected by the portal. + */ + sector_id_t node_sector_1; + + /** + * Exits in sector 0 reachable from the portal. + */ + exits_t exits_0; + + /** + * Exits in sector 1 reachable from the portal. + */ + exits_t exits_1; };