public void UpdateTopplingCantAndSpring()
{
if (CarSections.Length != 0)
{
//FRONT BOGIE
// get direction, up and side vectors
Vector3 d = new Vector3(FrontAxle.Follower.WorldPosition - RearAxle.Follower.WorldPosition);
Vector3 s;
{
double t = 1.0 / d.Norm();
d *= t;
t = 1.0 / Math.Sqrt(d.X * d.X + d.Z * d.Z);
double ex = d.X * t;
double ez = d.Z * t;
s = new Vector3(ez, 0.0, -ex);
Up = Vector3.Cross(d, s);
}
// cant and radius
//TODO: This currently uses the figures from the base car
// apply position due to cant/toppling
{
double a = baseCar.Specs.RollDueToTopplingAngle +
baseCar.Specs.RollDueToCantAngle;
double x = Math.Sign(a) * 0.5 * TrainManagerBase.currentHost.Tracks[FrontAxle.Follower.TrackIndex].RailGauge * (1.0 - Math.Cos(a));
double y = Math.Abs(0.5 * TrainManagerBase.currentHost.Tracks[FrontAxle.Follower.TrackIndex].RailGauge * Math.Sin(a));
Vector3 c = new Vector3(s.X * x + Up.X * y, s.Y * x + Up.Y * y, s.Z * x + Up.Z * y);
FrontAxle.Follower.WorldPosition += c;
RearAxle.Follower.WorldPosition += c;
}
// apply rolling
{
s.Rotate(d, -baseCar.Specs.RollDueToTopplingAngle - baseCar.Specs.RollDueToCantAngle);
Up.Rotate(d, -baseCar.Specs.RollDueToTopplingAngle - baseCar.Specs.RollDueToCantAngle);
}
// apply pitching
if (CurrentCarSection >= 0 && CarSections[CurrentCarSection].Groups[0].Type == ObjectType.Overlay)
{
d.Rotate(s, baseCar.Specs.PitchDueToAccelerationAngle);
Up.Rotate(s, baseCar.Specs.PitchDueToAccelerationAngle);
Vector3 cc = 0.5 * (FrontAxle.Follower.WorldPosition + RearAxle.Follower.WorldPosition);
FrontAxle.Follower.WorldPosition -= cc;
RearAxle.Follower.WorldPosition -= cc;
FrontAxle.Follower.WorldPosition.Rotate(s, baseCar.Specs.PitchDueToAccelerationAngle);
RearAxle.Follower.WorldPosition.Rotate(s, baseCar.Specs.PitchDueToAccelerationAngle);
FrontAxle.Follower.WorldPosition += cc;
RearAxle.Follower.WorldPosition += cc;
}
}
}