--- a/roadveh_cmd.c	Mon May 29 18:39:42 2006 +0000
+++ b/roadveh_cmd.c	Tue May 30 10:53:27 2006 +0000
@@ -1149,14 +1149,26 @@
 
 static uint RoadFindPathToStop(const Vehicle *v, TileIndex tile)
 {
-	NPFFindStationOrTileData fstd;
-	byte trackdir = GetVehicleTrackdir(v);
-	assert(trackdir != 0xFF);
+	uint dist = UINT_MAX;
+	if (_patches.yapf.road_use_yapf) {
+		// use YAPF
+		dist = YapfRoadVehDistanceToTile(v, tile);
+	} else {
+		// use NPF
+		NPFFindStationOrTileData fstd;
+		byte trackdir = GetVehicleTrackdir(v);
+		uint dist = UINT_MAX;
+		assert(trackdir != 0xFF);
 
-	fstd.dest_coords = tile;
-	fstd.station_index = INVALID_STATION; // indicates that the destination is a tile, not a station
+		fstd.dest_coords = tile;
+		fstd.station_index = INVALID_STATION; // indicates that the destination is a tile, not a station
 
-	return NPFRouteToStationOrTile(v->tile, trackdir, &fstd, TRANSPORT_ROAD, v->owner, INVALID_RAILTYPE).best_path_dist;
+		dist = NPFRouteToStationOrTile(v->tile, trackdir, &fstd, TRANSPORT_ROAD, v->owner, INVALID_RAILTYPE).best_path_dist;
+		// change units from NPF_TILE_LENGTH to # of tiles
+		if (dist != UINT_MAX)
+			dist = (dist + NPF_TILE_LENGTH - 1) / NPF_TILE_LENGTH;
+	}
+	return dist;
 }
 
 typedef struct RoadDriveEntry {
@@ -1652,7 +1664,7 @@
 						DEBUG(ms, 4) (" ---- stop 0x%X is not reachable, not treating further", rs->xy);
 						continue;
 					}
-					badness = (rs->num_vehicles + 1) * (rs->num_vehicles + 1) + dist / NPF_TILE_LENGTH;
+					badness = (rs->num_vehicles + 1) * (rs->num_vehicles + 1) + dist;
 
 					DEBUG(ms, 4) (" ---- stop 0x%X has %d vehicle%s waiting", rs->xy, rs->num_vehicles, rs->num_vehicles == 1 ? "":"s");
 					DEBUG(ms, 4) (" ---- Distance is %u", dist);

