diff --git a/map.c b/map.c
--- a/map.c
+++ b/map.c
@@ -138,35 +138,49 @@ const TileIndexDiffC _tileoffs_by_dir[] 
 
 uint DistanceManhattan(TileIndex t0, TileIndex t1)
 {
-	return
-		abs(TileX(t0) - TileX(t1)) +
-		abs(TileY(t0) - TileY(t1));
+	const uint dx = abs(TileX(t0) - TileX(t1));
+	const uint dy = abs(TileY(t0) - TileY(t1));
+	return dx + dy;
 }
 
 
 uint DistanceSquare(TileIndex t0, TileIndex t1)
 {
-	const int x = TileX(t0) - TileX(t1);
-	const int y = TileY(t0) - TileY(t1);
-	return x * x + y * y;
+	const int dx = TileX(t0) - TileX(t1);
+	const int dy = TileY(t0) - TileY(t1);
+	return dx * dx + dy * dy;
 }
 
 
 uint DistanceMax(TileIndex t0, TileIndex t1)
 {
-	const uint x = abs(TileX(t0) - TileX(t1));
-	const uint y = abs(TileY(t0) - TileY(t1));
-	return x > y ? x : y;
+	const uint dx = abs(TileX(t0) - TileX(t1));
+	const uint dy = abs(TileY(t0) - TileY(t1));
+	return dx > dy ? dx : dy;
 }
 
 
 uint DistanceMaxPlusManhattan(TileIndex t0, TileIndex t1)
 {
-	const uint x = abs(TileX(t0) - TileX(t1));
-	const uint y = abs(TileY(t0) - TileY(t1));
-	return x > y ? 2 * x + y : 2 * y + x;
+	const uint dx = abs(TileX(t0) - TileX(t1));
+	const uint dy = abs(TileY(t0) - TileY(t1));
+	return dx > dy ? 2 * dx + dy : 2 * dy + dx;
 }
 
+uint DistanceTrack(TileIndex t0, TileIndex t1)
+{
+	const uint dx = abs(TileX(t0) - TileX(t1));
+	const uint dy = abs(TileY(t0) - TileY(t1));
+
+	const uint straightTracks = 2 * min(dx, dy); /* The number of straight (not full length) tracks */
+	/* OPTIMISATION:
+	 * Original: diagTracks = max(dx, dy) - min(dx,dy);
+	 * Proof:
+	 * (dx-dy) - straightTracks  == (min + max) - straightTracks = min + // max - 2 * min = max - min */
+	const uint diagTracks = dx + dy - straightTracks; /* The number of diagonal (full tile length) tracks. */
+
+	return diagTracks + straightTracks * STRAIGHT_TRACK_LENGTH;
+}
 
 uint DistanceFromEdge(TileIndex tile)
 {
diff --git a/map.h b/map.h
--- a/map.h
+++ b/map.h
@@ -102,10 +102,11 @@ static inline TileIndex AddTileIndexDiff
 }
 
 // Functions to calculate distances
-uint DistanceManhattan(TileIndex, TileIndex); // also known as L1-Norm
+uint DistanceManhattan(TileIndex, TileIndex); // also known as L1-Norm. Is the shortest distance one could go over diagonal tracks (or roads)
 uint DistanceSquare(TileIndex, TileIndex); // euclidian- or L2-Norm squared
 uint DistanceMax(TileIndex, TileIndex); // also known as L-Infinity-Norm
 uint DistanceMaxPlusManhattan(TileIndex, TileIndex); // Max + Manhattan
+uint DistanceTrack(TileIndex, TileIndex); // Returns the shortest distance one could go over tracks
 uint DistanceFromEdge(TileIndex); // shortest distance from any edge of the map
 
 
@@ -117,4 +118,10 @@ static inline TileIndexDiff TileOffsByDi
 	return ToTileIndexDiff(_tileoffs_by_dir[dir]);
 }
 
