Coordinates/CoordinateSharp/Coordinate.ECEF.cs

567 lines
17 KiB
C#
Raw Normal View History

2019-06-28 11:14:53 +02:00
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;
namespace CoordinateSharp
{
/// <summary>
/// Earth Centered - Earth Fixed (X,Y,Z) Coordinate
/// </summary>
[Serializable]
public class ECEF : INotifyPropertyChanged
{
/// <summary>
/// Create an ECEF Object
/// </summary>
/// <param name="c">Coordinate</param>
public ECEF(Coordinate c)
{
equatorial_radius = 6378137.0;
inverse_flattening = 298.257223563;
WGS84();
geodetic_height = new Distance(0);
double[] ecef = LatLong_To_ECEF(c.Latitude.DecimalDegree, c.Longitude.DecimalDegree, geodetic_height.Kilometers);
x = ecef[0];
y = ecef[1];
z = ecef[2];
}
/// <summary>
/// Create an ECEF Object
/// </summary>
/// <param name="c">Coordinate</param>
/// <param name="height">Coordinate</param>
public ECEF(Coordinate c, Distance height)
{
equatorial_radius = 6378137.0;
inverse_flattening = 298.257223563;
WGS84();
geodetic_height = height;
double[] ecef = LatLong_To_ECEF(c.Latitude.DecimalDegree, c.Longitude.DecimalDegree, geodetic_height.Kilometers);
x = ecef[0];
y = ecef[1];
z = ecef[2];
}
/// <summary>
/// Create an ECEF Object
/// </summary>
/// <param name="xc">X</param>
/// <param name="yc">Y</param>
/// <param name="zc">Z</param>
public ECEF(double xc, double yc, double zc)
{
equatorial_radius = 6378137.0;
inverse_flattening = 298.257223563;
WGS84();
geodetic_height = new Distance(0);
x = xc;
y = yc;
z = zc;
}
/// <summary>
/// Updates ECEF Values
/// </summary>
/// <param name="c">Coordinate</param>
public void ToECEF(Coordinate c)
{
equatorial_radius = 6378137.0;
inverse_flattening = 298.257223563;
WGS84();
double[] ecef = LatLong_To_ECEF(c.Latitude.DecimalDegree, c.Longitude.DecimalDegree, geodetic_height.Kilometers);
x = ecef[0];
y = ecef[1];
z = ecef[2];
}
//Globals for calucations
private double EARTH_A;
private double EARTH_B;
private double EARTH_F;
private double EARTH_Ecc;
private double EARTH_Esq;
//ECEF Values
private double x;
private double y;
private double z;
private Distance geodetic_height;
//Datum
internal double equatorial_radius;
internal double inverse_flattening;
/// <summary>
/// Datum Equatorial Radius / Semi Major Axis
/// </summary>
public double Equatorial_Radius
{
get { return equatorial_radius; }
}
/// <summary>
/// Datum Flattening
/// </summary>
public double Inverse_Flattening
{
get { return inverse_flattening; }
}
/// <summary>
/// X Coordinate
/// </summary>
public double X
{
get { return x; }
set
{
if (x != value)
{
x = value;
NotifyPropertyChanged("X");
}
}
}
/// <summary>
/// y Coordinate
/// </summary>
public double Y
{
get { return y; }
set
{
if (y != value)
{
y = value;
NotifyPropertyChanged("Y");
}
}
}
/// <summary>
/// Z Coordinate
/// </summary>
public double Z
{
get { return z; }
set
{
if (z != value)
{
z = value;
NotifyPropertyChanged("Z");
}
}
}
/// <summary>
/// GeoDetic Height from Mean Sea Level.
/// Used for converting Lat Long / ECEF.
/// Default value is 0. Adjust as needed.
/// </summary>
public Distance GeoDetic_Height
{
get { return geodetic_height; }
internal set
{
if (geodetic_height != value)
{
geodetic_height = value;
NotifyPropertyChanged("Height");
}
}
}
/// <summary>
/// Sets GeoDetic height for ECEF conversion.
/// Recalculate ECEF Coordinate
/// </summary>
/// <param name="c">Coordinate</param>
/// <param name="dist">Height</param>
public void Set_GeoDetic_Height(Coordinate c, Distance dist)
{
geodetic_height = dist;
double[] values = LatLong_To_ECEF(c.Latitude.DecimalDegree, c.Longitude.DecimalDegree, dist.Kilometers);
x = values[0];
y = values[1];
z = values[2];
}
/// <summary>
/// Returns a Geodetic Coordinate object based on the provided ECEF Coordinate
/// </summary>
/// <param name="x">X</param>
/// <param name="y">Y</param>
/// <param name="z">Z</param>
/// <returns>Coordinate</returns>
public static Coordinate ECEFToLatLong(double x, double y, double z)
{
ECEF ecef = new ECEF(x, y, z);
double[] values = ecef.ECEF_To_LatLong(x, y, z);
ecef.geodetic_height =new Distance(values[2]);
Coordinate c = new Coordinate(values[0], values[1]);
c.ECEF = ecef;
return c;
}
/// <summary>
/// Returns a Geodetic Coordinate object based on the provided ECEF Coordinate
/// </summary>
/// <param name="ecef">ECEF Coordinate</param>
/// <returns>Coordinate</returns>
public static Coordinate ECEFToLatLong(ECEF ecef)
{
double[] values = ecef.ECEF_To_LatLong(ecef.X, ecef.Y, ecef.Z);
Coordinate c = new Coordinate(values[0], values[1]);
Distance height = new Distance(values[2]);
ecef.geodetic_height = new Distance(values[2]);
c.ECEF = ecef;
return c;
}
/// <summary>
/// ECEF Default String Format
/// </summary>
/// <returns>ECEF Formatted Coordinate String</returns>
/// <returns>Values rounded to the 3rd place</returns>
public override string ToString()
{
return Math.Round(x, 3).ToString() + " km, " + Math.Round(y, 3).ToString() + " km, " + Math.Round(z, 3).ToString() + " km";
}
/// <summary>
/// Property changed event
/// </summary>
public event PropertyChangedEventHandler PropertyChanged;
/// <summary>
/// Notify property changed
/// </summary>
/// <param name="propName">Property name</param>
public void NotifyPropertyChanged(string propName)
{
if (PropertyChanged != null)
{
PropertyChanged(this, new PropertyChangedEventArgs(propName));
}
}
//CONVERSION LOGIC
/// <summary>
/// Initialize EARTH global variables based on the Datum
/// </summary>
private void WGS84()
{
double wgs84a = equatorial_radius / 1000;
double wgs84f = 1.0 / inverse_flattening;
double wgs84b = wgs84a * (1.0 - wgs84f);
EarthCon(wgs84a, wgs84b);
}
/// <summary>
/// Sets Earth Constants as Globals
/// </summary>
/// <param name="a">a</param>
/// <param name="b">b</param>
private void EarthCon(double a, double b)
{
double f = 1 - b / a;
double eccsq = 1 - b * b / (a * a);
double ecc = Math.Sqrt(eccsq);
EARTH_A = a;
EARTH_B = b;
EARTH_F = f;
EARTH_Ecc = ecc;
EARTH_Esq = eccsq;
}
/// <summary>
/// Compute the radii at the geodetic latitude (degrees)
/// </summary>
/// <param name="lat">Latitude in degres</param>
/// <returns>double[]</returns>
private double[] radcur(double lat)
{
double[] rrnrm = new double[3];
double dtr = Math.PI / 180.0;
double a = EARTH_A;
double b = EARTH_B;
double asq = a * a;
double bsq = b * b;
double eccsq = 1 - bsq / asq;
double ecc = Math.Sqrt(eccsq);
double clat = Math.Cos(dtr * lat);
double slat = Math.Sin(dtr * lat);
double dsq = 1.0 - eccsq * slat * slat;
double d = Math.Sqrt(dsq);
double rn = a / d;
double rm = rn * (1.0 - eccsq) / dsq;
double rho = rn * clat;
double z = (1.0 - eccsq) * rn * slat;
double rsq = rho * rho + z * z;
double r = Math.Sqrt(rsq);
rrnrm[0] = r;
rrnrm[1] = rn;
rrnrm[2] = rm;
return (rrnrm);
}
/// <summary>
/// Physical radius of the Earth
/// </summary>
/// <param name="lat">Latidude in degrees</param>
/// <returns>double</returns>
private double rearth(double lat)
{
double[] rrnrm;
rrnrm = radcur(lat);
double r = rrnrm[0];
return r;
}
/// <summary>
/// Converts geocentric latitude to geodetic latitude
/// </summary>
/// <param name="flatgc">Geocentric latitude</param>
/// <param name="altkm">Altitude in KM</param>
/// <returns>double</returns>
private double gc2gd(double flatgc, double altkm)
{
var dtr = Math.PI / 180.0;
var rtd = 1 / dtr;
double ecc = EARTH_Ecc;
double esq = ecc * ecc;
//approximation by stages
//1st use gc-lat as if is gd, then correct alt dependence
double altnow = altkm;
double[] rrnrm = radcur(flatgc);
double rn = rrnrm[1];
double ratio = 1 - esq * rn / (rn + altnow);
double tlat = Math.Tan(dtr * flatgc) / ratio;
double flatgd = rtd * Math.Atan(tlat);
//now use this approximation for gd-lat to get rn etc.
rrnrm = radcur(flatgd);
rn = rrnrm[1];
ratio = 1 - esq * rn / (rn + altnow);
tlat = Math.Tan(dtr * flatgc) / ratio;
flatgd = rtd * Math.Atan(tlat);
return flatgd;
}
/// <summary>
/// Converts geodetic latitude to geocentric latitude
/// </summary>
/// <param name="flatgd">Geodetic latitude tp geocentric latitide</param>
/// <param name="altkm">Altitude in KM</param>
/// <returns>double</returns>
private double gd2gc(double flatgd, double altkm)
{
double dtr = Math.PI / 180.0;
double rtd = 1 / dtr;
double ecc = EARTH_Ecc;
double esq = ecc * ecc;
double altnow = altkm;
double[] rrnrm = radcur(flatgd);
double rn = rrnrm[1];
double ratio = 1 - esq * rn / (rn + altnow);
double tlat = Math.Tan(dtr * flatgd) * ratio;
double flatgc = rtd * Math.Atan(tlat);
return flatgc;
}
/// <summary>
/// Converts lat / long to east, north, up vectors
/// </summary>
/// <param name="flat">Latitude</param>
/// <param name="flon">Longitude</param>
/// <returns>Array[] of double[]</returns>
private Array[] llenu(double flat, double flon)
{
double clat, slat, clon, slon;
double[] ee = new double[3];
double[] en = new double[3];
double[] eu = new double[3];
Array[] enu = new Array[3];
double dtr = Math.PI / 180.0;
clat = Math.Cos(dtr * flat);
slat = Math.Sin(dtr * flat);
clon = Math.Cos(dtr * flon);
slon = Math.Sin(dtr * flon);
ee[0] = -slon;
ee[1] = clon;
ee[2] = 0.0;
en[0] = -clon * slat;
en[1] = -slon * slat;
en[2] = clat;
eu[0] = clon * clat;
eu[1] = slon * clat;
eu[2] = slat;
enu[0] = ee;
enu[1] = en;
enu[2] = eu;
return enu;
}
/// <summary>
/// Gets ECEF vector in KM
/// </summary>
/// <param name="lat">Latitude</param>
/// <param name="longi">Longitude</param>
/// <param name="altkm">Altitude in KM</param>
/// <returns>double[]</returns>
private double[] LatLong_To_ECEF(double lat, double longi, double altkm)
{
double dtr = Math.PI / 180.0;
double clat = Math.Cos(dtr * lat);
double slat = Math.Sin(dtr * lat);
double clon = Math.Cos(dtr * longi);
double slon = Math.Sin(dtr * longi);
double[] rrnrm = radcur(lat);
double rn = rrnrm[1];
double re = rrnrm[0];
double ecc = EARTH_Ecc;
double esq = ecc * ecc;
double x = (rn + altkm) * clat * clon;
double y = (rn + altkm) * clat * slon;
double z = ((1 - esq) * rn + altkm) * slat;
double[] xvec = new double[3];
xvec[0] = x;
xvec[1] = y;
xvec[2] = z;
return xvec;
}
/// <summary>
/// Converts ECEF X, Y, Z to GeoDetic Lat / Long and Height in KM
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <returns></returns>
private double[] ECEF_To_LatLong(double x, double y, double z)
{
var dtr = Math.PI / 180.0;
double[] rrnrm = new double[3];
double[] llhvec = new double[3];
double slat, tangd, flatn, dlat, clat;
double flat;
double altkm;
double esq = EARTH_Esq;
double rp = Math.Sqrt(x * x + y * y + z * z);
double flatgc = Math.Asin(z / rp) / dtr;
double flon;
double testval = Math.Abs(x) + Math.Abs(y);
if (testval < 1.0e-10)
{ flon = 0.0; }
else
{ flon = Math.Atan2(y, x) / dtr; }
if (flon < 0.0) { flon = flon + 360.0; }
double p = Math.Sqrt(x * x + y * y);
//Pole special case
if (p < 1.0e-10)
{
flat = 90.0;
if (z < 0.0) { flat = -90.0; }
altkm = rp - rearth(flat);
llhvec[0] = flat;
llhvec[1] = flon;
llhvec[2] = altkm;
return llhvec;
}
//first iteration, use flatgc to get altitude
//and alt needed to convert gc to gd lat.
double rnow = rearth(flatgc);
altkm = rp - rnow;
flat = gc2gd(flatgc, altkm);
rrnrm = radcur(flat);
double rn = rrnrm[1];
for (int kount = 0; kount < 5; kount++)
{
slat = Math.Sin(dtr * flat);
tangd = (z + rn * esq * slat) / p;
flatn = Math.Atan(tangd) / dtr;
dlat = flatn - flat;
flat = flatn;
clat = Math.Cos(dtr * flat);
rrnrm = radcur(flat);
rn = rrnrm[1];
altkm = (p / clat) - rn;
if (Math.Abs(dlat) < 1.0e-12) { break; }
}
//CONVERTER WORKS IN E LAT ONLY, IF E LAT > 180 LAT IS WEST SO IT MUCST BE CONVERTED TO Decimal
if (flon > 180) { flon = flon - 360; }
llhvec[0] = flat;
llhvec[1] = flon;
llhvec[2] = altkm;
return llhvec;
}
}
}