using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;
namespace CoordinateSharp
{
///
/// Earth Centered - Earth Fixed (X,Y,Z) Coordinate
///
[Serializable]
public class ECEF : INotifyPropertyChanged
{
///
/// Create an ECEF Object
///
/// Coordinate
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];
}
///
/// Create an ECEF Object
///
/// Coordinate
/// Coordinate
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];
}
///
/// Create an ECEF Object
///
/// X
/// Y
/// Z
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;
}
///
/// Updates ECEF Values
///
/// Coordinate
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;
///
/// Datum Equatorial Radius / Semi Major Axis
///
public double Equatorial_Radius
{
get { return equatorial_radius; }
}
///
/// Datum Flattening
///
public double Inverse_Flattening
{
get { return inverse_flattening; }
}
///
/// X Coordinate
///
public double X
{
get { return x; }
set
{
if (x != value)
{
x = value;
NotifyPropertyChanged("X");
}
}
}
///
/// y Coordinate
///
public double Y
{
get { return y; }
set
{
if (y != value)
{
y = value;
NotifyPropertyChanged("Y");
}
}
}
///
/// Z Coordinate
///
public double Z
{
get { return z; }
set
{
if (z != value)
{
z = value;
NotifyPropertyChanged("Z");
}
}
}
///
/// GeoDetic Height from Mean Sea Level.
/// Used for converting Lat Long / ECEF.
/// Default value is 0. Adjust as needed.
///
public Distance GeoDetic_Height
{
get { return geodetic_height; }
internal set
{
if (geodetic_height != value)
{
geodetic_height = value;
NotifyPropertyChanged("Height");
}
}
}
///
/// Sets GeoDetic height for ECEF conversion.
/// Recalculate ECEF Coordinate
///
/// Coordinate
/// Height
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];
}
///
/// Returns a Geodetic Coordinate object based on the provided ECEF Coordinate
///
/// X
/// Y
/// Z
/// Coordinate
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;
}
///
/// Returns a Geodetic Coordinate object based on the provided ECEF Coordinate
///
/// ECEF Coordinate
/// Coordinate
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;
}
///
/// ECEF Default String Format
///
/// ECEF Formatted Coordinate String
/// Values rounded to the 3rd place
public override string ToString()
{
return Math.Round(x, 3).ToString() + " km, " + Math.Round(y, 3).ToString() + " km, " + Math.Round(z, 3).ToString() + " km";
}
///
/// Property changed event
///
public event PropertyChangedEventHandler PropertyChanged;
///
/// Notify property changed
///
/// Property name
public void NotifyPropertyChanged(string propName)
{
if (PropertyChanged != null)
{
PropertyChanged(this, new PropertyChangedEventArgs(propName));
}
}
//CONVERSION LOGIC
///
/// Initialize EARTH global variables based on the Datum
///
private void WGS84()
{
double wgs84a = equatorial_radius / 1000;
double wgs84f = 1.0 / inverse_flattening;
double wgs84b = wgs84a * (1.0 - wgs84f);
EarthCon(wgs84a, wgs84b);
}
///
/// Sets Earth Constants as Globals
///
/// a
/// b
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;
}
///
/// Compute the radii at the geodetic latitude (degrees)
///
/// Latitude in degres
/// double[]
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);
}
///
/// Physical radius of the Earth
///
/// Latidude in degrees
/// double
private double rearth(double lat)
{
double[] rrnrm;
rrnrm = radcur(lat);
double r = rrnrm[0];
return r;
}
///
/// Converts geocentric latitude to geodetic latitude
///
/// Geocentric latitude
/// Altitude in KM
/// double
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;
}
///
/// Converts geodetic latitude to geocentric latitude
///
/// Geodetic latitude tp geocentric latitide
/// Altitude in KM
/// double
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;
}
///
/// Converts lat / long to east, north, up vectors
///
/// Latitude
/// Longitude
/// Array[] of double[]
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;
}
///
/// Gets ECEF vector in KM
///
/// Latitude
/// Longitude
/// Altitude in KM
/// double[]
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;
}
///
/// Converts ECEF X, Y, Z to GeoDetic Lat / Long and Height in KM
///
///
///
///
///
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;
}
}
}