3 files changed, 102 insertions, 5 deletions

diff --git a/src/ground_vehicle.hpp b/src/ground_vehicle.hpp
index 4fa9eb4fa..141f99731 100644
--- a/src/ground_vehicle.hpp
+++ b/src/ground_vehicle.hpp
@@ -148,16 +148,73 @@ struct GroundVehicle : public SpecializedVehicle<T, Type> {
 	/**
 	 * Updates vehicle's Z position.
 	 * Inclination can't change in the middle of a tile.
+	 * The faster code is used for trains and road vehicles unless they are
+	 * reversing on a sloped tile.
 	 */
 	FORCEINLINE void UpdateZPosition()
 	{
-		/* Vehicle's Z position can change only if it has GVF_GOINGUP_BIT or GVF_GOINGDOWN_BIT set */
+#if 0
+		/* The following code does this: */
+
+		if (HasBit(this->gv_flags, GVF_GOINGUP_BIT)) {
+			switch (this->direction) {
+				case DIR_NE:
+					this->z_pos += (this->x_pos & 1); break;
+				case DIR_SW:
+					this->z_pos += (this->x_pos & 1) ^ 1; break;
+				case DIR_NW:
+					this->z_pos += (this->y_pos & 1); break;
+				case DIR_SE:
+					this->z_pos += (this->y_pos & 1) ^ 1; break;
+				default: break;
+			}
+		} else if (HasBit(this->gv_flags, GVF_GOINGDOWN_BIT)) {
+			switch (this->direction) {
+				case DIR_NE:
+					this->z_pos -= (this->x_pos & 1); break;
+				case DIR_SW:
+					this->z_pos -= (this->x_pos & 1) ^ 1; break;
+				case DIR_NW:
+					this->z_pos -= (this->y_pos & 1); break;
+				case DIR_SE:
+					this->z_pos -= (this->y_pos & 1) ^ 1; break;
+				default: break;
+			}
+		}
+
+		/* But gcc 4.4.5 isn't able to nicely optimise it, and the resulting
+		 * code is full of conditional jumps. */
+#endif
+
+		/* Vehicle's Z position can change only if it has GVF_GOINGUP_BIT or GVF_GOINGDOWN_BIT set.
+		 * Furthermore, if this function is called once every time the vehicle's position changes,
+		 * we know the Z position changes by +/-1 at certain moments - when x_pos, y_pos is odd/even,
+		 * depending on orientation of the slope and vehicle's direction */
+
 		if (HasBit(this->gv_flags, GVF_GOINGUP_BIT) || HasBit(this->gv_flags, GVF_GOINGDOWN_BIT)) {
-			this->z_pos = GetSlopeZ(this->x_pos, this->y_pos);
-		} else {
-			/* Verify that assumption. */
-			assert(this->z_pos == GetSlopeZ(this->x_pos, this->y_pos));
+			if (T::From(this)->HasToUseGetSlopeZ()) {
+				/* In some cases, we have to use GetSlopeZ() */
+				this->z_pos = GetSlopeZ(this->x_pos, this->y_pos);
+				return;
+			}
+			/* DirToDiagDir() is a simple right shift */
+			DiagDirection dir = DirToDiagDir(this->direction);
+			/* Read variables, so the compiler knows the access doesn't trap */
+			int8 x_pos = this->x_pos;
+			int8 y_pos = this->y_pos;
+			/* DiagDirToAxis() is a simple mask */
+			int8 d = DiagDirToAxis(dir) == AXIS_X ? x_pos : y_pos;
+			/* We need only the least significant bit */
+			d &= 1;
+			/* Conditional "^ 1". Optimised to "(dir - 1) <= 1". */
+			d ^= (int8)(dir == DIAGDIR_SW || dir == DIAGDIR_SE);
+			/* Subtraction instead of addition because we are testing for GVF_GOINGUP_BIT.
+			 * GVF_GOINGUP_BIT is used because it's bit 0, so simple AND can be used,
+			 * without any shift */
+			this->z_pos += HasBit(this->gv_flags, GVF_GOINGUP_BIT) ? d : -d;
 		}
+
+		assert(this->z_pos == GetSlopeZ(this->x_pos, this->y_pos));
 	}
 
 	/**
diff --git a/src/roadveh.h b/src/roadveh.h
index c73575a13..e49badbec 100644
--- a/src/roadveh.h
+++ b/src/roadveh.h
@@ -262,6 +262,36 @@ protected: // These functions should not be called outside acceleration code.
 
 		return trackbits == TRACK_BIT_X || trackbits == TRACK_BIT_Y;
 	}
+
+	/**
+	 * Road vehicles have to use GetSlopeZ() to compute their height
+	 * if they are reversing because in that case, their direction
+	 * is not parallel with the road. It is safe to return \c true
+	 * even if it is not reversing.
+	 * @return are we (possibly) reversing?
+	 */
+	FORCEINLINE bool HasToUseGetSlopeZ()
+	{
+		const RoadVehicle *rv = this->First();
+
+		/* Check if this vehicle is in the same direction as the road under.
+		 * We already know it has either GVF_GOINGUP_BIT or GVF_GOINGDOWN_BIT set. */
+
+		if (rv->state <= RVSB_TRACKDIR_MASK && IsReversingRoadTrackdir((Trackdir)rv->state)) {
+			/* If the first vehicle is reversing, this vehicle may be reversing too
+			 * (especially if this is the first, and maybe the only, vehicle).*/
+			return true;
+		}
+
+		while (rv != this) {
+			/* If any previous vehicle has different direction,
+			 * we may be in the middle of reversing. */
+			if (this->direction != rv->direction) return true;
+			rv = rv->Next();
+		}
+
+		return false;
+	}
 };
 
 #define FOR_ALL_ROADVEHICLES(var) FOR_ALL_VEHICLES_OF_TYPE(RoadVehicle, var)
diff --git a/src/train.h b/src/train.h
index bfeffa554..f7142e47a 100644
--- a/src/train.h
+++ b/src/train.h
@@ -380,6 +380,16 @@ protected: // These functions should not be called outside acceleration code.
 		/* Any track that isn't TRACK_BIT_X or TRACK_BIT_Y cannot be sloped. */
 		return this->track == TRACK_BIT_X || this->track == TRACK_BIT_Y;
 	}
+
+	/**
+	 * Trains can always use the faster algorithm because they
+	 * have always the same direction as the track under them.
+	 * @return false
+	 */
+	FORCEINLINE bool HasToUseGetSlopeZ()
+	{
+		return false;
+	}
 };
 
 #define FOR_ALL_TRAINS(var) FOR_ALL_VEHICLES_OF_TYPE(Train, var)