internal static void UpdateBogieTopplingCantAndSpring(Train Train, int CarIndex, double TimeElapsed)
{
if (TimeElapsed == 0.0 | TimeElapsed > 0.5)
{
return;
}
//Same hack: Check if any car sections are defined for the offending bogie
if (Train.Cars[CarIndex].FrontBogie.CarSections.Length != 0)
{
//FRONT BOGIE
// get direction, up and side vectors
double dx, dy, dz;
double ux, uy, uz;
double sx, sy, sz;
{
dx = Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.X -
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.X;
dy = Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Y -
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Y;
dz = Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Z -
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Z;
double t = 1.0/Math.Sqrt(dx*dx + dy*dy + dz*dz);
dx *= t;
dy *= t;
dz *= t;
t = 1.0/Math.Sqrt(dx*dx + dz*dz);
double ex = dx*t;
double ez = dz*t;
sx = ez;
sy = 0.0;
sz = -ex;
World.Cross(dx, dy, dz, sx, sy, sz, out ux, out uy, out uz);
}
// cant and radius
//TODO: Hopefully we can apply the base toppling and roll figures from the car itself
// apply position due to cant/toppling
{
double a = Train.Cars[CarIndex].Specs.CurrentRollDueToTopplingAngle +
Train.Cars[CarIndex].Specs.CurrentRollDueToCantAngle;
double x = Math.Sign(a)*0.5*Game.RouteRailGauge*(1.0 - Math.Cos(a));
double y = Math.Abs(0.5*Game.RouteRailGauge*Math.Sin(a));
double cx = sx*x + ux*y;
double cy = sy*x + uy*y;
double cz = sz*x + uz*y;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Z += cz;
}
// apply rolling
{
double a = -Train.Cars[CarIndex].Specs.CurrentRollDueToTopplingAngle -
Train.Cars[CarIndex].Specs.CurrentRollDueToCantAngle;
double cosa = Math.Cos(a);
double sina = Math.Sin(a);
World.Rotate(ref sx, ref sy, ref sz, dx, dy, dz, cosa, sina);
World.Rotate(ref ux, ref uy, ref uz, dx, dy, dz, cosa, sina);
Train.Cars[CarIndex].FrontBogie.Up.X = ux;
Train.Cars[CarIndex].FrontBogie.Up.Y = uy;
Train.Cars[CarIndex].FrontBogie.Up.Z = uz;
}
// apply pitching
if (Train.Cars[CarIndex].FrontBogie.CurrentCarSection >= 0 &&
Train.Cars[CarIndex].FrontBogie.CarSections[Train.Cars[CarIndex].FrontBogie.CurrentCarSection].Overlay)
{
double a = Train.Cars[CarIndex].Specs.CurrentPitchDueToAccelerationAngle;
double cosa = Math.Cos(a);
double sina = Math.Sin(a);
World.Rotate(ref dx, ref dy, ref dz, sx, sy, sz, cosa, sina);
World.Rotate(ref ux, ref uy, ref uz, sx, sy, sz, cosa, sina);
double cx = 0.5*
(Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.X +
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.X);
double cy = 0.5*
(Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Y +
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Y);
double cz = 0.5*
(Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Z +
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Z);
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.X -= cx;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Y -= cy;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Z -= cz;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.X -= cx;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Y -= cy;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Z -= cz;
World.Rotate(ref Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition, sx, sy, sz, cosa, sina);
World.Rotate(ref Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition, sx, sy, sz, cosa, sina);
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].FrontBogie.FrontAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].FrontBogie.RearAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].FrontBogie.Up.X = ux;
Train.Cars[CarIndex].FrontBogie.Up.Y = uy;
Train.Cars[CarIndex].FrontBogie.Up.Z = uz;
}
}
//Same hack: Check if any car sections are defined for the offending bogie
if (Train.Cars[CarIndex].RearBogie.CarSections.Length != 0)
{
//REAR BOGIE
// get direction, up and side vectors
double dx, dy, dz;
double ux, uy, uz;
double sx, sy, sz;
{
dx = Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.X -
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.X;
dy = Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Y -
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Y;
dz = Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Z -
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Z;
double t = 1.0 / Math.Sqrt(dx * dx + dy * dy + dz * dz);
dx *= t;
dy *= t;
dz *= t;
t = 1.0 / Math.Sqrt(dx * dx + dz * dz);
double ex = dx * t;
double ez = dz * t;
sx = ez;
sy = 0.0;
sz = -ex;
World.Cross(dx, dy, dz, sx, sy, sz, out ux, out uy, out uz);
}
// cant and radius
//TODO: Hopefully we can apply the base toppling and roll etc. figures from the car itself
// apply position due to cant/toppling
{
double a = Train.Cars[CarIndex].Specs.CurrentRollDueToTopplingAngle +
Train.Cars[CarIndex].Specs.CurrentRollDueToCantAngle;
double x = Math.Sign(a) * 0.5 * Game.RouteRailGauge * (1.0 - Math.Cos(a));
double y = Math.Abs(0.5 * Game.RouteRailGauge * Math.Sin(a));
double cx = sx * x + ux * y;
double cy = sy * x + uy * y;
double cz = sz * x + uz * y;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Z += cz;
}
// apply rolling
{
double a = -Train.Cars[CarIndex].Specs.CurrentRollDueToTopplingAngle -
Train.Cars[CarIndex].Specs.CurrentRollDueToCantAngle;
double cosa = Math.Cos(a);
double sina = Math.Sin(a);
World.Rotate(ref sx, ref sy, ref sz, dx, dy, dz, cosa, sina);
World.Rotate(ref ux, ref uy, ref uz, dx, dy, dz, cosa, sina);
Train.Cars[CarIndex].RearBogie.Up.X = ux;
Train.Cars[CarIndex].RearBogie.Up.Y = uy;
Train.Cars[CarIndex].RearBogie.Up.Z = uz;
}
// apply pitching
if (Train.Cars[CarIndex].RearBogie.CurrentCarSection >= 0 &&
Train.Cars[CarIndex].RearBogie.CarSections[Train.Cars[CarIndex].RearBogie.CurrentCarSection].Overlay)
{
double a = Train.Cars[CarIndex].Specs.CurrentPitchDueToAccelerationAngle;
double cosa = Math.Cos(a);
double sina = Math.Sin(a);
World.Rotate(ref dx, ref dy, ref dz, sx, sy, sz, cosa, sina);
World.Rotate(ref ux, ref uy, ref uz, sx, sy, sz, cosa, sina);
double cx = 0.5 *
(Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.X +
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.X);
double cy = 0.5 *
(Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Y +
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Y);
double cz = 0.5 *
(Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Z +
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Z);
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.X -= cx;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Y -= cy;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Z -= cz;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.X -= cx;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Y -= cy;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Z -= cz;
World.Rotate(ref Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition, sx, sy, sz, cosa, sina);
World.Rotate(ref Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition, sx, sy, sz, cosa, sina);
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].RearBogie.FrontAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.X += cx;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Y += cy;
Train.Cars[CarIndex].RearBogie.RearAxle.Follower.WorldPosition.Z += cz;
Train.Cars[CarIndex].RearBogie.Up.X = ux;
Train.Cars[CarIndex].RearBogie.Up.Y = uy;
Train.Cars[CarIndex].RearBogie.Up.Z = uz;
}
}
}