Add: Road vehicle path cache. (#7261)

9 files changed, 109 insertions, 25 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);
diff --git a/src/roadveh.h b/src/roadveh.h
index ca069b7ac..c2a799951 100644
--- a/src/roadveh.h
+++ b/src/roadveh.h
@@ -18,6 +18,7 @@
 #include "track_func.h"
 #include "road_type.h"
 #include "newgrf_engine.h"
+#include <deque>
 
 struct RoadVehicle;
 
@@ -82,10 +83,30 @@ static const byte RV_OVERTAKE_TIMEOUT = 35;
 void RoadVehUpdateCache(RoadVehicle *v, bool same_length = false);
 void GetRoadVehSpriteSize(EngineID engine, uint &width, uint &height, int &xoffs, int &yoffs, EngineImageType image_type);
 
+struct RoadVehPathCache {
+	std::deque<TrackdirByte> td;
+	std::deque<TileIndex> tile;
+
+	inline bool empty() const { return this->td.empty(); }
+
+	inline size_t size() const
+	{
+		assert(this->td.size() == this->tile.size());
+		return this->td.size();
+	}
+
+	inline void clear()
+	{
+		this->td.clear();
+		this->tile.clear();
+	}
+};
+
 /**
  * Buses, trucks and trams belong to this class.
  */
 struct RoadVehicle FINAL : public GroundVehicle<RoadVehicle, VEH_ROAD> {
+	RoadVehPathCache path;  ///< Cached path.
 	byte state;             ///< @see RoadVehicleStates
 	byte frame;
 	uint16 blocked_ctr;
@@ -125,6 +146,7 @@ struct RoadVehicle FINAL : public GroundVehicle<RoadVehicle, VEH_ROAD> {
 
 	int GetCurrentMaxSpeed() const;
 	int UpdateSpeed();
+	void SetDestTile(TileIndex tile);
 
 protected: // These functions should not be called outside acceleration code.
 
diff --git a/src/roadveh_cmd.cpp b/src/roadveh_cmd.cpp
index 970477001..559b87547 100644
--- a/src/roadveh_cmd.cpp
+++ b/src/roadveh_cmd.cpp
@@ -924,6 +924,8 @@ static Trackdir RoadFindPathToDest(RoadVehicle *v, TileIndex tile, DiagDirection
 	/* Remove tracks unreachable from the enter dir */
 	trackdirs &= DiagdirReachesTrackdirs(enterdir);
 	if (trackdirs == TRACKDIR_BIT_NONE) {
+		/* If vehicle expected a path, it no longer exists, so invalidate it. */
+		if (!v->path.empty()) v->path.clear();
 		/* No reachable tracks, so we'll reverse */
 		return_track(_road_reverse_table[enterdir]);
 	}
@@ -954,12 +956,35 @@ static Trackdir RoadFindPathToDest(RoadVehicle *v, TileIndex tile, DiagDirection
 
 	/* Only one track to choose between? */
 	if (KillFirstBit(trackdirs) == TRACKDIR_BIT_NONE) {
+		if (!v->path.empty() && v->path.tile.front() == tile) {
+			/* Vehicle expected a choice here, invalidate its path. */
+			v->path.clear();
+		}
 		return_track(FindFirstBit2x64(trackdirs));
 	}
 
+	/* Attempt to follow cached path. */
+	if (!v->path.empty()) {
+		if (v->path.tile.front() != tile) {
+			/* Vehicle didn't expect a choice here, invalidate its path. */
+			v->path.clear();
+		} else {
+			Trackdir trackdir = v->path.td.front();
+
+			if (HasBit(trackdirs, trackdir)) {
+				v->path.td.pop_front();
+				v->path.tile.pop_front();
+				return_track(trackdir);
+			}
+
+			/* Vehicle expected a choice which is no longer available. */
+			v->path.clear();
+		}
+	}
+
 	switch (_settings_game.pf.pathfinder_for_roadvehs) {
 		case VPF_NPF:  best_track = NPFRoadVehicleChooseTrack(v, tile, enterdir, path_found); break;
-		case VPF_YAPF: best_track = YapfRoadVehicleChooseTrack(v, tile, enterdir, trackdirs, path_found); break;
+		case VPF_YAPF: best_track = YapfRoadVehicleChooseTrack(v, tile, enterdir, trackdirs, path_found, v->path); break;
 
 		default: NOT_REACHED();
 	}
@@ -1600,6 +1625,13 @@ bool RoadVehicle::Tick()
 	return true;
 }
 
+void RoadVehicle::SetDestTile(TileIndex tile)
+{
+	if (tile == this->dest_tile) return;
+	this->path.clear();
+	this->dest_tile = tile;
+}
+
 static void CheckIfRoadVehNeedsService(RoadVehicle *v)
 {
 	/* If we already got a slot at a stop, use that FIRST, and go to a depot later */
diff --git a/src/saveload/saveload.h b/src/saveload/saveload.h
index 331d62d7f..77cce4c4a 100644
--- a/src/saveload/saveload.h
+++ b/src/saveload/saveload.h
@@ -295,6 +295,7 @@ enum SaveLoadVersion : uint16 {
 	SLV_SHIP_CURVE_PENALTY,                 ///< 209  PR#7289 Configurable ship curve penalties.
 
 	SLV_SERVE_NEUTRAL_INDUSTRIES,           ///< 210  PR#7234 Company stations can serve industries with attached neutral stations.
+	SLV_ROADVEH_PATH_CACHE,                 ///< 211  PR#7261 Add path cache for road vehicles.
 
 	SL_MAX_VERSION,                         ///< Highest possible saveload version
 };
diff --git a/src/saveload/vehicle_sl.cpp b/src/saveload/vehicle_sl.cpp
index 540416586..058587139 100644
--- a/src/saveload/vehicle_sl.cpp
+++ b/src/saveload/vehicle_sl.cpp
@@ -752,21 +752,23 @@ const SaveLoad *GetVehicleDescription(VehicleType vt)
 	static const SaveLoad _roadveh_desc[] = {
 		SLE_WRITEBYTE(Vehicle, type),
 		SLE_VEH_INCLUDE(),
-		     SLE_VAR(RoadVehicle, state,                SLE_UINT8),
-		     SLE_VAR(RoadVehicle, frame,                SLE_UINT8),
-		     SLE_VAR(RoadVehicle, blocked_ctr,          SLE_UINT16),
-		     SLE_VAR(RoadVehicle, overtaking,           SLE_UINT8),
-		     SLE_VAR(RoadVehicle, overtaking_ctr,       SLE_UINT8),
-		     SLE_VAR(RoadVehicle, crashed_ctr,          SLE_UINT16),
-		     SLE_VAR(RoadVehicle, reverse_ctr,          SLE_UINT8),
-
-		SLE_CONDNULL(2,                                                               SLV_6,  SLV_69),
-		 SLE_CONDVAR(RoadVehicle, gv_flags,             SLE_UINT16,                 SLV_139, SL_MAX_VERSION),
-		SLE_CONDNULL(4,                                                              SLV_69, SLV_131),
-		SLE_CONDNULL(2,                                                               SLV_6, SLV_131),
-		SLE_CONDNULL(16,                                                              SLV_2, SLV_144), // old reserved space
-
-		     SLE_END()
+		      SLE_VAR(RoadVehicle, state,                SLE_UINT8),
+		      SLE_VAR(RoadVehicle, frame,                SLE_UINT8),
+		      SLE_VAR(RoadVehicle, blocked_ctr,          SLE_UINT16),
+		      SLE_VAR(RoadVehicle, overtaking,           SLE_UINT8),
+		      SLE_VAR(RoadVehicle, overtaking_ctr,       SLE_UINT8),
+		      SLE_VAR(RoadVehicle, crashed_ctr,          SLE_UINT16),
+		      SLE_VAR(RoadVehicle, reverse_ctr,          SLE_UINT8),
+		SLE_CONDDEQUE(RoadVehicle, path.td,              SLE_UINT8,                  SLV_ROADVEH_PATH_CACHE, SL_MAX_VERSION),
+		SLE_CONDDEQUE(RoadVehicle, path.tile,            SLE_UINT32,                 SLV_ROADVEH_PATH_CACHE, SL_MAX_VERSION),
+
+		 SLE_CONDNULL(2,                                                               SLV_6,  SLV_69),
+		  SLE_CONDVAR(RoadVehicle, gv_flags,             SLE_UINT16,                 SLV_139, SL_MAX_VERSION),
+		 SLE_CONDNULL(4,                                                              SLV_69, SLV_131),
+		 SLE_CONDNULL(2,                                                               SLV_6, SLV_131),
+		 SLE_CONDNULL(16,                                                              SLV_2, SLV_144), // old reserved space
+
+		      SLE_END()
 	};
 
 	static const SaveLoad _ship_desc[] = {