(svn r13944) -Add [YAPP]: Add YAPF provider to find a safe tile and reserve a path. (michi_cc)

6 files changed, 184 insertions, 4 deletions

diff --git a/src/yapf/follow_track.hpp b/src/yapf/follow_track.hpp
index 035bc2460..a2f727dab 100644
--- a/src/yapf/follow_track.hpp
+++ b/src/yapf/follow_track.hpp
@@ -10,8 +10,8 @@
 
 /** Track follower helper template class (can serve pathfinders and vehicle
  *  controllers). See 6 different typedefs below for 3 different transport
- *  types w/ of w/o 90-deg turns allowed */
-template <TransportType Ttr_type_, bool T90deg_turns_allowed_ = true>
+ *  types w/ or w/o 90-deg turns allowed */
+template <TransportType Ttr_type_, bool T90deg_turns_allowed_ = true, bool Tmask_reserved_tracks = false>
 struct CFollowTrackT
 {
 	enum ErrorCode {
@@ -20,6 +20,7 @@ struct CFollowTrackT
 		EC_RAIL_TYPE,
 		EC_90DEG,
 		EC_NO_WAY,
+		EC_RESERVED,
 	};
 
 	const Vehicle      *m_veh;           ///< moving vehicle
@@ -62,6 +63,7 @@ struct CFollowTrackT
 	FORCEINLINE bool IsTram() {return IsRoadTT() && HasBit(m_veh->u.road.compatible_roadtypes, ROADTYPE_TRAM);}
 	FORCEINLINE static bool IsRoadTT() {return TT() == TRANSPORT_ROAD;}
 	FORCEINLINE static bool Allow90degTurns() {return T90deg_turns_allowed_;}
+	FORCEINLINE static bool MaskReservedTracks() {return Tmask_reserved_tracks;}
 
 	/** Tests if a tile is a road tile with a single tramtrack (tram can reverse) */
 	FORCEINLINE DiagDirection GetSingleTramBit(TileIndex tile)
@@ -106,6 +108,21 @@ struct CFollowTrackT
 				return false;
 			}
 		}
+		if (MaskReservedTracks()) {
+			TrackBits reserved = GetReservedTrackbits(m_new_tile);
+			/* Mask already reserved trackdirs. */
+			m_new_td_bits &= ~TrackBitsToTrackdirBits(reserved);
+			/* Mask out all trackdirs that conflict with the reservation. */
+			uint bits = (uint)TrackdirBitsToTrackBits(m_new_td_bits);
+			int i;
+			FOR_EACH_SET_BIT(i, bits) {
+				if (TracksOverlap(reserved | TrackToTrackBits((Track)i))) m_new_td_bits &= ~TrackToTrackdirBits((Track)i);
+			}
+			if (m_new_td_bits == TRACKDIR_BIT_NONE) {
+				m_err = EC_RESERVED;
+				return false;
+			}
+		}
 		return true;
 	}
 
@@ -382,4 +399,7 @@ typedef CFollowTrackT<TRANSPORT_WATER, false> CFollowTrackWaterNo90;
 typedef CFollowTrackT<TRANSPORT_ROAD , false> CFollowTrackRoadNo90;
 typedef CFollowTrackT<TRANSPORT_RAIL , false> CFollowTrackRailNo90;
 
+typedef CFollowTrackT<TRANSPORT_RAIL , true , true> CFollowTrackFreeRail;
+typedef CFollowTrackT<TRANSPORT_RAIL , false, true> CFollowTrackFreeRailNo90;
+
 #endif /* FOLLOW_TRACK_HPP */
