public void InitializationWithRadians()
{
var target = Longitude.FromRadians(Math.PI / 4);
Assert.AreEqual(45.0, target.Degrees);
Assert.AreEqual(Math.PI / 4, target.Radians);
}
/// <summary>
/// transform the geodetic datum
/// </summary>
/// <param name="point">geodetic point</param>
/// <param name="ellipsoid">source ellipsoid</param>
/// <param name="da">semi-major difference of ellipsoids</param>
/// <param name="df">flattening difference of ellipsoids</param>
/// <param name="dX">translation along the X-axis</param>
/// <param name="dY">translation along the Y-axis</param>
/// <param name="dZ">translation along the Z-axis</param>
/// <returns>geodetic point in new datum</returns>
public static GeodeticCoord Transform(GeodeticCoord point, Ellipsoid ellipsoid, double da, double df, double dX, double dY, double dZ)
{
double B = point.Latitude.Radians;
double L = point.Longitude.Radians;
double H = point.Height;
double sinB = Math.Sin(B);
double cosB = Math.Cos(B);
double sinL = Math.Sin(L);
double cosL = Math.Cos(L);
double a = ellipsoid.SemiMajorAxis;
double b_a = 1 - ellipsoid.Flattening;
double ee = ellipsoid.ee;
double w = 1.0 - ee * sinB * sinB;
double Rm = a * (1.0 - ee) / Math.Pow(w, 1.5);
double Rn = a / Math.Sqrt(w);
// formula is from http://earth-info.nga.mil/GandG/coordsys/datums/standardmolodensky.html
double dB = (-dX * sinB * cosL - dY * sinB * sinL + dZ * cosB
+ da * (Rn * ee * sinB * cosB) / a
+ df * (Rm / b_a + Rn * b_a) * sinB * cosB) / (Rm + H);
double dL = (-dX * sinL + dY * cosL) / ((Rn + H) * cosB);
double dH = dX * cosB * cosL + dY * cosB * sinL + dZ * sinB
- da * a / Rn + df * Rn * b_a * sinB * sinB;
return(new GeodeticCoord(Latitude.FromRadians(B + dB), Longitude.FromRadians(L + dL), H + dH));
}
/// <summary>
/// Conversion of space rectangular coordinate to geodetic coordinate.
/// </summary>
/// <param name="ellipsoid">ellipsoid</param>
/// <param name="X">X component of space rectangular coordinate</param>
/// <param name="Y">Y component of space rectangular coordinate</param>
/// <param name="Z">Z component of space rectangular coordinate</param>
/// <param name="lng">longitude</param>
/// <param name="lat">latitude</param>
/// <param name="hgt">ellipsoid height</param>
public static void XYZ_BLH(Ellipsoid ellipsoid, double X, double Y, double Z, out Latitude lat, out Longitude lng, out double hgt)
{
double a = ellipsoid.a;
double ee = ellipsoid.ee;
double rL, rB, rB0;
//求解经度
rL = Math.Atan2(Y, X);
// 迭代精度为1E-5秒
rB = Math.Atan(Z / Math.Sqrt(X * X + Y * Y));
rB0 = rB + 1;
while (Math.Abs(rB - rB0) > Settings.Epsilon5)
{
rB0 = rB;
double temp = a * ee * Math.Tan(rB0) / Math.Sqrt(1 + (1 - ee) * Math.Pow(Math.Tan(rB0), 2));
rB = Math.Atan((Z + temp) / Math.Sqrt(X * X + Y * Y));
}
lng = Longitude.FromRadians(rL);
lat = Latitude.FromRadians(rB);
hgt = Math.Sqrt(X * X + Y * Y) / Math.Cos(rB) - a / Math.Sqrt(1 - ee * Math.Pow(Math.Sin(rB), 2));
}
/// <summary>
/// transform the geodetic datum
/// </summary>
/// <param name="point">geodetic point</param>
/// <param name="from">source ellipsoid</param>
/// <param name="to">target ellipsoid</param>
/// <param name="para">three translation parameters</param>
/// <returns>geodetic point in new datum</returns>
public static GeodeticCoord Transform(GeodeticCoord point, Ellipsoid from, Ellipsoid to, TransParameters para)
{
double B = point.Latitude.Radians;
double L = point.Longitude.Radians;
double H = point.Height;
double sinB = Math.Sin(B);
double cosB = Math.Cos(B);
double sinL = Math.Sin(L);
double cosL = Math.Cos(L);
double a = from.SemiMajorAxis;
double b_a = 1 - from.Flattening;
double da = to.SemiMajorAxis - from.SemiMajorAxis;
double df = to.Flattening - from.Flattening;
double ee = from.ee;
double w = 1.0 - ee * sinB * sinB;
double Rm = a * (1.0 - ee) / Math.Pow(w, 1.5);
double Rn = a / Math.Sqrt(w);
// formula is from http://earth-info.nga.mil/GandG/coordsys/datums/standardmolodensky.html
double dB = (-para.Tx * sinB * cosL - para.Ty * sinB * sinL + para.Tz * cosB
+ da * (Rn * ee * sinB * cosB) / a
+ df * (Rm / b_a + Rn * b_a) * sinB * cosB) / (Rm + H);
double dL = (-para.Tx * sinL + para.Ty * cosL) / ((Rn + H) * cosB);
double dH = para.Tx * cosB * cosL + para.Ty * cosB * sinL + para.Tz * sinB
- da * a / Rn + df * Rn * b_a * sinB * sinB;
return(new GeodeticCoord(Latitude.FromRadians(B + dB), Longitude.FromRadians(L + dL), H + dH));
}
/// <summary>
/// converts geodetic coordinates to Lambert conformal conic projection coordinates.
/// </summary>
/// <param name="lng">longitude</param>
/// <param name="lat">latitude</param>
/// <param name="easting">easting</param>
/// <param name="northing">northing</param>
public override void Forward(Latitude lat, Longitude lng, out double northing, out double easting)
{
double rho;
double rB = lat.Radians;
double rL = lng.Radians;
double temp = Math.Abs(Math.Abs(rB) - Math.PI / 2);
if (temp > 1E-10)
{
double t = Get_t(rB);
rho = SemiMajor * _F * Math.Pow(t, _n);
}
else
{
temp = rB * _n;
if (temp <= 0)
{
throw new GeodeticException("");
}
rho = 0;
}
Longitude L = Longitude.FromRadians(rL - CenteralMaridian.Radians);
L.Normalize();
double gamma = _n * L.Radians;
easting = rho * Math.Sin(gamma) + FalseEasting;
northing = _rho - rho * Math.Cos(gamma) + FalseNorthing;
}
/// <summary>
/// Directed solution of geodetic problem from start point, distance and azimuth to end point and inverse azimuth.
/// </summary>
/// <param name="start">start point of geodesic</param>
/// <param name="distance">distance of geodesic</param>
/// <param name="bearing">azimuth of geodesic</param>
/// <param name="end">end point of geodesic</param>
/// <param name="ivBearing">inverse azimuth of geodesic</param>
protected override void Direct(GeoPoint start, double distance, Angle bearing, out GeoPoint end, out Angle ivBearing)
{
double lat1 = start.Latitude.Radians;
double brng = bearing.Radians;
double dR = distance / R;
var lat2 = Math.Asin(Math.Sin(lat1) * Math.Cos(dR) + Math.Cos(lat1) * Math.Sin(dR) * Math.Cos(brng));
var lng2 = start.Longitude.Radians + Math.Atan2(Math.Sin(brng) * Math.Sin(dR) * Math.Cos(lat1),
Math.Cos(dR) - Math.Sin(lat1) * Math.Sin(lat2));
end = new GeoPoint(Latitude.FromRadians(lat2), Longitude.FromRadians(lng2));
ivBearing = GetBearing(end, start);
}
/// <summary>
/// converts Lambert conformal conic projection coordinates to geodetic coordinates.
/// </summary>
/// <param name="easting">easting</param>
/// <param name="northing">northing</param>
/// <param name="lng">longitude</param>
/// <param name="lat">latitude</param>
public override void Reverse(double northing, double easting, out Latitude lat, out Longitude lng)
{
double rho;
double temp;
double dX = easting - FalseEasting;
double dY = _rho - northing + FalseNorthing;
if (_n > 0)
{
rho = Math.Sqrt(dX * dX + dY * dY);
temp = 1.0;
}
else
{
rho = -Math.Sqrt(dX * dX + dY * dY);
temp = -1.0;
}
double gamma = 0.0;
if (rho != 0)
{
gamma = Math.Atan2((temp * dX), (temp * dY));
}
if ((rho != 0) || (_n > 0.0))
{
double t = Math.Pow((rho / (SemiMajor * _F)), 0.1 * _n);
lat = Latitude.FromRadians(phi2z(t, out long flag));
if (flag != 0)
{
throw new ArgumentException();
}
}
else
{
lat = new Latitude(-90.0);
}
lng = Longitude.FromRadians(gamma / _n + CenteralMaridian.Radians);
lng.Normalize();
}
/// <summary>
/// Converts the current instance to a geodetic (latitude/longitude) coordinate using the specified ellipsoid.
/// </summary>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <returns>A <strong>Position</strong> object containing the converted result.</returns>
/// <remarks>The conversion formula will convert the Cartesian coordinate to
/// latitude and longitude using the WGS1984 ellipsoid (the default ellipsoid for
/// GPS coordinates). The resulting three-dimensional coordinate is accurate to within two millimeters
/// (2 mm).</remarks>
public Position3D ToPosition3D(Ellipsoid ellipsoid)
{
if (ellipsoid == null)
{
throw new ArgumentNullException("ellipsoid");
}
#region New code
/*
* % ECEF2LLA - convert earth-centered earth-fixed (ECEF)
* % cartesian coordinates to latitude, longitude,
* % and altitude
* %
* % USAGE:
* % [lat, lon, alt] = ecef2lla(x, y, z)
* %
* % lat = geodetic latitude (radians)
* % lon = longitude (radians)
* % alt = height above WGS84 ellipsoid (m)
* % x = ECEF X-coordinate (m)
* % y = ECEF Y-coordinate (m)
* % z = ECEF Z-coordinate (m)
* %
* % Notes: (1) This function assumes the WGS84 model.
* % (2) Latitude is customary geodetic (not geocentric).
* % (3) Inputs may be scalars, vectors, or matrices of the same
* % size and shape. Outputs will have that same size and shape.
* % (4) Tested but no warranty; use at your own risk.
* % (5) Michael Kleder, April 2006
*
* function [lat, lon, alt] = ecef2lla(x, y, z)
*
* % WGS84 ellipsoid constants:
* a = 6378137;
* e = 8.1819190842622e-2;
*
* % calculations:
* b = sqrt(a^2*(1-e^2));
* ep = sqrt((a^2-b^2)/b^2);
* p = sqrt(x.^2+y.^2);
* th = atan2(a*z, b*p);
* lon = atan2(y, x);
* lat = atan2((z+ep^2.*b.*sin(th).^3), (p-e^2.*a.*cos(th).^3));
* N = a./sqrt(1-e^2.*sin(lat).^2);
* alt = p./cos(lat)-N;
*
* % return lon in range [0, 2*pi)
* lon = mod(lon, 2*pi);
*
* % correct for numerical instability in altitude near exact poles:
* % (after this correction, error is about 2 millimeters, which is about
* % the same as the numerical precision of the overall function)
*
* k=abs(x)<1 & abs(y)<1;
* alt(k) = abs(z(k))-b;
*
* return
*/
double x = _x.ToMeters().Value;
double y = _y.ToMeters().Value;
double z = _z.ToMeters().Value;
//% WGS84 ellipsoid constants:
//a = 6378137;
double a = ellipsoid.EquatorialRadius.ToMeters().Value;
//e = 8.1819190842622e-2;
double e = ellipsoid.Eccentricity;
//% calculations:
//b = sqrt(a^2*(1-e^2));
double b = Math.Sqrt(Math.Pow(a, 2) * (1 - Math.Pow(e, 2)));
//ep = sqrt((a^2-b^2)/b^2);
double ep = Math.Sqrt((Math.Pow(a, 2) - Math.Pow(b, 2)) / Math.Pow(b, 2));
//p = sqrt(x.^2+y.^2);
double p = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
//th = atan2(a*z, b*p);
double th = Math.Atan2(a * z, b * p);
//lon = atan2(y, x);
double lon = Math.Atan2(y, x);
//lat = atan2((z+ep^2.*b.*sin(th).^3), (p-e^2.*a.*cos(th).^3));
double lat = Math.Atan2((z + Math.Pow(ep, 2) * b * Math.Pow(Math.Sin(th), 3)), (p - Math.Pow(e, 2) * a * Math.Pow(Math.Cos(th), 3)));
//N = a./sqrt(1-e^2.*sin(lat).^2);
double n = a / Math.Sqrt(1 - Math.Pow(e, 2) * Math.Pow(Math.Sin(lat), 2));
//alt = p./cos(lat)-N;
double alt = p / Math.Cos(lat) - n;
//% return lon in range [0, 2*pi)
//lon = mod(lon, 2*pi);
lon = lon % (2 * Math.PI);
//% correct for numerical instability in altitude near exact poles:
//% (after this correction, error is about 2 millimeters, which is about
//% the same as the numerical precision of the overall function)
//k=abs(x)<1 & abs(y)<1;
bool k = Math.Abs(x) < 1.0 && Math.Abs(y) < 1.0;
//alt(k) = abs(z(k))-b;
if (k)
{
alt = Math.Abs(z) - b;
}
//return
return(new Position3D(
Distance.FromMeters(alt),
Latitude.FromRadians(lat),
Longitude.FromRadians(lon)));
#endregion New code
}
/// <summary>
/// converts Lambert equal-area conic projection coordinates to geodetic coordinates.
/// </summary>
/// <param name="northing">northing</param>
/// <param name="easting">easting</param>
/// <param name="lat">latitude</param>
/// <param name="lng">longitude</param>
public override void Reverse(double northing, double easting, out Latitude lat, out Longitude lng)
{
//STEP 1
double east = (easting - FalseEasting) / ScaleFactor;
double north = (northing - FalseNorthing) / ScaleFactor;
double rLng = Math.PI + Math.Atan(east / north) * (_hemisphere == 'S' ? 1 : -1);
lng = (double.IsNaN(rLng) ? new Longitude(0) : Longitude.FromRadians(rLng));
// SET TO 0 or it will equate to 180
if (north >= 0 && east == 0 && _hemisphere == 'S')
{
lng = new Longitude(0);
}
// SET TO 0 or it will equate to 180
if (north < 0 && east == 0 && _hemisphere == 'N')
{
lng = new Longitude(0);
}
lng += CenteralMaridian;
lng.Normalize();
//STEP 2
if (north == 0)
{
north = 1;
}
double temp = Math.PI + Math.Atan(east / north) * (_hemisphere == 'S' ? 1 : -1);
//STEP 3
double a = SemiMajor;
double es = SquaredEccentricity;
double e = Math.Sqrt(es);
double b = a * Math.Sqrt(1 - es);
double K = (2 * Math.Pow(a, 2) / b) * Math.Pow(((1 - e) / (1 + e)), (e / 2));
//STEP 4
double kCos = K * Math.Abs(Math.Cos(temp));
double q = Math.Log(Math.Abs(north) / kCos) / Math.Log(Math.E) * -1;
//STEP 5
double rLat = 2 * Math.Atan(Math.Pow(Math.E, q)) - Math.PI / 2;
double rB = Math.PI / 2;
while (Math.Abs(rLat - rB) > Settings.Epsilon5)
{
if (double.IsInfinity(rLat))
{
break;
}
rB = rLat;
//STEP 6
double sLat = Math.Sin(rLat);
double bracket = (1 + sLat) / (1 - sLat) * Math.Pow((1 - e * sLat) / (1 + e * sLat), e);
double fLat = -q + 1 / 2.0 * Math.Log(bracket);
double fLat2 = (1 - Math.Pow(e, 2)) / ((1 - Math.Pow(e, 2) * Math.Pow(sLat, 2)) * Math.Cos(rLat));
//STEP 7
rLat -= fLat / fLat2;
}
if (!double.IsInfinity(rLat))
{
rB = rLat;
}
//NaN signals poles
lat = double.IsNaN(rB) ? new Latitude(90) : Latitude.FromRadians(rB);
if (_hemisphere == 'S')
{
lat = -lat;
}
}
/// <summary>
/// Directed solution of geodetic problem from start point, distance and azimuth to end point and inverse azimuth.
/// </summary>
/// <param name="start">start point of geodesic</param>
/// <param name="distance">distance of geodesic</param>
/// <param name="bearing">azimuth of geodesic</param>
/// <param name="end">end point of geodesic</param>
/// <param name="ivBearing">inverse azimuth of geodesic</param>
protected override void Direct(GeoPoint start, double distance, Angle bearing, out GeoPoint end, out Angle ivBearing)
{
double a = start.Ellipsoid.a;
double es = start.Ellipsoid.ee;
double ses = start.Ellipsoid._ee;
double B1 = start.Latitude.Radians;
double L1 = start.Longitude.Radians;
double A1 = bearing.Radians;
double u1 = Math.Atan(Math.Sqrt(1 - es) * Math.Tan(B1)); //u1 is (-90, 90)
double m = Math.Cos(u1) * Math.Sin(A1); //sin(m)
m = Math.Atan(m / Math.Sqrt(1 - m * m)); //m is (-90, 90)
if (m < 0)
{
m += 2 * Math.PI;
}
double M = Math.Atan(Math.Tan(u1) / Math.Cos(A1)); //M is (-90, 90)
if (M < 0)
{
M += Math.PI; //change M to (0, 180)
}
//compute cofficients
double KK = ses * Math.Pow(Math.Cos(m), 2);
double alpha = Math.Sqrt(1 + ses) * (1 - KK / 4 + 7 * KK * KK / 64 - 15 * Math.Pow(KK, 3) / 256) / a;
double beta = KK / 4 - KK * KK / 8 + 37 * Math.Pow(KK, 3) / 512;
double gamma = KK * KK * (1 - KK) / 128;
//loop for compute sigma
double sigma, temp;
sigma = alpha * distance;
temp = 0;
while (Math.Abs(temp - sigma) > Settings.Epsilon5) //精度0.00001秒
{
temp = sigma;
sigma = alpha * distance + beta * Math.Sin(sigma) * Math.Cos(2 * M + sigma) + gamma * Math.Sin(2 * sigma) * Math.Cos(4 * M + 2 * sigma);
}
double A2 = Math.Atan(Math.Tan(m) / Math.Cos(M + sigma)); //A2 is (-90, 90)
if (A2 < 0)
{
A2 += Math.PI;
}
if (A1 < Math.PI)
{
A2 += Math.PI;
}
ivBearing = Angle.FromRadians(A2);
double u2 = Math.Atan(-Math.Cos(A2) * Math.Tan(M + sigma)); //u2 is (-90, 90)
Latitude B = Latitude.FromRadians(Math.Atan(Math.Sqrt(1 + ses) * Math.Tan(u2))); //B in (-90, 90)
double lamda1;
lamda1 = Math.Atan(Math.Sin(u1) * Math.Tan(A1)); //lamda1
if (lamda1 < 0)
{
lamda1 += Math.PI;
}
if (m >= Math.PI)
{
lamda1 += Math.PI;
}
double lamda2; //lamda2
lamda2 = Math.Atan(Math.Sin(u2) * Math.Tan(A2));
if (Math.Abs(lamda2) < Math.Exp(-15))
{
lamda2 = 0; //当A2为180度时，上式因舍入误差使得lamda2为负
}
if (lamda2 < 0)
{
lamda2 += Math.PI;
}
if (m >= Math.PI)
{
if (M + sigma < Math.PI)
{
lamda2 += Math.PI;
}
}
else
{
if (M + sigma > Math.PI)
{
lamda2 += Math.PI;
}
}
KK = es * Math.Pow(Math.Cos(m), 2);
alpha = es / 2 + es * es / 8 + Math.Pow(es, 3) / 16 - es * (1 + es) * KK / 16 + 3 * es * KK * KK / 128;
beta = es * (1 + es) * KK / 16 - es * KK * KK / 32;
gamma = es * KK * KK / 256;
double dL = lamda2 - lamda1 - Math.Sin(m) * (alpha * sigma + beta * Math.Sin(sigma) * Math.Cos(2 * M + sigma) + gamma * Math.Sin(2 * sigma) * Math.Cos(4 * M + 2 * sigma));
end = new GeoPoint(B, Longitude.FromRadians(L1 + dL));
}
