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,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<PortalNode>(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

libopenage/pathfinding/grid.h

@@ -7,8 +7,10 @@
 #include <utility>
 #include <vector>
 
+#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<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

@@ -201,6 +201,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;
@@ -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<portal_id_t> visited_portals;
+	std::unordered_set<portal_id_t> 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<PortalNode>(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{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) :
@@ -517,7 +529,6 @@ std::vector<node_t> PortalNode::get_exits(const nodemap_t &nodes,
 	std::vector<node_t> 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<node_t> 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<PortalNode>(exit,
 			                                                  exit_sector,
 			                                                  this->shared_from_this()));
@@ -535,6 +547,21 @@ std::vector<node_t> 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;
 }

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<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,8 @@ 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);
@@ -181,6 +185,12 @@ class PortalNode : public std::enable_shared_from_this<PortalNode> {
 	 * Get all exits of a node.
 	 */
 	std::vector<node_t> 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<PortalNode> {
 	 * 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<portal_id_t, int>;
+
+
+	/*
+	* 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<sector_id_t, neighbours_t> exits;
+
+
 };