/// <summary>
/// Geoid Conversion from Waypoint origin geoid to WGS84 geoid.
/// </summary>
/// <returns>WgsPoint converted waypoint</returns>
public override WgsPoint toWgs()
{
double lat = this.latitude * Math.PI / 180.0;
double lon = this.longitude * Math.PI / 180.0;
double n = Math.Pow(hayMajorAxis, 2) /
Math.Sqrt(
Math.Pow(hayMajorAxis, 2) * Math.Pow(Math.Cos(lat), 2) +
(Math.Pow(hayMinorAxis, 2) * Math.Pow(Math.Sin(lat), 2)));
double z;
if (this.altitude == null)
{
z = 0;
}
else
{
z = (double)this.altitude;
}
double x = (n + z) * Math.Cos(lat) * Math.Cos(lon);
double y = (n + z) * Math.Cos(lat) * Math.Sin(lon);
z = ((Math.Pow((hayMinorAxis / hayMajorAxis), 2)) * n + z)
* Math.Sin(lat);
double x_, y_, z_;
x_ = dX + (1 + e) * (x + rZ * y - rY * z);
y_ = dY + (1 + e) * (-rZ * x + y + rX * z);
z_ = dZ + (1 + e) * (rY * x - rX * y + z);
double p = Math.Sqrt(Math.Pow(x_, 2) + Math.Pow(y_, 2));
double theta = Math.Atan((z_ * wgsMajorAxis)
/ (p * wgsMinorAxis));
double pow_eccentricity = (Math.Pow(wgsMajorAxis, 2)
- Math.Pow(wgsMinorAxis, 2))
/ Math.Pow(wgsMajorAxis, 2);
double pow_second_eccentricity = (Math.Pow(wgsMajorAxis, 2)
- Math.Pow(wgsMinorAxis, 2))
/ Math.Pow(wgsMinorAxis, 2);
double latf = Math.Atan((z_ + pow_second_eccentricity
* wgsMinorAxis * Math.Pow(Math.Sin(theta), 3))
/ (p - pow_eccentricity * wgsMajorAxis
* Math.Pow(Math.Cos(theta), 3)));
double lonf = Math.Atan(y_ / x_);
double nf = Math.Pow(wgsMajorAxis, 2) /
Math.Sqrt(
Math.Pow(wgsMajorAxis, 2) * Math.Pow(Math.Cos(latf), 2) +
Math.Pow(wgsMinorAxis, 2) * Math.Pow(Math.Sin(latf), 2));
double hf = (p / Math.Cos(latf)) - nf;
latf = latf * 180 / Math.PI;
lonf = lonf * 180 / Math.PI;
WgsPoint point = new WgsPoint(latf, lonf, hf);
return(point);
}
public override Point fromWgs(WgsPoint point)
{
double lat = point.getLatitude() * Math.PI / 180.0;
double lon = point.getLongitude() * Math.PI / 180.0;
double n = Math.Pow(wgsMajorAxis, 2) /
Math.Sqrt(
Math.Pow(wgsMajorAxis, 2) * Math.Pow(Math.Cos(lat), 2) +
(Math.Pow(wgsMinorAxis, 2) * Math.Pow(Math.Sin(lat), 2)));
double z;
if (point.getAltitude() == null)
{
z = 0;
}
else
{
z = (double)point.getAltitude();
}
double x = (n + z) * Math.Cos(lat) * Math.Cos(lon);
double y = (n + z) * Math.Cos(lat) * Math.Sin(lon);
z = ((Math.Pow((wgsMinorAxis / wgsMajorAxis), 2)) * n + z)
* Math.Sin(point.getLatitude() * Math.PI / 180.0);
double x_, y_, z_;
x_ = -dX + (1 + (-1) * e) * (x - rZ * y + rY * z);
y_ = -dY + (1 + (-1) * e) * (rZ * x + y - rX * z);
z_ = -dZ + (1 + (-1) * e) * (-rY * x + rX * y + z);
double p = Math.Sqrt(Math.Pow(x_, 2) + Math.Pow(y_, 2));
double theta = Math.Atan((z_ * hayMajorAxis)
/ (p * hayMinorAxis));
double pow_eccentricity = (Math.Pow(hayMajorAxis, 2)
- Math.Pow(hayMinorAxis, 2))
/ Math.Pow(hayMajorAxis, 2);
double pow_second_eccentricity = (Math.Pow(hayMajorAxis, 2)
- Math.Pow(hayMinorAxis, 2))
/ Math.Pow(hayMinorAxis, 2);
double latf = Math.Atan((z_ + pow_second_eccentricity
* hayMinorAxis * Math.Pow(Math.Sin(theta), 3))
/ (p - pow_eccentricity * hayMajorAxis
* Math.Pow(Math.Cos(theta), 3)));
double lonf = Math.Atan(y_ / x_);
double nf = Math.Pow(hayMajorAxis, 2) /
Math.Sqrt(
Math.Pow(hayMajorAxis, 2) * Math.Pow(Math.Cos(latf), 2) +
Math.Pow(hayMinorAxis, 2) * Math.Pow(Math.Sin(latf), 2));
double hf = (p / Math.Cos(latf)) - nf;
latf = latf * 180 / Math.PI;
lonf = lonf * 180 / Math.PI;
HayPoint _point = new HayPoint(latf, lonf, hf);
return(_point);
}
public HayPoint(WgsPoint p)
: base()
{
HayPoint hayPoint = new HayPoint();
hayPoint = (HayPoint)hayPoint.fromWgs(p);
this.latitude = hayPoint.latitude;
this.longitude = hayPoint.longitude;
this.altitude = hayPoint.altitude;
this.timeZone = hayPoint.timeZone;
this.utmX = hayPoint.utmX;
this.utmY = hayPoint.utmY;
}
public HayPoint(WgsPoint p)
: base()
{
HayPoint hayPoint = new HayPoint();
hayPoint = (HayPoint)hayPoint.fromWgs(p);
this.latitude = hayPoint.latitude;
this.longitude = hayPoint.longitude;
this.altitude = hayPoint.altitude;
this.timeZone = hayPoint.timeZone;
this.utmX = hayPoint.utmX;
this.utmY = hayPoint.utmY;
}
/// <summary>
/// Creates the message.
/// </summary>
/// <param name='b'>
/// Message as a byte array
/// </param>
public override void CreateMessage(ulong time, byte[] b)
{
Array.Reverse(b, 1, 4);
this.latitude.V = BitConverter.ToInt32(b, 1) / 1000000.0;
Array.Reverse(b, 5, 4);
this.longitude.V = BitConverter.ToInt32(b, 5) / 100000.0;
Array.Reverse(b, 9, 2);
this.gndSpeed.V = BitConverter.ToInt16(b, 9) * 1852 / 360000.0;
Array.Reverse(b, 11, 2);
this.trackAngle.V = BitConverter.ToInt16(b, 11) / 100.0;
WgsPoint wgs = new WgsPoint(this.latitude.V, this.longitude.V, null);
}
/// <summary>
/// Creates the message.
/// </summary>
/// <param name='m'>
/// Message as a string
/// </param>
public override void CreateMessage(string m)
{
try
{
string[] words = m.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
this.latitude.V = Convert.ToDouble(words[3].Substring(0, 2)) + Convert.ToDouble(words[3].Substring(2)) / 60;
this.longitude.V = Convert.ToDouble(words[5].Substring(0, 3)) + Convert.ToDouble(words[5].Substring(3)) / 60;
this.gndSpeed.V = Convert.ToDouble(words[7]);
this.trackAngle.V = Convert.ToDouble(words[8]);
this.pos = new WgsPoint(this.latitude.V, this.longitude.V, null);
}
catch (Exception e)
{
Console.WriteLine("Error GPS: " + e.Message);
}
}
/// <summary>
/// Creates the message.
/// </summary>
/// <param name='m'>
/// Message as a string
/// </param>
public override void CreateMessage(string m)
{
try
{
string[] words = m.Split(new char[]{','}, StringSplitOptions.RemoveEmptyEntries);
this.latitude.V = Convert.ToDouble(words[3].Substring(0, 2)) + Convert.ToDouble(words[3].Substring(2)) / 60;
this.longitude.V = Convert.ToDouble(words[5].Substring(0, 3)) + Convert.ToDouble(words[5].Substring(3)) / 60;
this.gndSpeed.V = Convert.ToDouble(words[7]);
this.trackAngle.V = Convert.ToDouble(words[8]);
this.pos = new WgsPoint(this.latitude.V, this.longitude.V, null);
}
catch (Exception e)
{
Console.WriteLine("Error GPS: " + e.Message);
}
}
/// <summary>
/// Creates the message.
/// </summary>
/// <param name='b'>
/// Message as a byte array
/// </param>
public override void CreateMessage(ulong time, byte[] b)
{
Array.Reverse(b, 1, 4);
this.latitude.V = BitConverter.ToInt32(b, 1)/1000000.0;
Array.Reverse(b, 5, 4);
this.longitude.V = BitConverter.ToInt32(b, 5)/100000.0;
Array.Reverse(b, 9, 2);
this.gndSpeed.V = BitConverter.ToInt16(b, 9) * 1852 / 360000.0 ;
Array.Reverse(b, 11, 2);
this.trackAngle.V = BitConverter.ToInt16(b, 11) / 100.0;
WgsPoint wgs = new WgsPoint(this.latitude.V, this.longitude.V, null);
}
/// <summary>
/// Do nothing.
/// </summary>
/// <returns>The same waypoint.</returns>
public override Point fromWgs(WgsPoint p)
{
return p;
}
/// <summary>
/// Do nothing.
/// </summary>
/// <returns>The same waypoint.</returns>
public override Point fromWgs(WgsPoint p)
{
return(p);
}
public override Point fromWgs(WgsPoint point)
{
double lat = point.getLatitude() * Math.PI / 180.0;
double lon = point.getLongitude() * Math.PI / 180.0;
double n = Math.Pow(wgsMajorAxis, 2) /
Math.Sqrt(
Math.Pow(wgsMajorAxis, 2) * Math.Pow(Math.Cos(lat), 2) +
(Math.Pow(wgsMinorAxis, 2) * Math.Pow(Math.Sin(lat), 2)));
double z;
if (point.getAltitude() == null)
z = 0;
else
z = (double)point.getAltitude();
double x = (n + z) * Math.Cos(lat) * Math.Cos(lon);
double y = (n + z) * Math.Cos(lat) * Math.Sin(lon);
z = ((Math.Pow((wgsMinorAxis / wgsMajorAxis), 2)) * n + z)
* Math.Sin(point.getLatitude() * Math.PI / 180.0);
double x_, y_, z_;
x_ = -dX + (1 + (-1) * e) * (x - rZ * y + rY * z);
y_ = -dY + (1 + (-1) * e) * (rZ * x + y - rX * z);
z_ = -dZ + (1 + (-1) * e) * (-rY * x + rX * y + z);
double p = Math.Sqrt(Math.Pow(x_, 2) + Math.Pow(y_, 2));
double theta = Math.Atan((z_ * hayMajorAxis)
/ (p * hayMinorAxis));
double pow_eccentricity = (Math.Pow(hayMajorAxis, 2)
- Math.Pow(hayMinorAxis, 2))
/ Math.Pow(hayMajorAxis, 2);
double pow_second_eccentricity = (Math.Pow(hayMajorAxis, 2)
- Math.Pow(hayMinorAxis, 2))
/ Math.Pow(hayMinorAxis, 2);
double latf = Math.Atan((z_ + pow_second_eccentricity
* hayMinorAxis * Math.Pow(Math.Sin(theta), 3))
/ (p - pow_eccentricity * hayMajorAxis
* Math.Pow(Math.Cos(theta), 3)));
double lonf = Math.Atan(y_ / x_);
double nf = Math.Pow(hayMajorAxis, 2) /
Math.Sqrt(
Math.Pow(hayMajorAxis, 2) * Math.Pow(Math.Cos(latf), 2) +
Math.Pow(hayMinorAxis, 2) * Math.Pow(Math.Sin(latf), 2));
double hf = (p / Math.Cos(latf)) - nf;
latf = latf * 180 / Math.PI;
lonf = lonf * 180 / Math.PI;
HayPoint _point = new HayPoint(latf, lonf, hf);
return _point;
}
/// <summary>
/// Geoid Conversion from Waypoint origin geoid to WGS84 geoid.
/// </summary>
/// <returns>WgsPoint converted waypoint</returns>
public override WgsPoint toWgs()
{
double lat = this.latitude * Math.PI / 180.0;
double lon = this.longitude * Math.PI / 180.0;
double n = Math.Pow(hayMajorAxis, 2) /
Math.Sqrt(
Math.Pow(hayMajorAxis, 2) * Math.Pow(Math.Cos(lat), 2) +
(Math.Pow(hayMinorAxis, 2) * Math.Pow(Math.Sin(lat), 2)));
double z;
if (this.altitude == null)
z = 0;
else
z = (double)this.altitude;
double x = (n + z) * Math.Cos(lat) * Math.Cos(lon);
double y = (n + z) * Math.Cos(lat) * Math.Sin(lon);
z = ((Math.Pow((hayMinorAxis / hayMajorAxis), 2)) * n + z)
* Math.Sin(lat);
double x_, y_, z_;
x_ = dX + (1 + e) * (x + rZ * y - rY * z);
y_ = dY + (1 + e) * (-rZ * x + y + rX * z);
z_ = dZ + (1 + e) * (rY * x - rX * y + z);
double p = Math.Sqrt(Math.Pow(x_, 2) + Math.Pow(y_, 2));
double theta = Math.Atan((z_ * wgsMajorAxis)
/ (p * wgsMinorAxis));
double pow_eccentricity = (Math.Pow(wgsMajorAxis, 2)
- Math.Pow(wgsMinorAxis, 2))
/ Math.Pow(wgsMajorAxis, 2);
double pow_second_eccentricity = (Math.Pow(wgsMajorAxis, 2)
- Math.Pow(wgsMinorAxis, 2))
/ Math.Pow(wgsMinorAxis, 2);
double latf = Math.Atan((z_ + pow_second_eccentricity
* wgsMinorAxis * Math.Pow(Math.Sin(theta), 3))
/ (p - pow_eccentricity * wgsMajorAxis
* Math.Pow(Math.Cos(theta), 3)));
double lonf = Math.Atan(y_ / x_);
double nf = Math.Pow(wgsMajorAxis, 2) /
Math.Sqrt(
Math.Pow(wgsMajorAxis, 2) * Math.Pow(Math.Cos(latf), 2) +
Math.Pow(wgsMinorAxis, 2) * Math.Pow(Math.Sin(latf), 2));
double hf = (p / Math.Cos(latf)) - nf;
latf = latf * 180 / Math.PI;
lonf = lonf * 180 / Math.PI;
WgsPoint point = new WgsPoint(latf, lonf, hf);
return point;
}
/// <summary> /// Geoid Conversion from WGS84 origin geoid to current waypoint geoid. /// </summary> /// <param name="p">The object waypoint.</param> /// <returns>Converted waypoint.</returns> public abstract Point fromWgs(WgsPoint p);
/// <summary> /// Geoid Conversion from WGS84 origin geoid to current waypoint geoid. /// </summary> /// <param name="p">The object waypoint.</param> /// <returns>Converted waypoint.</returns> public abstract Point fromWgs(WgsPoint p);
