--- a/src/train_cmd.cpp
+++ b/src/train_cmd.cpp
@@ -3026,8 +3026,6 @@
 	ReverseTrainDirection(v);
 }
 
-extern TileIndex CheckTunnelBusy(TileIndex tile, uint *length);
-
 /**
  * Deletes/Clears the last wagon of a crashed train. It takes the engine of the
  * train, then goes to the last wagon and deletes that. Each call to this function
@@ -3062,9 +3060,9 @@
 	DisableTrainCrossing(v->tile);
 
 	if ((v->u.rail.track == TRACK_BIT_WORMHOLE && v->vehstatus & VS_HIDDEN)) { // inside a tunnel
-		TileIndex endtile = CheckTunnelBusy(v->tile, NULL);
-
-		if (endtile == INVALID_TILE) return; // tunnel is busy (error returned)
+		TileIndex endtile = GetOtherTunnelEnd(v->tile);
+
+		if (GetVehicleTunnelBridge(v->tile, endtile) != NULL) return; // tunnel is busy (error returned)
 
 		switch (v->direction) {
 			case 1:

--- a/src/tunnelbridge_cmd.cpp
+++ b/src/tunnelbridge_cmd.cpp
@@ -561,34 +561,6 @@
 	return cost;
 }
 
-TileIndex CheckTunnelBusy(TileIndex tile, uint *length)
-{
-	uint z = GetTileZ(tile);
-	DiagDirection dir = GetTunnelDirection(tile);
-	TileIndexDiff delta = TileOffsByDiagDir(dir);
-	uint len = 0;
-	TileIndex starttile = tile;
-	Vehicle *v;
-
-	do {
-		tile += delta;
-		len++;
-	} while (
-		!IsTunnelTile(tile) ||
-		ReverseDiagDir(GetTunnelDirection(tile)) != dir ||
-		GetTileZ(tile) != z
-	);
-
-	v = FindVehicleBetween(starttile, tile, z);
-	if (v != NULL) {
-		_error_message = v->type == VEH_TRAIN ?
-			STR_5000_TRAIN_IN_TUNNEL : STR_5001_ROAD_VEHICLE_IN_TUNNEL;
-		return INVALID_TILE;
-	}
-
-	if (length != NULL) *length = len;
-	return tile;
-}
 
 static inline bool CheckAllowRemoveTunnelBridge(TileIndex tile)
 {
@@ -605,14 +577,14 @@
 {
 	Town *t = NULL;
 	TileIndex endtile;
-	uint length;
 
 	SET_EXPENSES_TYPE(EXPENSES_CONSTRUCTION);
 
 	if (!CheckAllowRemoveTunnelBridge(tile)) return CMD_ERROR;
 
-	endtile = CheckTunnelBusy(tile, &length);
-	if (endtile == INVALID_TILE) return CMD_ERROR;
+	endtile = GetOtherTunnelEnd(tile);
+
+	if (GetVehicleTunnelBridge(tile, endtile) != NULL) return CMD_ERROR;
 
 	_build_tunnel_endtile = endtile;
 
@@ -645,22 +617,10 @@
 		YapfNotifyTrackLayoutChange(tile, track);
 		YapfNotifyTrackLayoutChange(endtile, track);
 	}
-	return CommandCost(_price.clear_tunnel * (length + 1));
+	return CommandCost(_price.clear_tunnel * (DistanceManhattan(tile, endtile) + 1));
 }
 
 
-static bool IsVehicleOnBridge(TileIndex starttile, TileIndex endtile, uint z)
-{
-	const Vehicle *v;
-	FOR_ALL_VEHICLES(v) {
-		if ((v->tile == starttile || v->tile == endtile) && v->z_pos == z) {
-			_error_message = VehicleInTheWayErrMsg(v);
-			return true;
-		}
-	}
-	return false;
-}
-
 static CommandCost DoClearBridge(TileIndex tile, uint32 flags)
 {
 	DiagDirection direction;
@@ -673,13 +633,8 @@
 	if (!CheckAllowRemoveTunnelBridge(tile)) return CMD_ERROR;
 
 	endtile = GetOtherBridgeEnd(tile);
-	byte bridge_height = GetBridgeHeight(tile);
 
-	if (FindVehicleOnTileZ(tile, bridge_height) != NULL ||
-			FindVehicleOnTileZ(endtile, bridge_height) != NULL ||
-			IsVehicleOnBridge(tile, endtile, bridge_height)) {
-		return CMD_ERROR;
-	}
+	if (GetVehicleTunnelBridge(tile, endtile) != NULL) return CMD_ERROR;
 
 	direction = GetBridgeRampDirection(tile);
 	delta = TileOffsByDiagDir(direction);
@@ -747,16 +702,12 @@
 CommandCost DoConvertTunnelBridgeRail(TileIndex tile, RailType totype, bool exec)
 {
 	if (IsTunnel(tile) && GetTunnelTransportType(tile) == TRANSPORT_RAIL) {
-		uint length;
-		TileIndex endtile;
+		TileIndex endtile = GetOtherTunnelEnd(tile);
 
 		/* If not coverting rail <-> el. rail, any vehicle cannot be in tunnel */
