Tweaks to default superelevation, adjustments to gravity and tilting to handle new system better

SteelFill · SteelFill · commit a7f2303c7aa7 · 2024-10-10T22:55:43.000-05:00
diff --git a/Source/Orts.Formats.Msts/RouteFile.cs b/Source/Orts.Formats.Msts/RouteFile.cs
@@ -240,7 +240,7 @@ public class SuperElevationStandard
         public float MaxPaxUnderbalanceM = float.PositiveInfinity; // Limit to be set elsewhere
         public float MinCantM = 0.010f; // Default 10 mm (0.5 inches on imperial routes)
         public float MaxCantM = 0.180f; // Default limit on superelevation is 180 mm (5 inches on imperial routes)
-        public float MinSpeedMpS = MpS.FromKpH(25.0f); // Default 25 kmh (15 mph on imperial routes)
+        public float MinSpeedMpS = MpS.FromKpH(25.5f); // Default 25 kmh (15 mph on imperial routes)
         public float MaxSpeedMpS = float.PositiveInfinity; // Default unlimited
         public float PrecisionM = 0.005f; // Default 5 mm (0.25 inches on imperial routes)
         public float RunoffSlope = 0.003f; // Maximum rate of change of superelevation per track length, default 0.3%
@@ -272,7 +272,7 @@ public SuperElevationStandard(bool metric = true, bool highSpeed = false)
                 // Set values in imperial units
                 MinCantM = Me.FromIn(0.5f);
                 MaxCantM = Me.FromIn(6.0f);
-                MinSpeedMpS = MpS.FromMpH(15.0f);
+                MinSpeedMpS = MpS.FromMpH(15.5f);
                 PrecisionM = Me.FromIn(0.25f);
 
                 if (highSpeed)
diff --git a/Source/Orts.Simulation/Simulation/Physics/Train.cs b/Source/Orts.Simulation/Simulation/Physics/Train.cs
@@ -4692,7 +4692,7 @@ public void CalculatePositionOfCars(float elapsedTime, float distance)
 
                     traveller.Move((car.CarLengthM - car.CarBogieCentreLengthM) / 2.0f);  // Move to the front of the car 
 
-                    car.UpdatedTraveler(traveller, elapsedTime, distance, SpeedMpS);
+                    car.UpdateVibration(traveller, elapsedTime, distance, SpeedMpS);
                 }
                 length += car.CarLengthM;
                 // update position of container in discrete freight animations
@@ -4785,7 +4785,7 @@ public void CalculatePositionOfEOT()
 
                 traveller.Move((car.CarLengthM - car.CarBogieCentreLengthM) / 2.0f);  // Move to the front of the car 
 
-                car.UpdatedTraveler(traveller, elapsedTime, distance, SpeedMpS);
+                car.UpdateVibration(traveller, elapsedTime, distance, SpeedMpS);
                 length += car.CarLengthM;
             }
             traveller = new Traveller(traveller, Traveller.TravellerDirection.Backward);
@@ -4821,7 +4821,7 @@ public void RecalculateRearTDBTraveller()
                 // advance to the front of the car 
                 traveller.Move(car.CarLengthM);
 
-                car.UpdatedTraveler(traveller, elapsedTime, distance, SpeedMpS);
+                car.UpdateVibration(traveller, elapsedTime, distance, SpeedMpS);
                 length += car.CarLengthM;
             }
             RearTDBTraveller = new Traveller(traveller);
diff --git a/Source/Orts.Simulation/Simulation/RollingStocks/TrainCar.cs b/Source/Orts.Simulation/Simulation/RollingStocks/TrainCar.cs
@@ -972,12 +972,6 @@ public virtual void Update(float elapsedClockSeconds)
             
             CarOutsideTempC = InitialCarOutsideTempC - TemperatureHeightVariationDegC;
 