+/* Approximation of the length of a straight track, relative to a diagonal
+ * track (ie the size of a tile side). #defined instead of const so it can
+ * stay integer. (no runtime float operations) Is this needed?
+ * This value should be sqrt(2)/2 ~ 0.7071 */
+#define STRAIGHT_TRACK_LENGTH (7071/10000)
+
 #endif
diff --git a/npf.c b/npf.c
--- a/npf.c
+++ b/npf.c
@@ -93,7 +93,7 @@ const byte _reverse_trackdir[14] = {
 /* The cost of each trackdir. A diagonal piece is the full NPF_TILE_LENGTH,
  * the shorter piece is sqrt(2)/2*NPF_TILE_LENGTH =~ 0.7071
  */
-#define NPF_STRAIGHT_LENGTH (uint)(NPF_TILE_LENGTH * 0.7071)
+#define NPF_STRAIGHT_LENGTH (uint)(NPF_TILE_LENGTH * STRAIGHT_TRACK_LENGTH)
 static const uint _trackdir_length[14] = {
 	NPF_TILE_LENGTH, NPF_TILE_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH,
 	0, 0,
@@ -154,36 +154,8 @@ TileIndex CalcClosestStationTile(int sta
 	return TILE_XY(tx,ty);
 };
 
-/* Calcs the heuristic to the target tile (using NPFFindStationOrTileData).
- * If the target is a station, the heuristic is probably "wrong"! Normally
- * this shouldn't matter, but if it turns out to be a problem, we could use
- * the heuristic below?
- * Afterthis  will save the heuristic into NPFFoundTargetData if it is the
- * smallest until now. It will then also save
- * AyStarNode.user_data[NPF_TRACKDIR_CHOICE] in best_trackdir
- */
-int32 NPFCalcTileHeuristic(AyStar* as, AyStarNode* current, OpenListNode* parent) {
-	NPFFindStationOrTileData* fstd = (NPFFindStationOrTileData*)as->user_target;
-	NPFFoundTargetData* ftd = (NPFFoundTargetData*)as->user_path;
-	TileIndex from = current->tile;
-	TileIndex to = fstd->dest_coords;
-	uint dist = DistanceManhattan(from, to) * NPF_TILE_LENGTH;
-
-	if (dist < ftd->best_bird_dist) {
-		ftd->best_bird_dist = dist;
-		ftd->best_trackdir = current->user_data[NPF_TRACKDIR_CHOICE];
-	}
-#ifdef NPF_DEBUG
-	debug("Calculating H for: (%d, %d). Result: %d", TileX(current->tile), TileY(current->tile), dist);
-#endif
-	return dist;
-}
-
-/* Calcs the heuristic to the target station or tile. Almost the same as above
- * function, but calculates the distance to train stations with
- * CalcClosestStationTile instead. So is somewhat more correct for stations
- * (truly optimistic), but this added correctness is not really required we
- * believe (matthijs & Hackykid)
+/* Calcs the heuristic to the target station or tile. For train stations, it
+ * takes into account the direction of approach.
  */
 int32 NPFCalcStationOrTileHeuristic(AyStar* as, AyStarNode* current, OpenListNode* parent) {
 	NPFFindStationOrTileData* fstd = (NPFFindStationOrTileData*)as->user_target;
@@ -196,7 +168,12 @@ int32 NPFCalcStationOrTileHeuristic(AySt
 	if ((as->user_data[NPF_TYPE] == TRANSPORT_RAIL) && (fstd->station_index != -1))
 		to = CalcClosestStationTile(fstd->station_index, from);
 
-	dist = DistanceManhattan(from, to) * NPF_TILE_LENGTH;
+	if (as->user_data[NPF_TYPE] == TRANSPORT_ROAD)
+		/* Since roads only have diagonal pieces, we use manhattan distance here */
+		dist = DistanceManhattan(from, to) * NPF_TILE_LENGTH;
+	else
+		/* Ships and trains can also go diagonal, so the minimum distance is shorter */
+		dist = DistanceTrack(from, to) * NPF_TILE_LENGTH;
 
 	if (dist < ftd->best_bird_dist) {
 		ftd->best_bird_dist = dist;