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);
}
public KeyValuePair <double, string> buildTrackFile(string appName, string nickName, PlaneInfoResponse _planeInfoResponse, PlaneAvionicsResponse _planeAvionicsResponse, MathClass _mathClass, string filename, bool timeAligned = false)
{
string str = "<?xml version=\"1.0\" encoding=\"UTF-8\"?><gpx creator=\"" + appName + "\" version=\"1.0\"><trk><name>" + _planeAvionicsResponse.Title + " - " + nickName + "</name><desc></desc><trkseg>";
TrackPoint prev = new TrackPoint(new GeoLocation(0, 0), 0, 0, 0, 0, 0, 0, 0, 0, 0, DateTime.Now, 0);
double distance = 0;
foreach (TrackPoint trackPoint in trackRecording)
{
if (prev.Elevation != 0)
{
double flatDistance = _mathClass.findDistanceBetweenPoints(prev.Location.Latitude, prev.Location.Longitude, trackPoint.Location.Latitude, trackPoint.Location.Longitude);
distance += flatDistance / Math.Cos((prev.Elevation - trackPoint.Elevation) / flatDistance);
}
DateTime timestamp = !timeAligned ? trackPoint.Time : (new DateTime(2000, 1, 1)).AddSeconds(trackPoint.Timer);
str += Environment.NewLine + "<trkpt lon=\"" + (trackPoint.Location.Longitude * 180 / Math.PI).ToString(CultureInfo.InvariantCulture) + "\" lat=\"" + (trackPoint.Location.Latitude * 180 / Math.PI).ToString(CultureInfo.InvariantCulture) + "\">" +
Environment.NewLine + " <ele>" + trackPoint.Elevation.ToString(CultureInfo.InvariantCulture) + "</ele>" +
Environment.NewLine + " <agl>" + trackPoint.AltitudeAboveGround.ToString() + "</agl>" +
Environment.NewLine + " <velocity>" + trackPoint.Velocity.ToString(CultureInfo.InvariantCulture) + "</velocity>" +
Environment.NewLine + " <airspeed>" + trackPoint.Airspeed.ToString(CultureInfo.InvariantCulture) + "</airspeed>" +
Environment.NewLine + " <heading>" + trackPoint.Heading + "</heading>" +
Environment.NewLine + " <pitch>" + trackPoint.Pitch + "</pitch>" +
Environment.NewLine + " <roll>" + trackPoint.Roll + "</roll>" +
Environment.NewLine + " <lights>" + trackPoint.Lights.ToString() + "</lights>" +
Environment.NewLine + " <avionics>" + trackPoint.Avionics.ToString() + "</avionics>" +
Environment.NewLine + " <time>" + timestamp.ToString("O") + "Z" + "</time>" +
Environment.NewLine + "</trkpt>";
prev = trackPoint;
}
str += "</trkseg></trk></gpx>";
if (trackRecording.Count <= 1)
{
//addLogMessage("Track data damaged!", 2);
}
return(new KeyValuePair <double, string>(distance, str));
}
public GhostPlane parseTrackFile(string file, MathClass _mathClass, double allowedRecordLength)
{
GhostPlane ghostPlane = new GhostPlane();
Console.WriteLine("Parsing " + file);
XElement trackpointsXml = XElement.Load(file);
if (trackpointsXml != null)
{
ghostPlane.TrackPoints = new List <TrackPoint>();
ghostPlane.ID = TARGETMAX;
if (trackpointsXml.Descendants("name").First() != null)
{
string[] title = trackpointsXml.Descendants("name").First().Value.Split(new string[] { " - " }, StringSplitOptions.None);
ghostPlane.Name = title[0].Trim();
ghostPlane.Username = title.Length > 1 ? title[1] : "";
}
else
{
ghostPlane.Name = "DA40-NG Asobo";
ghostPlane.Username = "";
}
double counter = 0;
DateTime lastTimer = new DateTime();
double latitudeOffset = 0;
double longitudeOffset = 0;
double headingOffset = 0;
double altitudeOffset = 0;
GeoLocation root = new GeoLocation(0, 0);
foreach (var trackPoint in trackpointsXml.Descendants("trkseg").First().Elements("trkpt"))
{
try
{
// FREE VERSION
if (counter > allowedRecordLength)
{
break;
}
double.TryParse(trackPoint.Attribute("lat").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out double lat);
double.TryParse(trackPoint.Attribute("lon").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out double lon);
lat *= Math.PI / 180;
lon *= Math.PI / 180;
if (counter == 0)
{
root = new GeoLocation(lat, lon);
}
double ele = 1000;
double.TryParse(trackPoint.Element("ele").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out ele);
DateTime time = DateTime.Parse(trackPoint.Element("time").Value);
int agl = 10;
if (trackPoint.Element("agl") != null)
{
int.TryParse(trackPoint.Element("agl").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out agl);
}
double velocity = 0;
if (trackPoint.Element("velocity") != null)
{
double.TryParse(trackPoint.Element("velocity").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out velocity);
}
double airspeed = 0;
if (trackPoint.Element("airspeed") != null)
{
double.TryParse(trackPoint.Element("airspeed").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out velocity);
}
short heading = 0;
if (trackPoint.Element("heading") != null)
{
short.TryParse(trackPoint.Element("heading").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out heading);
}
short pitch = 0;
if (trackPoint.Element("pitch") != null)
{
short.TryParse(trackPoint.Element("pitch").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out pitch);
}
short roll = 0;
if (trackPoint.Element("roll") != null)
{
short.TryParse(trackPoint.Element("roll").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out roll);
}
int lights = 0;
if (trackPoint.Element("lights") != null)
{
int.TryParse(trackPoint.Element("lights").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out lights);
}
int avionics = 0;
if (trackPoint.Element("avionics") != null)
{
int.TryParse(trackPoint.Element("avionics").Value, NumberStyles.Any, CultureInfo.InvariantCulture, out avionics);
}
if (counter != 0)
{
counter += (time - lastTimer).TotalSeconds;
}
GeoLocation loc = new GeoLocation(lat, lon);
// START OFFSETS
if (ghostTeleport.alt != 0 && altitudeOffset == 0)
{
//Console.WriteLine(JsonConvert.SerializeObject(ghostTeleport, Formatting.Indented));
latitudeOffset = ghostTeleport.location.Latitude - loc.Latitude;
longitudeOffset = ghostTeleport.location.Longitude - loc.Longitude;
headingOffset = (ghostTeleport.radius * 180 / Math.PI) - heading;
altitudeOffset = ghostTeleport.alt - ele;
Console.WriteLine("Offsets: lat" + latitudeOffset + " lon" + longitudeOffset + " head" + headingOffset + " alt" + altitudeOffset);
}
// CONTINUE OFFSETS
if (ghostTeleport.alt != 0 && ghostPlane.TrackPoints.Count > 0 && _mathClass.findDistanceBetweenPoints(ghostTeleport.location.Latitude, ghostTeleport.location.Longitude, root.Latitude, root.Longitude) > 100)
{
loc = _mathClass.RotatePointFrom(root, headingOffset * Math.PI / 180, loc);
}
loc.Latitude += latitudeOffset;
loc.Longitude += longitudeOffset;
heading += (short)headingOffset;
ele += altitudeOffset;
ghostPlane.TrackPoints.Add(new TrackPoint(loc, ele, agl, velocity, airspeed,
(short)(heading > 180 ? heading - 360 : heading), (short)(pitch > 180 ? pitch - 360 : pitch), (short)(roll > 180 ? roll - 360 : roll),
lights, avionics, time, counter, trackPoint.Element("message") != null ? trackPoint.Element("message").Value : ""));
if (counter == 0)
{
counter += 0.0001;
}
lastTimer = time;
}
catch { }
}
ghostPlane.Length = counter;
if (ghostPlane.TrackPoints.Count > 0)
{
Console.WriteLine(ghostPlane.TrackPoints.Count + " track points loaded");
ghostPlanes.Add(ghostPlane);
}
}
else
{
MessageBox.Show("GPX file is invalid");
}
//trackPlaying = null;
return(ghostPlane);
}