Add: Road vehicle path cache. (#7261)

5 files changed, 36 insertions, 9 deletions

diff --git a/src/pathfinder/pathfinder_type.h b/src/pathfinder/pathfinder_type.h
index 0ecf00bbd..3740d0390 100644
--- a/src/pathfinder/pathfinder_type.h
+++ b/src/pathfinder/pathfinder_type.h
@@ -43,6 +43,9 @@ static const int YAPF_INFINITE_PENALTY = 1000 * YAPF_TILE_LENGTH;
 /** Maximum length of ship path cache */
 static const int YAPF_SHIP_PATH_CACHE_LENGTH = 32;
 
+/** Maximum segments of road vehicle path cache */
+static const int YAPF_ROADVEH_PATH_CACHE_SEGMENTS = 8;
+
 /**
  * Helper container to find a depot
  */
diff --git a/src/pathfinder/yapf/yapf.h b/src/pathfinder/yapf/yapf.h
index 84bd35c8b..3bea692b7 100644
--- a/src/pathfinder/yapf/yapf.h
+++ b/src/pathfinder/yapf/yapf.h
@@ -16,6 +16,7 @@
 #include "../../track_type.h"
 #include "../../vehicle_type.h"
 #include "../../ship.h"
+#include "../../roadveh.h"
 #include "../pathfinder_type.h"
 
 /**
@@ -45,7 +46,7 @@ bool YapfShipCheckReverse(const Ship *v);
  * @param path_found [out] Whether a path has been found (true) or has been guessed (false)
  * @return          the best trackdir for next turn or INVALID_TRACKDIR if the path could not be found
  */
-Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found);
+Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found, RoadVehPathCache &path_cache);
 
 /**
  * Finds the best path for given train using YAPF.
diff --git a/src/pathfinder/yapf/yapf_node.hpp b/src/pathfinder/yapf/yapf_node.hpp
index b3021096b..e510b2a6c 100644
--- a/src/pathfinder/yapf/yapf_node.hpp
+++ b/src/pathfinder/yapf/yapf_node.hpp
@@ -67,6 +67,7 @@ struct CYapfNodeT {
 	Node       *m_parent;
 	int         m_cost;
 	int         m_estimate;
+	bool        m_is_choice;
 
 	inline void Set(Node *parent, TileIndex tile, Trackdir td, bool is_choice)
 	{
@@ -75,6 +76,7 @@ struct CYapfNodeT {
 		m_parent = parent;
 		m_cost = 0;
 		m_estimate = 0;
+		m_is_choice = is_choice;
 	}
 
 	inline Node *GetHashNext()
@@ -112,6 +114,11 @@ struct CYapfNodeT {
 		return m_estimate;
 	}
 
+	inline bool GetIsChoice() const
+	{
+		return m_is_choice;
+	}
+
 	inline bool operator<(const Node &other) const
 	{
 		return m_estimate < other.m_estimate;
diff --git a/src/pathfinder/yapf/yapf_road.cpp b/src/pathfinder/yapf/yapf_road.cpp
index 8da84444c..908915763 100644
--- a/src/pathfinder/yapf/yapf_road.cpp
+++ b/src/pathfinder/yapf/yapf_road.cpp
@@ -348,13 +348,13 @@ public:
 		return 'r';
 	}
 
-	static Trackdir stChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
+	static Trackdir stChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found, RoadVehPathCache &path_cache)
 	{
 		Tpf pf;
-		return pf.ChooseRoadTrack(v, tile, enterdir, path_found);
+		return pf.ChooseRoadTrack(v, tile, enterdir, path_found, path_cache);
 	}
 
-	inline Trackdir ChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found)
+	inline Trackdir ChooseRoadTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, bool &path_found, RoadVehPathCache &path_cache)
 	{
 		/* Handle special case - when next tile is destination tile.
 		 * However, when going to a station the (initial) destination
@@ -382,15 +382,30 @@ public:
 		Trackdir next_trackdir = INVALID_TRACKDIR;
 		Node *pNode = Yapf().GetBestNode();
 		if (pNode != NULL) {
+			uint steps = 0;
+			for (Node *n = pNode; n->m_parent != NULL; n = n->m_parent) steps++;
+
 			/* path was found or at least suggested
 			 * walk through the path back to its origin */
 			while (pNode->m_parent != NULL) {
+				steps--;
+				if (pNode->GetIsChoice() && steps < YAPF_ROADVEH_PATH_CACHE_SEGMENTS) {
+					TrackdirByte td;
+					td = pNode->GetTrackdir();
+					path_cache.td.push_front(td);
+					path_cache.tile.push_front(pNode->GetTile());
+				}
 				pNode = pNode->m_parent;
 			}
 			/* return trackdir from the best origin node (one of start nodes) */
 			Node &best_next_node = *pNode;
 			assert(best_next_node.GetTile() == tile);
 			next_trackdir = best_next_node.GetTrackdir();
+			/* remove last element for the special case when tile == dest_tile */
+			if (path_found && !path_cache.empty() && tile == v->dest_tile) {
+				path_cache.td.pop_back();
+				path_cache.tile.pop_back();
+			}
 		}
 		return next_trackdir;
 	}
@@ -497,18 +512,18 @@ struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNod
 struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {};
 
 
-Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found)
+Trackdir YapfRoadVehicleChooseTrack(const RoadVehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool &path_found, RoadVehPathCache &path_cache)
 {
 	/* default is YAPF type 2 */
-	typedef Trackdir (*PfnChooseRoadTrack)(const RoadVehicle*, TileIndex, DiagDirection, bool &path_found);
-	PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack; // default: ExitDir
+	typedef Trackdir (*PfnChooseRoadTrack)(const RoadVehicle*, TileIndex, DiagDirection, bool &path_found, RoadVehPathCache &path_cache);
+	PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack; // default: ExitDir, allow 90-deg
 
 	/* check if non-default YAPF type should be used */
 	if (_settings_game.pf.yapf.disable_node_optimization) {
 		pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack; // Trackdir
 	}
 
-	Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir, path_found);
+	Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir, path_found, path_cache);
 	return (td_ret != INVALID_TRACKDIR) ? td_ret : (Trackdir)FindFirstBit2x64(trackdirs);
 }
 
diff --git a/src/pathfinder/yapf/yapf_ship.cpp b/src/pathfinder/yapf/yapf_ship.cpp
index 63abd592e..b880128f5 100644
--- a/src/pathfinder/yapf/yapf_ship.cpp
+++ b/src/pathfinder/yapf/yapf_ship.cpp
@@ -96,7 +96,8 @@ public:
 			/* walk through the path back to the origin */
 			Node *pPrevNode = NULL;
 			while (pNode->m_parent != NULL) {
-				if (steps > 1 && --steps < YAPF_SHIP_PATH_CACHE_LENGTH) {
+				steps--;
+				if (steps > 0 && steps < YAPF_SHIP_PATH_CACHE_LENGTH) {
 					TrackdirByte td;
 					td = pNode->GetTrackdir();
 					path_cache.push_front(td);