--- a/yapf/follow_track.hpp	Mon May 29 18:39:42 2006 +0000
+++ b/yapf/follow_track.hpp	Tue May 30 10:53:27 2006 +0000
@@ -13,12 +13,12 @@
 {
 	CPerformanceTimer* m_pPerf;
 
-	FORCEINLINE CFollowTrackT(Vehicle* v = NULL, CPerformanceTimer* pPerf = NULL)
+	FORCEINLINE CFollowTrackT(const Vehicle* v = NULL, CPerformanceTimer* pPerf = NULL)
 	{
 		Init(v, pPerf);
 	}
 
-	FORCEINLINE void Init(Vehicle* v, CPerformanceTimer* pPerf)
+	FORCEINLINE void Init(const Vehicle* v, CPerformanceTimer* pPerf)
 	{
 		assert(!IsRailTT() || (v != NULL && v->type == VEH_Train));
 		m_veh = v;

--- a/yapf/yapf.h	Mon May 29 18:39:42 2006 +0000
+++ b/yapf/yapf.h	Tue May 30 10:53:27 2006 +0000
@@ -9,6 +9,8 @@
 Trackdir YapfChooseRoadTrack(Vehicle *v, TileIndex tile, DiagDirection enterdir);
 Trackdir YapfChooseRailTrack(Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackdirBits trackdirs);
 
+uint YapfRoadVehDistanceToTile(const Vehicle* v, TileIndex tile);
+
 Depot* YapfFindNearestRoadDepot(const Vehicle *v);
 bool YapfFindNearestRailDepotTwoWay(Vehicle *v, int max_distance, int reverse_penalty, TileIndex* depot_tile, bool* reversed);
 
@@ -27,7 +29,7 @@
 /** Base struct for track followers. */
 typedef struct FollowTrack_t
 {
-	Vehicle*      m_veh;
+	const Vehicle*      m_veh;
 	TileIndex     m_old_tile;
 	Trackdir      m_old_td;
 	TileIndex     m_new_tile;

--- a/yapf/yapf_base.hpp	Mon May 29 18:39:42 2006 +0000
+++ b/yapf/yapf_base.hpp	Tue May 30 10:53:27 2006 +0000
@@ -57,7 +57,7 @@
 	Node*                m_pBestIntermediateNode; ///< here should be node closest to the destination if path not found
 	const YapfSettings  *m_settings;           ///< current settings (_patches.yapf)
 	int                  m_max_search_nodes;   ///< maximum number of nodes we are allowed to visit before we give up
-	Vehicle*             m_veh;                ///< vehicle that we are trying to drive
+	const Vehicle*       m_veh;                ///< vehicle that we are trying to drive
 
 	int                  m_stats_cost_calcs;   ///< stats - how many node's costs were calculated
 	int                  m_stats_cache_hits;   ///< stats - how many node's costs were reused from cache
@@ -111,7 +111,7 @@
 					- or the open list is empty (no route to destination).
 					- or the maximum amount of loops reached - m_max_search_nodes (default = 10000)
 			@return true if the path was found */
-	inline bool FindPath(Vehicle* v)
+	inline bool FindPath(const Vehicle* v)
 	{
 		m_veh = v;
 
@@ -271,7 +271,7 @@
 		m_nodes.InsertOpenNode(n);
 	}
 
-	Vehicle* GetVehicle() const {return m_veh;}
+	const Vehicle* GetVehicle() const {return m_veh;}
 
 	// methods that should be implemented at derived class Types::Tpf (derived from CYapfBaseT)
 

--- a/yapf/yapf_costrail.hpp	Mon May 29 18:39:42 2006 +0000
+++ b/yapf/yapf_costrail.hpp	Tue May 30 10:53:27 2006 +0000
@@ -152,7 +152,7 @@
 		int first_tile_cost = 0;
 		int segment_cost = 0;
 		int extra_cost = 0;
-		Vehicle* v = Yapf().GetVehicle();
+		const Vehicle* v = Yapf().GetVehicle();
 
 		// start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment
 		TileIndex prev_tile      = (n.m_parent != NULL) ? n.m_parent->GetLastTile() : INVALID_TILE;

--- a/yapf/yapf_road.cpp	Mon May 29 18:39:42 2006 +0000
+++ b/yapf/yapf_road.cpp	Tue May 30 10:53:27 2006 +0000
@@ -97,7 +97,7 @@
 			// add min/max speed penalties
 			int min_speed = 0;
 			int max_speed = F.GetSpeedLimit(&min_speed);
-			Vehicle* v = Yapf().GetVehicle();
+			const Vehicle* v = Yapf().GetVehicle();
 			if (max_speed < v->max_speed) segment_cost += 1 * (v->max_speed - max_speed);
 			if (min_speed > v->max_speed) segment_cost += 10 * (min_speed - v->max_speed);
 
@@ -287,6 +287,46 @@
 		return next_trackdir;
 	}
 
+	static uint stDistanceToTile(const Vehicle *v, TileIndex tile)
+	{
+		Tpf pf;
+		return pf.DistanceToTile(v, tile);
+	}
+
+	FORCEINLINE uint DistanceToTile(const Vehicle *v, TileIndex dst_tile)
+	{
+		// handle special case - when current tile is the destination tile
+		if (dst_tile == v->tile) {
+			// distance is zero in this case
+			return 0;
+		}
+
+		// set origin (tile, trackdir)
+		TileIndex src_tile = v->tile;
+		Trackdir src_td = GetVehicleTrackdir(v);
+		Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td));
+
+		// set destination tile, trackdir
+		//   get available trackdirs on the destination tile
+		uint dest_ts = GetTileTrackStatus(dst_tile, TRANSPORT_ROAD);
+		TrackdirBits dst_td_bits = (TrackdirBits)(dest_ts & TRACKDIR_BIT_MASK);
+		Yapf().SetDestination(dst_tile, dst_td_bits);
+
+		// find the best path
+		Yapf().FindPath(v);
+
+		// if path not found - return distance = UINT_MAX
+		uint dist = UINT_MAX;
+		Node* pNode = &Yapf().GetBestNode();
+		if (pNode != NULL) {
+			// path was found or at least suggested
+			// get the path cost estimate
+			dist = pNode->GetCostEstimate();
+		}
+
+		return dist;
+	}
+
 	static Depot* stFindNearestDepot(Vehicle* v, TileIndex tile, Trackdir td)
 	{
 		Tpf pf;
@@ -349,6 +389,25 @@
 	return td_ret;
 }
 
+uint YapfRoadVehDistanceToTile(const Vehicle* v, TileIndex tile)
+{
+	// default is YAPF type 2
+	typedef uint (*PfnDistanceToTile)(const Vehicle*, TileIndex);
+	PfnDistanceToTile pfnDistanceToTile = &CYapfRoad2::stDistanceToTile; // default: ExitDir, allow 90-deg
+
+	// check if non-default YAPF type should be used
+	if (_patches.yapf.disable_node_optimization)
+		pfnDistanceToTile = &CYapfRoad1::stDistanceToTile; // Trackdir, allow 90-deg
+
+	// measure distance in YAPF units
+	uint dist = pfnDistanceToTile(v, tile);
+	// convert distance to tiles
+	if (dist != UINT_MAX)
+		dist = (dist + 10 - 1) / 10; // TODO: change road YAPF unit from 10 to YAPF_TILE_LENGTH
+
+	return dist;
+}
+
 Depot* YapfFindNearestRoadDepot(const Vehicle *v)
 {
 	TileIndex tile = v->tile;