diff --git a/src/yapf/yapf.h b/src/yapf/yapf.h
index fb39854b2..45ba94eb3 100644
--- a/src/yapf/yapf.h
+++ b/src/yapf/yapf.h
@@ -66,6 +66,17 @@ bool YapfFindNearestRailDepotTwoWay(const Vehicle *v, int max_distance, int reve
 /** Returns true if it is better to reverse the train before leaving station */
 bool YapfCheckReverseTrain(const Vehicle* v);
 
+/**
+ * Try to extend the reserved path of a train to the nearest safe tile.
+ *
+ * @param v    The train that needs to find a safe tile.
+ * @param tile Last tile of the current reserved path.
+ * @param td   Last trackdir of the current reserved path.
+ * @param override_railtype Should all physically compabtible railtypes be searched, even if the vehicle can't on them on it own?
+ * @return True if the path could be extended to a safe tile.
+ */
+bool YapfRailFindNearestSafeTile(const Vehicle *v, TileIndex tile, Trackdir td, bool override_railtype);
+
 /** Use this function to notify YAPF that track layout (or signal configuration) has change */
 void YapfNotifyTrackLayoutChange(TileIndex tile, Track track);
 
diff --git a/src/yapf/yapf_costrail.hpp b/src/yapf/yapf_costrail.hpp
index 60eaae390..c19353f1f 100644
--- a/src/yapf/yapf_costrail.hpp
+++ b/src/yapf/yapf_costrail.hpp
@@ -387,6 +387,11 @@ no_entry_cost: // jump here at the beginning if the node has no parent (it is th
 			} else if (cur.tile_type == MP_RAILWAY && IsRailWaypoint(cur.tile)) {
 				/* Waypoint is also a good reason to finish. */
 				end_segment_reason |= ESRB_WAYPOINT;
+			} else if (TrackFollower::MaskReservedTracks() && cur.tile_type == MP_RAILWAY) {
+				/* Searching for a safe tile? */
+				if (HasSignalOnTrackdir(cur.tile, cur.td) && !IsPbsSignal(GetSignalType(cur.tile, TrackdirToTrack(cur.td)))) {
+					end_segment_reason |= ESRB_SAFE_TILE;
+				}
 			}
 
 			/* Apply min/max speed penalties only when inside the look-ahead radius. Otherwise
@@ -419,6 +424,10 @@ no_entry_cost: // jump here at the beginning if the node has no parent (it is th
 				} else {
 					end_segment_reason |= ESRB_DEAD_END;
 				}
+
+				if (TrackFollower::MaskReservedTracks() && tf_local.m_err != TrackFollower::EC_90DEG) {
+					end_segment_reason |= ESRB_SAFE_TILE;
+				}
 				break;
 			}
 
@@ -432,6 +441,11 @@ no_entry_cost: // jump here at the beginning if the node has no parent (it is th
 			/* Gather the next tile/trackdir/tile_type/rail_type. */
 			TILE next(tf_local.m_new_tile, (Trackdir)FindFirstBit2x64(tf_local.m_new_td_bits));
 
+			if (TrackFollower::MaskReservedTracks() && HasPbsSignalOnTrackdir(next.tile, next.td)) {
+				/* Possible safe tile. */
+				end_segment_reason |= ESRB_SAFE_TILE;
+			}
+
 			/* Check the next tile for the rail type. */
 			if (next.rail_type != cur.rail_type) {
 				/* Segment must consist from the same rail_type tiles. */
diff --git a/src/yapf/yapf_destrail.hpp b/src/yapf/yapf_destrail.hpp
index 7ea73889e..9790758e4 100644
--- a/src/yapf/yapf_destrail.hpp
+++ b/src/yapf/yapf_destrail.hpp
@@ -63,6 +63,41 @@ public:
 };
 
 template <class Types>
