public GhostCommit updateGhostPlayer(uint ID, NearbyInfoResponse response, GhostCommit towCommit, FsConnect _fsConnect, MathClass _mathClass, double absoluteTime)
{
int index = 0;
double deltaTime = 0;
foreach (GhostPlane ghostPlane in ghostPlanes)
{
if (ghostPlane.ID == ID && ghostPlane.Progress > 0 && ghostPlane.TrackPoints.Count > 0)
{
deltaTime = absoluteTime - ghostPlane.LastTrackPlayed;
TrackPoint prev = new TrackPoint();
TrackPoint curr = new TrackPoint();
TrackPoint next = new TrackPoint();
bool found = false;
foreach (var point in ghostPlane.TrackPoints)
{
if (!found)
{
curr = point;
}
else
{
next = point;
break;
}
if (prev.Timer < (ghostPlane.Progress + deltaTime) && point.Timer >= (ghostPlane.Progress + deltaTime))
{
found = true;
if (!string.IsNullOrEmpty(curr.Message) && lastMessage != curr.Message)
{
message = new KeyValuePair <uint, string>(ghostPlane.ID, point.Message);
lastMessage = point.Message;
}
}
else
{
prev = point;
}
}
if (found && prev.Location != null && curr.Location != null && next.Location != null)
{
try
{
double progress = ((ghostPlane.Progress + deltaTime) - prev.Timer) / (curr.Timer - prev.Timer);
double timeLeft = curr.Timer - (ghostPlane.Progress + deltaTime);
double distancePrev = _mathClass.findDistanceBetweenPoints(response.Latitude, response.Longitude, prev.Location.Latitude, prev.Location.Longitude);
double distanceCurr = _mathClass.findDistanceBetweenPoints(response.Latitude, response.Longitude, curr.Location.Latitude, curr.Location.Longitude);
double distanceNext = _mathClass.findDistanceBetweenPoints(response.Latitude, response.Longitude, next.Location.Latitude, next.Location.Longitude);
double bearing = normalizeRadAngle(_mathClass.findBearingToPoint(response.Latitude, response.Longitude, next.Location.Latitude, next.Location.Longitude));
response.Heading = normalizeRadAngle(response.Heading);
bearing = zeroeRadAngle(bearing - response.Heading);
double newHeading = zeroeRadAngle(response.Heading + (absoluteTime - ghostPlane.LastTrackPlayed) * bearing);
towCommit.RotationVelocityBodyY = Math.Sin(newHeading - response.Heading);
//Console.WriteLine($"Tracking heading: {response.Heading:F4} bearing: {bearing:F4} newHeading: {newHeading:F4}");
double requiredBank = ((1 - progress) * (prev.Roll * Math.PI / 180 - response.Bank) + progress * (curr.Roll * Math.PI / 180 - response.Bank)) / 2;
towCommit.RotationVelocityBodyZ = Math.Sin(requiredBank - response.Bank);
double requiredPitch = ((1 - progress) * (prev.Pitch * Math.PI / 180 - response.Pitch) + progress * (prev.Pitch * Math.PI / 180 - response.Pitch)) / 2;
towCommit.RotationVelocityBodyX = Math.Sin(requiredPitch - response.Pitch);
//Console.WriteLine($"RotationVelocityBodyX: {towCommit.RotationVelocityBodyX:F4} RotationVelocityBodyY: {towCommit.RotationVelocityBodyY:F4} RotationVelocityBodyZ: {towCommit.RotationVelocityBodyZ:F4}");
//towCommit.Heading = newHeading;
towCommit.VelocityBodyX = 0;
towCommit.VelocityBodyY = ((1 - progress) * (prev.Elevation - response.Altitude) + progress * (curr.Elevation - response.Altitude)) / 2;
towCommit.VelocityBodyZ = (0.8 * curr.Velocity + 0.1 * distanceCurr / Math.Max(1, timeLeft)) * (Math.Abs(distanceCurr) < 10 ? Math.Abs(distanceCurr) / 10 : 1);
// TAXIING
if (towCommit.VelocityBodyZ < 1 && towCommit.VelocityBodyZ > -1)
{
towCommit.VelocityBodyZ = Math.Sign(towCommit.VelocityBodyZ) * Math.Pow(Math.Abs(towCommit.VelocityBodyZ), 4);
}
if (towCommit.VelocityBodyY > 10)
{
TowInfoResponse towInfo = new TowInfoResponse();
towInfo.Altitude = response.Altitude + towCommit.VelocityBodyY;
towInfo.Latitude = response.Latitude;
towInfo.Longitude = response.Longitude;
towInfo.Heading = response.Heading;
towInfo.Bank = response.Bank;
if (!double.IsNaN(towInfo.Altitude) && !double.IsNaN(towInfo.Latitude) && !double.IsNaN(towInfo.Longitude) && !double.IsNaN(towInfo.Heading) && !double.IsNaN(towInfo.Bank))
{
try
{
Console.WriteLine("Sinking, TELEPORT!");
_fsConnect.UpdateData(Definitions.TowPlane, towInfo, ID);
}
catch (Exception ex)
{
}
}
}
//Console.WriteLine("Tracking animation " + (ghostPlane.Progress + deltaTime) + " h" + towCommit.VelocityBodyZ + " v" + towCommit.VelocityBodyY + " d" + distanceCurr);
}
catch { }
}
else if (curr.Location != null && next.Location == null)
{
message = new KeyValuePair <uint, string>(ghostPlane.ID, "REMOVE");
}
break;
}
index++;
}
if (index < ghostPlanes.Count)
{
GhostPlane gp = ghostPlanes[index];
gp.LastTrackPlayed = absoluteTime;
gp.Progress += deltaTime;
ghostPlanes[index] = gp;
}
return(towCommit);
}