-		if (!IsCompatibleRail(GetRailType(tile), totype)) {
-			endtile = CheckTunnelBusy(tile, &length);
-			if (endtile == INVALID_TILE) return CMD_ERROR;
-		} else {
-			endtile = GetOtherTunnelEnd(tile);
-			length = DistanceManhattan(tile, endtile);
+		if (!IsCompatibleRail(GetRailType(tile), totype) &&
+				GetVehicleTunnelBridge(tile, endtile) != NULL) {
+			return CMD_ERROR;
 		}
 
 		if (exec) {
@@ -774,15 +725,12 @@
 			VehicleFromPos(endtile, &endtile, UpdateTrainPowerProc);
 		}
 
-		return CommandCost((length + 1) * RailConvertCost(GetRailType(tile), totype));
+		return CommandCost((DistanceManhattan(tile, endtile) + 1) * RailConvertCost(GetRailType(tile), totype));
 	} else if (IsBridge(tile) && GetBridgeTransportType(tile) == TRANSPORT_RAIL) {
 		TileIndex endtile = GetOtherBridgeEnd(tile);
-		byte bridge_height = GetBridgeHeight(tile);
 
 		if (!IsCompatibleRail(GetRailType(tile), totype) &&
-				(FindVehicleOnTileZ(tile, bridge_height) != NULL ||
-				FindVehicleOnTileZ(endtile, bridge_height) != NULL ||
-				IsVehicleOnBridge(tile, endtile, bridge_height))) {
+				GetVehicleTunnelBridge(tile, endtile) != NULL) {
 			return CMD_ERROR;
 		}
 

--- a/src/vehicle.cpp
+++ b/src/vehicle.cpp
@@ -175,6 +175,41 @@
 }
 
 
+/** Struct used for GetVehicleTunnelBridge() */
+struct TunnelBridgeInfo {
+	TileIndex tile;                      ///< tile
+};
+
+/** Procedure called for every vehicle found in tunnel/bridge in the hash map */
+static void *GetVehicleTunnelBridgeProc(Vehicle *v, void *data)
+{
+	TunnelBridgeInfo *tbi = (TunnelBridgeInfo*)data;
+
+	if (v->tile != tbi->tile || (v->type != VEH_TRAIN && v->type != VEH_ROAD)) return NULL;
+
+	_error_message = VehicleInTheWayErrMsg(v);
+	return v;
+}
+
+/**
+ * Finds vehicle in tunnel / bridge
+ * @param tile first end
+ * @param endtile second end
+ * @return pointer to vehicle found
+ */
+Vehicle *GetVehicleTunnelBridge(TileIndex tile, TileIndex endtile)
+{
+	TunnelBridgeInfo tbi = {tile};
+
+	Vehicle *v = (Vehicle*)VehicleFromPos(tile, &tbi, &GetVehicleTunnelBridgeProc);
+	if (v != NULL) return v;
+
+	tbi.tile = endtile;
+
+	return (Vehicle*)VehicleFromPos(endtile, &tbi, &GetVehicleTunnelBridgeProc);
+}
+
+
 static void UpdateVehiclePosHash(Vehicle* v, int x, int y);
 
 void VehiclePositionChanged(Vehicle *v)

--- a/src/vehicle.h
+++ b/src/vehicle.h
@@ -610,6 +610,7 @@
 
 StringID VehicleInTheWayErrMsg(const Vehicle* v);
 Vehicle *FindVehicleBetween(TileIndex from, TileIndex to, byte z, bool without_crashed = false);
+Vehicle *GetVehicleTunnelBridge(TileIndex tile, TileIndex endtile);
 
 bool UpdateSignalsOnSegment(TileIndex tile, DiagDirection direction);
 void SetSignalsOnBothDir(TileIndex tile, byte track);