Pathfinding: Precomputeportal nodes for A Star

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();

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 &sector : this->sectors) {
+		for (auto &portal : sector->get_portals()) {
+			if (!this->portal_nodes.contains(portal->get_id())) {
+				auto portal_node = std::make_shared<PortalNode>(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

libopenage/pathfinding/grid.h

@@ -7,6 +7,7 @@
 #include <utility>
 #include <vector>
 
+#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<std::shared_ptr<Sector>> sectors;
+
+	/**
+	 *	maps portal_ids to portal nodes, which store their neigbouring nodes and associated distance costs
+	 *  for pathfinding
+	 */
+
+	nodemap_t portal_nodes;
 };
 
 

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<std::shared_ptr<Portal>> 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<portal_id_t> 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<PortalNode>(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{portal},
+	entry_sector{NULL},
+	future_cost{std::numeric_limits<int>::max()},
+	current_cost{std::numeric_limits<int>::max()},
+	heuristic_cost{std::numeric_limits<int>::max()},
+	was_best{false},
+	prev_portal{nullptr},
+	heap_node{nullptr} {}
+
 PortalNode::PortalNode(const std::shared_ptr<Portal> &portal,
                        sector_id_t entry_sector,
                        const node_t &prev_portal) :
@@ -511,27 +526,37 @@ std::vector<node_t> PortalNode::generate_backtrace() {
 	return waypoints;
 }
 
-std::vector<node_t> PortalNode::get_exits(const nodemap_t &nodes,
-                                          sector_id_t entry_sector) {
-	auto &exits = this->portal->get_exits(entry_sector);
-	std::vector<node_t> 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<PortalNode>(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;
 }
 
 

libopenage/pathfinding/pathfinder.h

@@ -2,9 +2,9 @@
 
 #pragma once
 
+#include <map>
 #include <memory>
 #include <unordered_map>
-#include <unordered_set>
 
 #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<PathResult, std::vector<std::shared_ptr<Portal>>>;
 
@@ -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<portal_id_t, node_t>;
  */
 class PortalNode : public std::enable_shared_from_this<PortalNode> {
 public:
+	PortalNode(const std::shared_ptr<Portal> &portal);
 	PortalNode(const std::shared_ptr<Portal> &portal,
 	           sector_id_t entry_sector,
 	           const node_t &prev_portal);
@@ -178,9 +181,25 @@ class PortalNode : public std::enable_shared_from_this<PortalNode> {
 	std::vector<node_t> 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<node_t> get_exits(const nodemap_t &nodes, sector_id_t entry_sector);
+	using exits_t = std::map<const node_t, int>;
+
+
+	/**
+	 * 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<PortalNode> {
 	 * 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;
 };