private static double GetDistanceBetweenWaypoints(IofXml30Waypoint w1, IofXml30Waypoint w2)
{
// use spherical coordinates: rho, phi, theta
const double rho = 6378200; // earth radius in metres
double sinPhi0 = Math.Sin(0.5 * Math.PI + w1.Latitude / 180.0 * Math.PI);
double cosPhi0 = Math.Cos(0.5 * Math.PI + w1.Latitude / 180.0 * Math.PI);
double sinTheta0 = Math.Sin(w1.Longitude / 180.0 * Math.PI);
double cosTheta0 = Math.Cos(w1.Longitude / 180.0 * Math.PI);
double sinPhi1 = Math.Sin(0.5 * Math.PI + w2.Latitude / 180.0 * Math.PI);
double cosPhi1 = Math.Cos(0.5 * Math.PI + w2.Latitude / 180.0 * Math.PI);
double sinTheta1 = Math.Sin(w2.Longitude / 180.0 * Math.PI);
double cosTheta1 = Math.Cos(w2.Longitude / 180.0 * Math.PI);
var x1 = rho * sinPhi0 * cosTheta0;
var y1 = rho * sinPhi0 * sinTheta0;
var z1 = rho * cosPhi0;
var x2 = rho * sinPhi1 * cosTheta1;
var y2 = rho * sinPhi1 * sinTheta1;
var z2 = rho * cosPhi1;
return(DistancePointToPoint(x1, y1, z1, x2, y2, z2));
}
/// <summary>
/// Writes the route in IOF XML 3.0 binary format to the specified stream.
/// </summary>
/// <param name="stream">The stream to write to.</param>
public void WriteToStream(Stream stream)
{
IofXml30Waypoint previousWaypoint = null;
foreach (var waypoint in Waypoints)
{
waypoint.WriteToStream(stream, previousWaypoint);
previousWaypoint = waypoint;
}
}
/// <summary>
/// Reads a route in IOF XML 3.0 binary format from a stream.
/// </summary>
/// <param name="stream">The stream to read from.</param>
public static IofXml30Route FromStream(Stream stream)
{
var waypoints = new List <IofXml30Waypoint>();
while (stream.Position < stream.Length)
{
waypoints.Add(IofXml30Waypoint.FromStream(stream, waypoints.LastOrDefault()));
}
return(new IofXml30Route()
{
Waypoints = waypoints
});
}
/// <summary>
/// Writes the waypoint in IOF XML 3.0 binary format to a stream.
/// </summary>
/// <param name="stream">The stream to write to.</param>
/// <param name="previousWaypoint">The previous waypoint of the route, or null if this is the first waypoint.</param>
public void WriteToStream(Stream stream, IofXml30Waypoint previousWaypoint)
{
var timeStorageMode = TimeStorageMode.Full;
if (previousWaypoint != null)
{
if ((StorageTime - previousWaypoint.StorageTime) % 1000 == 0 && (StorageTime - previousWaypoint.StorageTime) / 1000 <= timeSecondsThreshold)
{
timeStorageMode = TimeStorageMode.Seconds;
}
else if (StorageTime - previousWaypoint.StorageTime <= timeMillisecondsThreshold)
{
timeStorageMode = TimeStorageMode.Milliseconds;
}
}
var positionStorageMode = PositionStorageMode.Full;
if (previousWaypoint != null &&
(StorageAltitude == null || (previousWaypoint.StorageAltitude != null && StorageAltitude - previousWaypoint.StorageAltitude >= altitudeDeltaLowerThreshold && StorageAltitude - previousWaypoint.StorageAltitude <= altitudeDeltaUpperThreshold)))
{
if (StorageLatitude - previousWaypoint.StorageLatitude >= lanLngSmallDeltaLowerThreshold && StorageLatitude - previousWaypoint.StorageLatitude <= lanLngSmallDeltaUpperThreshold &&
StorageLongitude - previousWaypoint.StorageLongitude >= lanLngSmallDeltaLowerThreshold && StorageLongitude - previousWaypoint.StorageLongitude <= lanLngSmallDeltaUpperThreshold)
{
positionStorageMode = PositionStorageMode.SmallDelta;
}
else if (StorageLatitude - previousWaypoint.StorageLatitude >= lanLngBigDeltaLowerThreshold && StorageLatitude - previousWaypoint.StorageLatitude <= lanLngBigDeltaUpperThreshold &&
StorageLongitude - previousWaypoint.StorageLongitude >= lanLngBigDeltaLowerThreshold && StorageLongitude - previousWaypoint.StorageLongitude <= lanLngBigDeltaUpperThreshold)
{
positionStorageMode = PositionStorageMode.BigDelta;
}
}
var headerByte = 0;
if (Type == IofXml30WaypointType.Interruption) headerByte |= (1 << 7);
if (timeStorageMode == TimeStorageMode.Milliseconds) headerByte |= (1 << 6);
if (timeStorageMode == TimeStorageMode.Seconds) headerByte |= (1 << 5);
if (positionStorageMode == PositionStorageMode.BigDelta) headerByte |= (1 << 4);
if (positionStorageMode == PositionStorageMode.SmallDelta) headerByte |= (1 << 3);
if (StorageAltitude != null) headerByte |= (1 << 2);
// header byte
stream.WriteByte((byte)headerByte);
// time byte(s)
switch (timeStorageMode)
{
case TimeStorageMode.Full: // 6 bytes
stream.Write(BitConverter.GetBytes(StorageTime).Reverse().ToArray(), 2, 6);
break;
case TimeStorageMode.Milliseconds: // 2 bytes
stream.Write(BitConverter.GetBytes((ushort)(StorageTime - previousWaypoint.StorageTime)).Reverse().ToArray(), 0, 2);
break;
case TimeStorageMode.Seconds: // 1 byte
stream.WriteByte((byte)((StorageTime - previousWaypoint.StorageTime) / 1000));
break;
}
// position bytes
switch (positionStorageMode)
{
case PositionStorageMode.Full: // 4 + 4 + 3 bytes
stream.Write(BitConverter.GetBytes(StorageLatitude).Reverse().ToArray(), 0, 4);
stream.Write(BitConverter.GetBytes(StorageLongitude).Reverse().ToArray(), 0, 4);
if (StorageAltitude != null) stream.Write(BitConverter.GetBytes(StorageAltitude.Value).Reverse().ToArray(), 1, 3);
break;
case PositionStorageMode.BigDelta: // 2 + 2 + 1 bytes
stream.Write(BitConverter.GetBytes((short)(StorageLatitude - previousWaypoint.StorageLatitude)).Reverse().ToArray(), 0, 2);
stream.Write(BitConverter.GetBytes((short)(StorageLongitude - previousWaypoint.StorageLongitude)).Reverse().ToArray(), 0, 2);
if (StorageAltitude != null) stream.Write(BitConverter.GetBytes((sbyte)(StorageAltitude - previousWaypoint.StorageAltitude).Value), 0, 1);
break;
case PositionStorageMode.SmallDelta: // 1 + 1 + 1 bytes
stream.Write(BitConverter.GetBytes((sbyte)(StorageLatitude - previousWaypoint.StorageLatitude)), 0, 1);
stream.Write(BitConverter.GetBytes((sbyte)(StorageLongitude - previousWaypoint.StorageLongitude)), 0, 1);
if (StorageAltitude != null) stream.Write(BitConverter.GetBytes((sbyte)(StorageAltitude - previousWaypoint.StorageAltitude).Value), 0, 1);
break;
}
}
/// <summary>
/// Reads a waypoint in IOF XML 3.0 binary format from a stream.
/// </summary>
/// <param name="stream">The stream to read from.</param>
/// <param name="previousWaypoint">The previous waypoint of the route, or null if this is the first waypoint.</param>
/// <returns></returns>
public static IofXml30Waypoint FromStream(Stream stream, IofXml30Waypoint previousWaypoint)
{
var waypoint = new IofXml30Waypoint();
// header byte
var headerByte = stream.ReadByte();
waypoint.Type = (headerByte & (1 << 7)) == 0 ? IofXml30WaypointType.Normal : IofXml30WaypointType.Interruption;
var timeStorageMode = TimeStorageMode.Full;
if ((headerByte & (1 << 6)) > 0)
{
timeStorageMode = TimeStorageMode.Milliseconds;
}
else if ((headerByte & (1 << 5)) > 0)
{
timeStorageMode = TimeStorageMode.Seconds;
}
var positionStorageMode = PositionStorageMode.Full;
if ((headerByte & (1 << 4)) > 0)
{
positionStorageMode = PositionStorageMode.BigDelta;
}
else if ((headerByte & (1 << 3)) > 0)
{
positionStorageMode = PositionStorageMode.SmallDelta;
}
var altitudePresent = (headerByte & (1 << 2)) > 0;
byte[] bytes;
int b;
// time byte(s)
switch (timeStorageMode)
{
case TimeStorageMode.Full: // 4 bytes
bytes = new byte[8];
stream.Read(bytes, 2, 6);
waypoint.StorageTime = BitConverter.ToUInt64(bytes.Reverse().ToArray(), 0);
break;
case TimeStorageMode.Milliseconds: // 2 bytes
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageTime = previousWaypoint.StorageTime + BitConverter.ToUInt16(bytes.Reverse().ToArray(), 0);
break;
case TimeStorageMode.Seconds: // 1 byte
b = stream.ReadByte();
waypoint.StorageTime = previousWaypoint.StorageTime + (ulong)b * 1000;
break;
}
// position bytes
switch (positionStorageMode)
{
case PositionStorageMode.Full: // 4 + 4 + 3 bytes
bytes = new byte[4];
stream.Read(bytes, 0, 4);
waypoint.StorageLatitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
bytes = new byte[4];
stream.Read(bytes, 0, 4);
waypoint.StorageLongitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
if (altitudePresent)
{
bytes = new byte[4];
stream.Read(bytes, 1, 3);
waypoint.StorageAltitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
}
break;
case PositionStorageMode.BigDelta: // 2 + 2 + 1 bytes
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageLatitude = previousWaypoint.StorageLatitude + BitConverter.ToInt16(bytes.Reverse().ToArray(), 0);
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageLongitude = previousWaypoint.StorageLongitude + BitConverter.ToInt16(bytes.Reverse().ToArray(), 0);
if (altitudePresent)
{
b = stream.ReadByte();
waypoint.StorageAltitude = previousWaypoint.StorageAltitude + (sbyte)b;
}
break;
case PositionStorageMode.SmallDelta: // 1 + 1 + 1 bytes
b = stream.ReadByte();
waypoint.StorageLatitude = previousWaypoint.StorageLatitude + (sbyte)b;
b = stream.ReadByte();
waypoint.StorageLongitude = previousWaypoint.StorageLongitude + (sbyte)b;
if (altitudePresent)
{
b = stream.ReadByte();
waypoint.StorageAltitude = previousWaypoint.StorageAltitude + (sbyte)b;
}
break;
}
return waypoint;
}
private static double GetDistanceBetweenWaypoints(IofXml30Waypoint w1, IofXml30Waypoint w2)
{
// use spherical coordinates: rho, phi, theta
const double rho = 6378200; // earth radius in metres
double sinPhi0 = Math.Sin(0.5 * Math.PI + w1.Latitude / 180.0 * Math.PI);
double cosPhi0 = Math.Cos(0.5 * Math.PI + w1.Latitude / 180.0 * Math.PI);
double sinTheta0 = Math.Sin(w1.Longitude / 180.0 * Math.PI);
double cosTheta0 = Math.Cos(w1.Longitude / 180.0 * Math.PI);
double sinPhi1 = Math.Sin(0.5 * Math.PI + w2.Latitude / 180.0 * Math.PI);
double cosPhi1 = Math.Cos(0.5 * Math.PI + w2.Latitude / 180.0 * Math.PI);
double sinTheta1 = Math.Sin(w2.Longitude / 180.0 * Math.PI);
double cosTheta1 = Math.Cos(w2.Longitude / 180.0 * Math.PI);
var x1 = rho * sinPhi0 * cosTheta0;
var y1 = rho * sinPhi0 * sinTheta0;
var z1 = rho * cosPhi0;
var x2 = rho * sinPhi1 * cosTheta1;
var y2 = rho * sinPhi1 * sinTheta1;
var z2 = rho * cosPhi1;
return DistancePointToPoint(x1, y1, z1, x2, y2, z2);
}
/// <summary>
/// Reads a waypoint in IOF XML 3.0 binary format from a stream.
/// </summary>
/// <param name="stream">The stream to read from.</param>
/// <param name="previousWaypoint">The previous waypoint of the route, or null if this is the first waypoint.</param>
/// <returns></returns>
public static IofXml30Waypoint FromStream(Stream stream, IofXml30Waypoint previousWaypoint)
{
var waypoint = new IofXml30Waypoint();
// header byte
var headerByte = stream.ReadByte();
waypoint.Type = (headerByte & (1 << 7)) == 0 ? IofXml30WaypointType.Normal : IofXml30WaypointType.Interruption;
var timeStorageMode = TimeStorageMode.Full;
if ((headerByte & (1 << 6)) > 0)
{
timeStorageMode = TimeStorageMode.Milliseconds;
}
else if ((headerByte & (1 << 5)) > 0)
{
timeStorageMode = TimeStorageMode.Seconds;
}
var positionStorageMode = PositionStorageMode.Full;
if ((headerByte & (1 << 4)) > 0)
{
positionStorageMode = PositionStorageMode.BigDelta;
}
else if ((headerByte & (1 << 3)) > 0)
{
positionStorageMode = PositionStorageMode.SmallDelta;
}
var altitudePresent = (headerByte & (1 << 2)) > 0;
byte[] bytes;
int b;
// time byte(s)
switch (timeStorageMode)
{
case TimeStorageMode.Full: // 4 bytes
bytes = new byte[8];
stream.Read(bytes, 2, 6);
waypoint.StorageTime = BitConverter.ToUInt64(bytes.Reverse().ToArray(), 0);
break;
case TimeStorageMode.Milliseconds: // 2 bytes
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageTime = previousWaypoint.StorageTime + BitConverter.ToUInt16(bytes.Reverse().ToArray(), 0);
break;
case TimeStorageMode.Seconds: // 1 byte
b = stream.ReadByte();
waypoint.StorageTime = previousWaypoint.StorageTime + (ulong)b * 1000;
break;
}
// position bytes
switch (positionStorageMode)
{
case PositionStorageMode.Full: // 4 + 4 + 3 bytes
bytes = new byte[4];
stream.Read(bytes, 0, 4);
waypoint.StorageLatitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
bytes = new byte[4];
stream.Read(bytes, 0, 4);
waypoint.StorageLongitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
if (altitudePresent)
{
bytes = new byte[4];
stream.Read(bytes, 1, 3);
waypoint.StorageAltitude = BitConverter.ToInt32(bytes.Reverse().ToArray(), 0);
}
break;
case PositionStorageMode.BigDelta: // 2 + 2 + 1 bytes
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageLatitude = previousWaypoint.StorageLatitude + BitConverter.ToInt16(bytes.Reverse().ToArray(), 0);
bytes = new byte[2];
stream.Read(bytes, 0, 2);
waypoint.StorageLongitude = previousWaypoint.StorageLongitude + BitConverter.ToInt16(bytes.Reverse().ToArray(), 0);
if (altitudePresent)
{
b = stream.ReadByte();
waypoint.StorageAltitude = previousWaypoint.StorageAltitude + (sbyte)b;
}
break;
case PositionStorageMode.SmallDelta: // 1 + 1 + 1 bytes
b = stream.ReadByte();
waypoint.StorageLatitude = previousWaypoint.StorageLatitude + (sbyte)b;
b = stream.ReadByte();
waypoint.StorageLongitude = previousWaypoint.StorageLongitude + (sbyte)b;
if (altitudePresent)
{
b = stream.ReadByte();
waypoint.StorageAltitude = previousWaypoint.StorageAltitude + (sbyte)b;
}
break;
}
return(waypoint);
}
/// <summary>
/// Writes the waypoint in IOF XML 3.0 binary format to a stream.
/// </summary>
/// <param name="stream">The stream to write to.</param>
/// <param name="previousWaypoint">The previous waypoint of the route, or null if this is the first waypoint.</param>
public void WriteToStream(Stream stream, IofXml30Waypoint previousWaypoint)
{
var timeStorageMode = TimeStorageMode.Full;
if (previousWaypoint != null)
{
if ((StorageTime - previousWaypoint.StorageTime) % 1000 == 0 && (StorageTime - previousWaypoint.StorageTime) / 1000 <= timeSecondsThreshold)
{
timeStorageMode = TimeStorageMode.Seconds;
}
else if (StorageTime - previousWaypoint.StorageTime <= timeMillisecondsThreshold)
{
timeStorageMode = TimeStorageMode.Milliseconds;
}
}
var positionStorageMode = PositionStorageMode.Full;
if (previousWaypoint != null &&
(StorageAltitude == null || (previousWaypoint.StorageAltitude != null && StorageAltitude - previousWaypoint.StorageAltitude >= altitudeDeltaLowerThreshold && StorageAltitude - previousWaypoint.StorageAltitude <= altitudeDeltaUpperThreshold)))
{
if (StorageLatitude - previousWaypoint.StorageLatitude >= lanLngSmallDeltaLowerThreshold && StorageLatitude - previousWaypoint.StorageLatitude <= lanLngSmallDeltaUpperThreshold &&
StorageLongitude - previousWaypoint.StorageLongitude >= lanLngSmallDeltaLowerThreshold && StorageLongitude - previousWaypoint.StorageLongitude <= lanLngSmallDeltaUpperThreshold)
{
positionStorageMode = PositionStorageMode.SmallDelta;
}
else if (StorageLatitude - previousWaypoint.StorageLatitude >= lanLngBigDeltaLowerThreshold && StorageLatitude - previousWaypoint.StorageLatitude <= lanLngBigDeltaUpperThreshold &&
StorageLongitude - previousWaypoint.StorageLongitude >= lanLngBigDeltaLowerThreshold && StorageLongitude - previousWaypoint.StorageLongitude <= lanLngBigDeltaUpperThreshold)
{
positionStorageMode = PositionStorageMode.BigDelta;
}
}
var headerByte = 0;
if (Type == IofXml30WaypointType.Interruption)
{
headerByte |= (1 << 7);
}
if (timeStorageMode == TimeStorageMode.Milliseconds)
{
headerByte |= (1 << 6);
}
if (timeStorageMode == TimeStorageMode.Seconds)
{
headerByte |= (1 << 5);
}
if (positionStorageMode == PositionStorageMode.BigDelta)
{
headerByte |= (1 << 4);
}
if (positionStorageMode == PositionStorageMode.SmallDelta)
{
headerByte |= (1 << 3);
}
if (StorageAltitude != null)
{
headerByte |= (1 << 2);
}
// header byte
stream.WriteByte((byte)headerByte);
// time byte(s)
switch (timeStorageMode)
{
case TimeStorageMode.Full: // 6 bytes
stream.Write(BitConverter.GetBytes(StorageTime).Reverse().ToArray(), 2, 6);
break;
case TimeStorageMode.Milliseconds: // 2 bytes
stream.Write(BitConverter.GetBytes((ushort)(StorageTime - previousWaypoint.StorageTime)).Reverse().ToArray(), 0, 2);
break;
case TimeStorageMode.Seconds: // 1 byte
stream.WriteByte((byte)((StorageTime - previousWaypoint.StorageTime) / 1000));
break;
}
// position bytes
switch (positionStorageMode)
{
case PositionStorageMode.Full: // 4 + 4 + 3 bytes
stream.Write(BitConverter.GetBytes(StorageLatitude).Reverse().ToArray(), 0, 4);
stream.Write(BitConverter.GetBytes(StorageLongitude).Reverse().ToArray(), 0, 4);
if (StorageAltitude != null)
{
stream.Write(BitConverter.GetBytes(StorageAltitude.Value).Reverse().ToArray(), 1, 3);
}
break;
case PositionStorageMode.BigDelta: // 2 + 2 + 1 bytes
stream.Write(BitConverter.GetBytes((short)(StorageLatitude - previousWaypoint.StorageLatitude)).Reverse().ToArray(), 0, 2);
stream.Write(BitConverter.GetBytes((short)(StorageLongitude - previousWaypoint.StorageLongitude)).Reverse().ToArray(), 0, 2);
if (StorageAltitude != null)
{
stream.Write(BitConverter.GetBytes((sbyte)(StorageAltitude - previousWaypoint.StorageAltitude).Value), 0, 1);
}
break;
case PositionStorageMode.SmallDelta: // 1 + 1 + 1 bytes
stream.Write(BitConverter.GetBytes((sbyte)(StorageLatitude - previousWaypoint.StorageLatitude)), 0, 1);
stream.Write(BitConverter.GetBytes((sbyte)(StorageLongitude - previousWaypoint.StorageLongitude)), 0, 1);
if (StorageAltitude != null)
{
stream.Write(BitConverter.GetBytes((sbyte)(StorageAltitude - previousWaypoint.StorageAltitude).Value), 0, 1);
}
break;
}
}
