Add: Penalty for occupied docking points.

2 files changed, 34 insertions, 0 deletions

diff --git a/src/pathfinder/npf/npf.cpp b/src/pathfinder/npf/npf.cpp
index 53f84f2a7..f10e4cd9e 100644
--- a/src/pathfinder/npf/npf.cpp
+++ b/src/pathfinder/npf/npf.cpp
@@ -14,6 +14,7 @@
 #include "../../viewport_func.h"
 #include "../../ship.h"
 #include "../../roadstop_base.h"
+#include "../../vehicle_func.h"
 #include "../pathfinder_func.h"
 #include "../pathfinder_type.h"
 #include "../follow_track.hpp"
@@ -303,6 +304,15 @@ static void NPFMarkTile(TileIndex tile)
 	}
 }
 
+static Vehicle *CountShipProc(Vehicle *v, void *data)
+{
+	uint *count = (uint *)data;
+	/* Ignore other vehicles (aircraft) and ships inside depot. */
+	if (v->type == VEH_SHIP && (v->vehstatus & VS_HIDDEN) == 0) (*count)++;
+
+	return nullptr;
+}
+
 static int32 NPFWaterPathCost(AyStar *as, AyStarNode *current, OpenListNode *parent)
 {
 	/* TileIndex tile = current->tile; */
@@ -319,6 +329,13 @@ static int32 NPFWaterPathCost(AyStar *as, AyStarNode *current, OpenListNode *par
 		cost += _settings_game.pf.npf.npf_water_curve_penalty;
 	}
 
+	if (IsDockingTile(current->tile)) {
+		/* Check docking tile for occupancy */
+		uint count = 1;
+		HasVehicleOnPos(current->tile, &count, &CountShipProc);
+		cost += count * 3 * _trackdir_length[trackdir];
+	}
+
 	/* @todo More penalties? */
 
 	return cost;
diff --git a/src/pathfinder/yapf/yapf_ship.cpp b/src/pathfinder/yapf/yapf_ship.cpp
index c665fc53e..6e0775505 100644
--- a/src/pathfinder/yapf/yapf_ship.cpp
+++ b/src/pathfinder/yapf/yapf_ship.cpp
@@ -12,6 +12,7 @@
 #include "../../stdafx.h"
 #include "../../ship.h"
 #include "../../industry.h"
+#include "../../vehicle_func.h"
 
 #include "yapf.hpp"
 #include "yapf_node_ship.hpp"
@@ -261,6 +262,15 @@ public:
 		return 0;
 	}
 
+	static Vehicle *CountShipProc(Vehicle *v, void *data)
+	{
+		uint *count = (uint *)data;
+		/* Ignore other vehicles (aircraft) and ships inside depot. */
+		if (v->type == VEH_SHIP && (v->vehstatus & VS_HIDDEN) == 0) (*count)++;
+
+		return nullptr;
+	}
+
 	/**
 	 * Called by YAPF to calculate the cost from the origin to the given node.
 	 *  Calculates only the cost of given node, adds it to the parent node cost
@@ -273,6 +283,13 @@ public:
 		/* additional penalty for curves */
 		c += CurveCost(n.m_parent->GetTrackdir(), n.GetTrackdir());
 
+		if (IsDockingTile(n.GetTile())) {
+			/* Check docking tile for occupancy */
+			uint count = 1;
+			HasVehicleOnPos(n.GetTile(), &count, &CountShipProc);
+			c += count * 3 * YAPF_TILE_LENGTH;
+		}
+
 		/* Skipped tile cost for aqueducts. */
 		c += YAPF_TILE_LENGTH * tf->m_tiles_skipped;