-            // gravity force, M32 is up component of forward vector
-            // Percent slope = rise / run -> the Y position of the forward vector gives us the 'rise'
-            // Derive the 'run' by assuming a hypotenuse length of 1, so run = sqrt(1 - rise^2)
-            float rise = WorldPosition.XNAMatrix.M32;
-            GravityForceN = MassKG * GravitationalAccelerationMpS2 * rise;
-            CurrentElevationPercent = 100f * (rise / (float)Math.Sqrt(1 - rise * rise));
             AbsSpeedMpS = Math.Abs(_SpeedMpS);
 
             //TODO: next if block has been inserted to flip trainset physics in order to get viewing direction coincident with loco direction when using rear cab.
@@ -2172,7 +2166,7 @@ public virtual void Save(BinaryWriter outf)
             outf.Write(CouplerSlackM);
             outf.Write(Headlight);
             outf.Write(OrgConsist);
-            outf.Write(PrevTiltingZRot);
+            outf.Write(TiltingZRot);
             outf.Write(BrakesStuck);
             outf.Write(IsCarHeatingInitialized);
             outf.Write(SteamHoseLeakRateRandom);
@@ -2195,7 +2189,7 @@ public virtual void Restore(BinaryReader inf)
             CouplerSlackM = inf.ReadSingle();
             Headlight = inf.ReadInt32();
             OrgConsist = inf.ReadString();
-            PrevTiltingZRot = inf.ReadSingle();
+            TiltingZRot = inf.ReadSingle();
             BrakesStuck = inf.ReadBoolean();
             IsCarHeatingInitialized = inf.ReadBoolean();
             SteamHoseLeakRateRandom = inf.ReadSingle();