+class CYapfDestinationAnySafeTileRailT
+	: public CYapfDestinationRailBase
+{
+public:
+	typedef typename Types::Tpf Tpf;              ///< the pathfinder class (derived from THIS class)
+	typedef typename Types::NodeList::Titem Node; ///< this will be our node type
+	typedef typename Node::Key Key;               ///< key to hash tables
+
+	/// to access inherited path finder
+	Tpf& Yapf() {return *static_cast<Tpf*>(this);}
+
+	/// Called by YAPF to detect if node ends in the desired destination
+	FORCEINLINE bool PfDetectDestination(Node& n)
+	{
+		return PfDetectDestination(n.GetLastTile(), n.GetLastTrackdir());
+	}
+
+	/// Called by YAPF to detect if node ends in the desired destination
+	FORCEINLINE bool PfDetectDestination(TileIndex tile, Trackdir td)
+	{
+		return
+			IsSafeWaitingPosition(Yapf().GetVehicle(), tile, td, true, Types::TrackFollower::Allow90degTurns()) &&
+			IsWaitingPositionFree(Yapf().GetVehicle(), tile, td, Types::TrackFollower::Allow90degTurns());
+	}
+
+	/** Called by YAPF to calculate cost estimate. Calculates distance to the destination
+	 *  adds it to the actual cost from origin and stores the sum to the Node::m_estimate. */
+	FORCEINLINE bool PfCalcEstimate(Node& n)
+	{
+		n.m_estimate = n.m_cost;
+		return true;
+	}
+};
+
+template <class Types>
 class CYapfDestinationTileOrStationRailT
 	: public CYapfDestinationRailBase
 {
diff --git a/src/yapf/yapf_node_rail.hpp b/src/yapf/yapf_node_rail.hpp
index 6b42d30cf..192bce2ed 100644
--- a/src/yapf/yapf_node_rail.hpp
+++ b/src/yapf/yapf_node_rail.hpp
@@ -39,6 +39,7 @@ enum EndSegmentReason {
 	ESR_DEPOT,             ///< stop in the depot (could be a target next time)
 	ESR_WAYPOINT,          ///< waypoint encountered (could be a target next time)
 	ESR_STATION,           ///< station encountered (could be a target next time)
+	ESR_SAFE_TILE,         ///< safe waiting position found (could be a target)
 
 	/* The following reasons are used only internally by PfCalcCost().
 	*   They should not be found in the cached segment. */
@@ -62,6 +63,7 @@ enum EndSegmentReasonBits {
 	ESRB_DEPOT             = 1 << ESR_DEPOT,
 	ESRB_WAYPOINT          = 1 << ESR_WAYPOINT,
 	ESRB_STATION           = 1 << ESR_STATION,
+	ESRB_SAFE_TILE         = 1 << ESR_SAFE_TILE,
 
 	ESRB_PATH_TOO_LONG     = 1 << ESR_PATH_TOO_LONG,
 	ESRB_FIRST_TWO_WAY_RED = 1 << ESR_FIRST_TWO_WAY_RED,
@@ -71,10 +73,10 @@ enum EndSegmentReasonBits {
 	/* Additional (composite) values. */
 
 	/* What reasons mean that the target can be found and needs to be detected. */
-	ESRB_POSSIBLE_TARGET = ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION,
+	ESRB_POSSIBLE_TARGET = ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION | ESRB_SAFE_TILE,
 
 	/* What reasons can be stored back into cached segment. */
-	ESRB_CACHED_MASK = ESRB_DEAD_END | ESRB_RAIL_TYPE | ESRB_INFINITE_LOOP | ESRB_SEGMENT_TOO_LONG | ESRB_CHOICE_FOLLOWS | ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION,
+	ESRB_CACHED_MASK = ESRB_DEAD_END | ESRB_RAIL_TYPE | ESRB_INFINITE_LOOP | ESRB_SEGMENT_TOO_LONG | ESRB_CHOICE_FOLLOWS | ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION | ESRB_SAFE_TILE,
 
 	/* Reasons to abort pathfinding in this direction. */
 	ESRB_ABORT_PF_MASK = ESRB_DEAD_END | ESRB_PATH_TOO_LONG | ESRB_INFINITE_LOOP | ESRB_FIRST_TWO_WAY_RED,
diff --git a/src/yapf/yapf_rail.cpp b/src/yapf/yapf_rail.cpp
index f49d543b9..e4a29d78e 100644
--- a/src/yapf/yapf_rail.cpp
+++ b/src/yapf/yapf_rail.cpp
@@ -204,6 +204,88 @@ public:
 };
 
 template <class Types>
+class CYapfFollowAnySafeTileRailT : protected CYapfReserveTrack<Types>
+{
+public:
+	typedef typename Types::Tpf Tpf;                     ///< the pathfinder class (derived from THIS class)
+	typedef typename Types::TrackFollower TrackFollower;
+	typedef typename Types::NodeList::Titem Node;        ///< this will be our node type
+	typedef typename Node::Key Key;                      ///< key to hash tables
+
+protected:
+	/// to access inherited path finder
+	FORCEINLINE Tpf& Yapf() {return *static_cast<Tpf*>(this);}
+
+public:
+	/** Called by YAPF to move from the given node to the next tile. For each
+	 *  reachable trackdir on the new tile creates new node, initializes it
+	 *  and adds it to the open list by calling Yapf().AddNewNode(n) */
+	inline void PfFollowNode(Node& old_node)
+	{
+		TrackFollower F(Yapf().GetVehicle(), Yapf().GetCompatibleRailTypes());
+		if (F.Follow(old_node.GetLastTile(), old_node.GetLastTrackdir()))
+			Yapf().AddMultipleNodes(&old_node, F);
+	}
+
+	/** Return debug report character to identify the transportation type */
+	FORCEINLINE char TransportTypeChar() const {return 't';}
+
+	static bool stFindNearestSafeTile(const Vehicle *v, TileIndex t1, Trackdir td, bool override_railtype)
+	{
+		/* Create pathfinder instance */
+		Tpf pf1;
+#if !DEBUG_YAPF_CACHE
+		bool result1 = pf1.FindNearestSafeTile(v, t1, td, override_railtype, false);
+
+#else
+		bool result2 = pf1.FindNearestSafeTile(v, t1, td, override_railtype, true);
+		Tpf pf2;
+		pf2.DisableCache(true);
+		bool result1 = pf2.FindNearestSafeTile(v, t1, td, override_railtype, false);
+		if (result1 != result2) {
+			DEBUG(yapf, 0, "CACHE ERROR: FindSafeTile() = [%s, %s]", result2 ? "T" : "F", result1 ? "T" : "F");
+			DumpTarget dmp1, dmp2;
+			pf1.DumpBase(dmp1);
+			pf2.DumpBase(dmp2);
+			FILE *f1 = fopen("C:\\yapf1.txt", "wt");
+			FILE *f2 = fopen("C:\\yapf2.txt", "wt");
+			fwrite(dmp1.m_out.Data(), 1, dmp1.m_out.Size(), f1);
+			fwrite(dmp2.m_out.Data(), 1, dmp2.m_out.Size(), f2);
+			fclose(f1);
+			fclose(f2);
+		}
+#endif
+
+		return result1;
+	}
+
+	bool FindNearestSafeTile(const Vehicle *v, TileIndex t1, Trackdir td, bool override_railtype, bool dont_reserve)
+	{
+		/* Set origin and destination. */
+		Yapf().SetOrigin(t1, td);
+		Yapf().SetDestination(v, override_railtype);
+
+		bool bFound = Yapf().FindPath(v);
+		if (!bFound) return false;
+
+		/* Found a destination, set as reservation target. */
+		Node *pNode = Yapf().GetBestNode();
+		this->SetReservationTarget(pNode, pNode->GetLastTile(), pNode->GetLastTrackdir());
+
+		/* Walk through the path back to the origin. */
+		Node* pPrev = NULL;
+		while (pNode->m_parent != NULL) {
+			pPrev = pNode;
+			pNode = pNode->m_parent;
+
+			this->FindSafePositionOnNode(pPrev);
+		}
+
+		return dont_reserve || this->TryReservePath(NULL);
+	}
+};
+
+template <class Types>
 class CYapfFollowRailT : protected CYapfReserveTrack<Types>
 {
 public:
@@ -366,6 +448,9 @@ struct CYapfRail2         : CYapfT<CYapfRail_TypesT<CYapfRail2        , CFollowT
 struct CYapfAnyDepotRail1 : CYapfT<CYapfRail_TypesT<CYapfAnyDepotRail1, CFollowTrackRail    , CRailNodeListTrackDir, CYapfDestinationAnyDepotRailT     , CYapfFollowAnyDepotRailT> > {};
 struct CYapfAnyDepotRail2 : CYapfT<CYapfRail_TypesT<CYapfAnyDepotRail2, CFollowTrackRailNo90, CRailNodeListTrackDir, CYapfDestinationAnyDepotRailT     , CYapfFollowAnyDepotRailT> > {};
 
+struct CYapfAnySafeTileRail1 : CYapfT<CYapfRail_TypesT<CYapfAnySafeTileRail1, CFollowTrackFreeRail    , CRailNodeListTrackDir, CYapfDestinationAnySafeTileRailT , CYapfFollowAnySafeTileRailT> > {};
+struct CYapfAnySafeTileRail2 : CYapfT<CYapfRail_TypesT<CYapfAnySafeTileRail2, CFollowTrackFreeRailNo90, CRailNodeListTrackDir, CYapfDestinationAnySafeTileRailT , CYapfFollowAnySafeTileRailT> > {};
+
 
 Trackdir YapfChooseRailTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks, bool *path_not_found, bool reserve_track, PBSTileInfo *target)
 {
@@ -466,6 +551,19 @@ bool YapfFindNearestRailDepotTwoWay(const Vehicle *v, int max_distance, int reve
 	return ret;
 }
 
+bool YapfRailFindNearestSafeTile(const Vehicle *v, TileIndex tile, Trackdir td, bool override_railtype)
+{
+	typedef bool (*PfnFindNearestSafeTile)(const Vehicle*, TileIndex, Trackdir, bool);
+	PfnFindNearestSafeTile pfnFindNearestSafeTile = CYapfAnySafeTileRail1::stFindNearestSafeTile;
+
+	/* check if non-default YAPF type needed */
+	if (_settings_game.pf.forbid_90_deg) {
+		pfnFindNearestSafeTile = &CYapfAnySafeTileRail2::stFindNearestSafeTile;
+	}
+
+	return pfnFindNearestSafeTile(v, tile, td, override_railtype);
+}
+
 /** if any track changes, this counter is incremented - that will invalidate segment cost cache */
 int CSegmentCostCacheBase::s_rail_change_counter = 0;