--- a/yapf/yapf_common.hpp	Sun Dec 31 21:47:51 2006 +0000
+++ b/yapf/yapf_common.hpp	Sun Dec 31 23:48:04 2006 +0000
@@ -50,24 +50,24 @@
 	typedef typename Node::Key Key;               ///< key to hash tables
 
 protected:
-	TileIndex   m_orgTile;                        ///< first origin tile
-	Trackdir    m_orgTd;                          ///< first origin trackdir
-	TileIndex   m_revTile;                        ///< second (reversed) origin tile
-	Trackdir    m_revTd;                          ///< second (reversed) origin trackdir
-	int         m_reverse_penalty;                ///< penalty to be added for using the reversed origin
-	bool        m_treat_first_red_two_way_signal_as_eol; ///< in some cases (leaving station) we need to handle first two-way signal differently
+	TileIndex    m_org_tile;                        ///< first origin tile
+	TrackdirBits m_org_td_bits;                    ///< first origin trackdirbits
+	TileIndex    m_rev_tile;                        ///< second (reversed) origin tile
+	TrackdirBits m_rev_td_bits;                   ///< second (reversed) origin trackdir
+	int          m_reverse_penalty;                ///< penalty to be added for using the reversed origin
+	bool         m_treat_first_red_two_way_signal_as_eol; ///< in some cases (leaving station) we need to handle first two-way signal differently
 
 	/// to access inherited path finder
 	FORCEINLINE Tpf& Yapf() {return *static_cast<Tpf*>(this);}
 
 public:
 	/// set origin (tiles, trackdirs, etc.)
-	void SetOrigin(TileIndex tile, Trackdir td, TileIndex tiler = INVALID_TILE, Trackdir tdr = INVALID_TRACKDIR, int reverse_penalty = 0, bool treat_first_red_two_way_signal_as_eol = true)
+	void SetOrigin(TileIndex tile, TrackdirBits td_bits, TileIndex tiler = INVALID_TILE, TrackdirBits tdr_bits = INVALID_TRACKDIR_BIT, int reverse_penalty = 0, bool treat_first_red_two_way_signal_as_eol = true)
 	{
-		m_orgTile = tile;
-		m_orgTd = td;
-		m_revTile = tiler;
-		m_revTd = tdr;
+		m_org_tile = tile;
+		m_org_td_bits = td_bits;
+		m_rev_tile = tiler;
+		m_rev_td_bits = tdr_bits;
 		m_reverse_penalty = reverse_penalty;
 		m_treat_first_red_two_way_signal_as_eol = treat_first_red_two_way_signal_as_eol;
 	}
@@ -75,16 +75,22 @@
 	/// Called when YAPF needs to place origin nodes into open list
 	void PfSetStartupNodes()
 	{
-		if (m_orgTile != INVALID_TILE && m_orgTd != INVALID_TRACKDIR) {
-			Node& n1 = Yapf().CreateNewNode();
-			n1.Set(NULL, m_orgTile, m_orgTd, false);
-			Yapf().AddStartupNode(n1);
+		if (m_org_tile != INVALID_TILE && m_org_td_bits != INVALID_TRACKDIR_BIT) {
+			for (TrackdirBits td_bits = m_org_td_bits; td_bits != TRACKDIR_BIT_NONE; td_bits = (TrackdirBits)KillFirstBit2x64(td_bits)) {
+				Node& n = Yapf().CreateNewNode();
+				Trackdir td = (Trackdir)FindFirstBit2x64(td_bits);
+				n.Set(NULL, m_org_tile, td, false);
+				Yapf().AddStartupNode(n);
+			}
 		}
-		if (m_revTile != INVALID_TILE && m_revTd != INVALID_TRACKDIR) {
-			Node& n2 = Yapf().CreateNewNode();
-			n2.Set(NULL, m_revTile, m_revTd, false);
-			n2.m_cost = m_reverse_penalty;
-			Yapf().AddStartupNode(n2);
+		if (m_rev_tile != INVALID_TILE && m_rev_td_bits != INVALID_TRACKDIR_BIT) {
+			for (TrackdirBits td_bits = m_rev_td_bits; td_bits != TRACKDIR_BIT_NONE; td_bits = (TrackdirBits)KillFirstBit2x64(td_bits)) {
+				Node& n = Yapf().CreateNewNode();
+				Trackdir td = (Trackdir)FindFirstBit2x64(td_bits);
+				n.Set(NULL, m_rev_tile, td, false);
+				n.m_cost = m_reverse_penalty;
+				Yapf().AddStartupNode(n);
+			}
 		}
 	}
 

--- a/yapf/yapf_rail.cpp	Sun Dec 31 21:47:51 2006 +0000
+++ b/yapf/yapf_rail.cpp	Sun Dec 31 23:48:04 2006 +0000
@@ -49,7 +49,7 @@
 	FORCEINLINE bool FindNearestDepotTwoWay(Vehicle *v, TileIndex t1, Trackdir td1, TileIndex t2, Trackdir td2, int max_distance, int reverse_penalty, TileIndex* depot_tile, bool* reversed)
 	{
 		// set origin and destination nodes
-		Yapf().SetOrigin(t1, td1, t2, td2, reverse_penalty, true);
+		Yapf().SetOrigin(t1, TrackdirToTrackdirBits(td1), t2, TrackdirToTrackdirBits(td2), reverse_penalty, true);
 		Yapf().SetDestination(v);
 		Yapf().SetMaxCost(YAPF_TILE_LENGTH * max_distance);
 
@@ -113,7 +113,14 @@
 	FORCEINLINE Trackdir ChooseRailTrack(Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs, bool *path_not_found)
 	{
 		// set origin and destination nodes
-		Yapf().SetOrigin(v->tile, GetVehicleTrackdir(v), INVALID_TILE, INVALID_TRACKDIR, 1, true);
+		if (v->tile == tile) {
+			// probably bridge head
+			trackdirs = (TrackdirBits)(GetTileTrackStatus(tile, TRANSPORT_RAIL) & TRACKDIR_BIT_MASK);
+			trackdirs &= DiagdirReachesTrackdirs(ReverseDiagDir(TrackdirToExitdir(ReverseTrackdir(GetVehicleTrackdir(v)))));
+			Yapf().SetOrigin(tile, trackdirs, INVALID_TILE, INVALID_TRACKDIR_BIT, 1, true);
+		} else {
+			Yapf().SetOrigin(v->tile, TrackdirToTrackdirBits(GetVehicleTrackdir(v)), INVALID_TILE, INVALID_TRACKDIR_BIT, 1, true);
+		}
 		Yapf().SetDestination(v);
 
 		// find the best path
@@ -135,8 +142,8 @@
 				pNode = pNode->m_parent;
 			}
 			// return trackdir from the best origin node (one of start nodes)
-			Node& best_next_node = *pPrev;
-			assert(best_next_node.GetTile() == tile);
+			Node& best_next_node = (v->tile == tile ? *pNode : *pPrev);
+			assert(v->tile == tile || best_next_node.GetTile() == tile);
 			next_trackdir = best_next_node.GetTrackdir();
 		}
 		return next_trackdir;
@@ -152,7 +159,7 @@
 	{
 		// create pathfinder instance
 		// set origin and destination nodes
-		Yapf().SetOrigin(t1, td1, t2, td2, 1, false);
+		Yapf().SetOrigin(t1, TrackdirToTrackdirBits(td1), t2, TrackdirToTrackdirBits(td2), 1, false);
 		Yapf().SetDestination(v);
 
 		// find the best path