@@ -2758,6 +2752,8 @@ public void ComputePosition(Traveller traveler, bool backToFront, float elapsedT
 
             UpdateCurvePhys(new Traveller(traveler), BogieZOffsets);
 
+            int direction = traveler.Direction == Traveller.TravellerDirection.Forward ? -1 : 1;
+
             if (Flipped == backToFront)
             {
                 float o = -CarLengthM / 2 - CentreOfGravityM.Z;
@@ -2773,13 +2769,12 @@ public void ComputePosition(Traveller traveler, bool backToFront, float elapsedT
                     location.X += 2048 * (traveler.TileX - tileX);
                     location.Z += 2048 * (traveler.TileZ - tileZ);
 
-                    // This car is flipped, so flip roll direction.
-                    r *= -1;
-
-                    WheelAxles[k].Part.AddWheelSetLocation(1, o, location, r);
+                    // This car is flipped, so flip roll direction in part
+                    WheelAxles[k].Part.AddWheelSetLocation(1, o, location, -r);
                 }
                 o = CarLengthM / 2 - CentreOfGravityM.Z - o;
                 traveler.Move(o);
+                direction *= -1;
             }
             else
             {
@@ -2823,7 +2818,7 @@ public void ComputePosition(Traveller traveler, bool backToFront, float elapsedT
                     p.FindCenterLine();
                 }
             }
-            // Determine facing direction of train car
+            // Determine facing direction and position of train car
             p0.FindCenterLine();
             Vector3 fwd = new Vector3(p0.Dir[0], p0.Dir[1], -p0.Dir[2]);
             // Check if null (0-length) vector
@@ -2839,16 +2834,23 @@ public void ComputePosition(Traveller traveler, bool backToFront, float elapsedT
             m.Up = up;
             m.Backward = fwd;
 
+            // Update gravity force when position is updated, but before any secondary motion is added
+            GravityForceN = MassKG * GravitationalAccelerationMpS2 * fwd.Y;
+            CurrentElevationPercent = 100f * (fwd.Y / (float)Math.Sqrt(1 - fwd.Y * fwd.Y));
+
+            // Consider body roll from superelevation and from tilting.
+            UpdateTilting(traveler, elapsedTimeS, speed, direction);
             Matrix rollMat = Matrix.Identity;
+            float rollAngle = p0.Roll + TiltingZRot;
 
-            if (p0.Roll != 0.0f)
+            if (rollAngle != 0.0f)
             {
                 // For correct bogie positioning, need to offset rotation axis
                 Vector3 offset = new Vector3(0.0f, BogiePivotHeightM, 0.0f);
 
                 // Roll the car for superelevation about the offset axis of rotation
                 rollMat.Translation -= offset;
-                rollMat *= Matrix.CreateRotationZ(p0.Roll);
+                rollMat *= Matrix.CreateRotationZ(rollAngle);
                 rollMat.Translation += offset;
 
                 m = rollMat * m;
@@ -2863,19 +2865,36 @@ public void ComputePosition(Traveller traveler, bool backToFront, float elapsedT
             WorldPosition.TileX = tileX;
             WorldPosition.TileZ = tileZ;
 
-            UpdatedTraveler(traveler, elapsedTimeS, distance, speed);
+            UpdateVibration(traveler, elapsedTimeS, distance, speed);
         }
 
         #region Traveller-based updates
         public float CurrentCurveRadiusM;
 
-        internal void UpdatedTraveler(Traveller traveler, float elapsedTimeS, float distanceM, float speedMpS)
+        internal void UpdateTilting(Traveller traveller,  float elapsedTimeS, float speedMpS, int direction)
         {
-            // We need to avoid introducing any unbounded effects, so cap the elapsed time to 0.25 seconds (4FPS).
-            if (elapsedTimeS > 0.25f)
+            // If not a tilting train, skip processing this
+            // Future: Rework tilting to be per train car, instead of per consist
+            if (!Train.IsTilting)
                 return;
 
-            UpdateVibrationAndTilting(traveler, elapsedTimeS, distanceM, speedMpS);
+            float tiltDemand = 0.0f;
+
+            // No tilt needed if going too slow, or not on a curve
+            if (speedMpS > MinTiltSpeedMpS && CurrentCurveRadiusM != 0.0f)
+            {
+                // Compare actual superelevation to ideal, tilt makes up the difference
+                // Sine of superelevation angle is v^2 / (g * r)
+                float idealElevAngle = (speedMpS * speedMpS) / (GravitationalAccelerationMpS2 * CurrentCurveRadiusM);
+                idealElevAngle = (float)Math.Asin(Math.Min(idealElevAngle, 0.99f));
+
+                tiltDemand = MathHelper.Clamp(idealElevAngle - SuperElevationAngleRad, 0.0f, MaxTiltAngleRad);
+
+                tiltDemand *= direction * traveller.CurveDirection;
+            }
+
+            // Smooth rotation
+            TiltingZRot += (tiltDemand - TiltingZRot) * elapsedTimeS; 
         }
         #endregion
 
@@ -2953,16 +2972,21 @@ public void UpdateCurvePhys(Traveller traveller, float[] offsets)
         int VibrationTrackVectorSection;
         float VibrationTrackCurvaturepM;
 
-        float PrevTiltingZRot; // previous tilting angle
-        float TiltingZRot; // actual tilting angle
+        float TiltingZRot; // Actual tilting angle
+        float MinTiltSpeedMpS = MpS.FromKpH(50.0f); // Minimum speed for tilting to be activated
+        float MaxTiltAngleRad = MathHelper.ToRadians(8.0f); // Maximum angle of tilting allowed
 
-        internal void UpdateVibrationAndTilting(Traveller traveler, float elapsedTimeS, float distanceM, float speedMpS)
+        internal void UpdateVibration(Traveller traveler, float elapsedTimeS, float distanceM, float speedMpS)
         {
+            // We need to avoid introducing any unbounded effects, so cap the elapsed time to 0.25 seconds (4FPS).
+            if (elapsedTimeS > 0.25f)
+                return;
             // NOTE: Traveller is at the FRONT of the TrainCar!
 
             // Don't add vibrations to train cars less than 2.5 meter in length; they're unsuitable for these calculations.
             // Don't let vibrate car before EOT to avoid EOT not moving together with that car
-            if (CarLengthM < 2.5f || Train.EOT != null && Train.Cars.Count > 1 && Train.Cars[Train.Cars.Count - 2] == this) return;
+            if (CarLengthM < 2.5f || Train.EOT != null && Train.Cars.Count > 1 && Train.Cars[Train.Cars.Count - 2] == this)
+                return;
             if (Simulator.Settings.CarVibratingLevel != 0)
             {
 
@@ -3031,18 +3055,9 @@ internal void UpdateVibrationAndTilting(Traveller traveler, float elapsedTimeS,
                     AddVibrations(VibrationFactorTrackNode);
                     VibrationTrackNode = traveler.TrackNodeIndex;
                 }
-            }
-            if (Train != null && Train.IsTilting)
-            {
-                TiltingZRot = traveler.FindTiltedZ(speedMpS);//rotation if tilted, an indication of centrifugal force
-                TiltingZRot = PrevTiltingZRot + (TiltingZRot - PrevTiltingZRot) * elapsedTimeS;//smooth rotation
-                PrevTiltingZRot = TiltingZRot;
-                if (this.Flipped) TiltingZRot *= -1f;
-            }
-            if (Simulator.Settings.CarVibratingLevel != 0 || Train.IsTilting)
-            {
-                var rotation = Matrix.CreateFromYawPitchRoll(VibrationRotationRad.Y, VibrationRotationRad.X, VibrationRotationRad.Z + TiltingZRot);
-                var translation = Matrix.CreateTranslation(VibrationTranslationM.X, VibrationTranslationM.Y, 0);
+
+                Matrix rotation = Matrix.CreateFromYawPitchRoll(VibrationRotationRad.Y, VibrationRotationRad.X, VibrationRotationRad.Z);
+                Matrix translation = Matrix.CreateTranslation(VibrationTranslationM.X, VibrationTranslationM.Y, 0);
                 WorldPosition.XNAMatrix = rotation * translation * WorldPosition.XNAMatrix;
                 VibrationInverseMatrix = Matrix.Invert(rotation * translation);
             }
diff --git a/Source/Orts.Simulation/Simulation/Traveller.cs b/Source/Orts.Simulation/Simulation/Traveller.cs
@@ -132,6 +132,10 @@ public TravellerDirection Direction
         /// </summary>
         public bool IsTrackCurved { get { return IsTrack && trackSection != null && trackSection.SectionCurve != null; } }
         /// <summary>
+        /// Returns the direction of curvature based on track section angle. Returns 0 if not on a curve.
+        /// </summary>
+        public int CurveDirection { get { return (trackSection != null && trackSection.SectionCurve != null) ? Math.Sign(trackSection.SectionCurve.Angle) : 0; } }
+        /// <summary>
         /// Returns whether this traveller is currently on a section of track which is straight.
         /// </summary>
         public bool IsTrackStraight { get { return IsTrack && (trackSection == null || trackSection.SectionCurve == null); } }
@@ -1113,32 +1117,6 @@ public Vector3 GetTrackDirection()
             return directionVector;
         }
 
-        public float FindTiltedZ(float speed) //will test 1 second ahead, computed will return desired elev. only
-        {
-            if (speed < 12) return 0;//no tilt if speed too low (<50km/h)
-            var tn = trackNode;
-            if (tn.TrVectorNode == null) return 0f;
-            var tvs = trackVectorSection;
-            var ts = trackSection;
-            var desiredZ = 0f;
-            if (tvs == null)
-            {
-                desiredZ = 0f;
-            }
-            else if (ts.SectionCurve != null)
-            {
-                float maxv = 0.14f * speed / 40f;//max 8 degree
-                //maxv *= speed / 40f;
-                //if (maxv.AlmostEqual(0f, 0.001f)) maxv = 0.02f; //short curve, add some effect anyway
-                var sign = -Math.Sign(ts.SectionCurve.Angle);
-                if ((this.direction == TravellerDirection.Forward ? 1 : -1) * sign > 0) desiredZ = 1f;
-                else desiredZ = -1f;
-                desiredZ *= maxv;//max elevation
-            }
-            else desiredZ = 0f;
-            return desiredZ;
-        }
-
         /// <summary>
         /// Finds the nearest junction node in the direction this traveller is facing.
         /// </summary>
