protected static XyzCoordinates LatLongToXYZ(LatLonCoordinates p1, Datum datum)
{
//[4] Appendix B
//IgnoreAltitude = false;
//datum = NewDatum("Ordnance GB 1936");
//p1.Latitude = 52.657570;
//p1.Longitude = 1.717922;
//p1.Altitude = 24.7;
var latrad = p1.Latitude.Radians;
var longrad = p1.Longitude.Radians;
#if (USEALTITUDEINHELMERT)
var altitude = p1.Altitude;
#else
var altitude = 0;
#endif
var nu = datum.a / Math.Sqrt(1 - datum.e2 * Math.Sin(latrad) * Math.Sin(latrad));
var p2 = new XyzCoordinates(
(nu + altitude) * Math.Cos(latrad) * Math.Cos(longrad),
(nu + altitude) * Math.Cos(latrad) * Math.Sin(longrad),
((1 - datum.e2) * nu + altitude) * Math.Sin(latrad)
);
return(p2);
}
protected static LatLonCoordinates XYZToLatLong(XyzCoordinates p1, Datum datum)
{
//[4] Appendix B
//datum = NewDatum("Ordnance GB 1936");
//p1.Easting = 3874938.849;
//p1.Northing = 116218.624;
//p1.Altitude = 5047168.208;
//nu computation helper
Func <double, double> nu = phi =>
{
return(datum.a / Math.Sqrt(1 - datum.e2 * Math.Sin(phi) * Math.Sin(phi)));
};
var p = Math.Sqrt(p1.X * p1.X + p1.Y * p1.Y);
var latrad = Math.Atan2(p1.Z, p * (1 - datum.e2));
double latrad0;
do
{
latrad0 = latrad;
latrad = Math.Atan2(p1.Z + datum.e2 * nu(latrad0) * Math.Sin(latrad0), p);
} while (Math.Abs(latrad - latrad0) > 10E-10);//TODO: find the best precision
Angle lat = new Angle();
Angle lon = new Angle();
var p2 = new LatLonCoordinates(
datum,
new Angle()
{
Radians = latrad
},
new Angle()
{
Radians = Math.Atan2(p1.Y, p1.X)
},
p / Math.Cos(latrad) - nu(latrad)
);
return(p2);
}
public Sun(LatLonCoordinates position)
{
latitude = position.Latitude;
longitude = position.Longitude;
}
/// <summary>
/// Geodetic curve between two points on a specified reference ellipsoid.
/// This is the solution to the inverse geodetic problem.
/// </summary>
/// <param name="start">starting coordinates</param>
/// <param name="end">ending coordinates </param>
/// <param name="referenceDatum">reference datum to use, default="WGS84"</param>
/// <returns></returns>
public GeodeticCurve(LatLonCoordinates start, LatLonCoordinates end, string referenceDatum = "WGS84")
{
//
// All equation numbers refer back to Vincenty's publication:
// See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
//
// get constants
var datum = Datum.GetInstance(referenceDatum);
double a = datum.a;
double b = datum.a * Math.Sqrt(1 - datum.e2);
double f = (a - b) / a;
// get parameters as radians
double phi1 = start.Latitude.Radians;
double lambda1 = start.Longitude.Radians;
double phi2 = end.Latitude.Radians;
double lambda2 = end.Longitude.Radians;
// calculations
double a2 = a * a;
double b2 = b * b;
double a2b2b2 = (a2 - b2) / b2;
double omega = lambda2 - lambda1;
double tanphi1 = Math.Tan(phi1);
double tanU1 = (1.0 - f) * tanphi1;
double U1 = Math.Atan(tanU1);
double sinU1 = Math.Sin(U1);
double cosU1 = Math.Cos(U1);
double tanphi2 = Math.Tan(phi2);
double tanU2 = (1.0 - f) * tanphi2;
double U2 = Math.Atan(tanU2);
double sinU2 = Math.Sin(U2);
double cosU2 = Math.Cos(U2);
double sinU1sinU2 = sinU1 * sinU2;
double cosU1sinU2 = cosU1 * sinU2;
double sinU1cosU2 = sinU1 * cosU2;
double cosU1cosU2 = cosU1 * cosU2;
// eq. 13
double lambda = omega;
// intermediates we'll need to compute 's'
double A = 0.0;
double B = 0.0;
double sigma = 0.0;
double deltasigma = 0.0;
double lambda0;
bool converged = false;
for (int i = 0; i < 20; i++)
{
lambda0 = lambda;
double sinlambda = Math.Sin(lambda);
double coslambda = Math.Cos(lambda);
// eq. 14
double sin2sigma = (cosU2 * sinlambda * cosU2 * sinlambda) + Math.Pow(cosU1sinU2 - sinU1cosU2 * coslambda, 2.0);
double sinsigma = Math.Sqrt(sin2sigma);
// eq. 15
double cossigma = sinU1sinU2 + (cosU1cosU2 * coslambda);
// eq. 16
sigma = Math.Atan2(sinsigma, cossigma);
// eq. 17 Careful! sin2sigma might be almost 0!
double sinalpha = (sin2sigma == 0) ? 0.0 : cosU1cosU2 * sinlambda / sinsigma;
double alpha = Math.Asin(sinalpha);
double cosalpha = Math.Cos(alpha);
double cos2alpha = cosalpha * cosalpha;
// eq. 18 Careful! cos2alpha might be almost 0!
double cos2sigmam = cos2alpha == 0.0 ? 0.0 : cossigma - 2 * sinU1sinU2 / cos2alpha;
double u2 = cos2alpha * a2b2b2;
double cos2sigmam2 = cos2sigmam * cos2sigmam;
// eq. 3
A = 1.0 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)));
// eq. 4
B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)));
// eq. 6
deltasigma = B * sinsigma * (cos2sigmam + B / 4 * (cossigma * (-1 + 2 * cos2sigmam2) - B / 6 * cos2sigmam * (-3 + 4 * sin2sigma) * (-3 + 4 * cos2sigmam2)));
// eq. 10
double C = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha));
// eq. 11 (modified)
lambda = omega + (1 - C) * f * sinalpha * (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1 + 2 * cos2sigmam2)));
// see how much improvement we got
double change = Math.Abs((lambda - lambda0) / lambda);
if ((i > 1) && (change < 0.0000000000001))
{
converged = true;
break;
}
}
// eq. 19
double s = b * A * (sigma - deltasigma);
Angle alpha1;
Angle alpha2;
// didn't converge? must be N/S
if (!converged)
{
if (phi1 > phi2)
{
alpha1 = Angle.Angle180;
alpha2 = Angle.Angle0;
}
else if (phi1 < phi2)
{
alpha1 = Angle.Angle0;
alpha2 = Angle.Angle180;
}
else
{
alpha1 = Angle.NaA;
alpha2 = Angle.NaA;
}
}
// else, it converged, so do the math
else
{
double radians;
// eq. 20
radians = Math.Atan2(cosU2 * Math.Sin(lambda), (cosU1sinU2 - sinU1cosU2 * Math.Cos(lambda)));
if (radians < 0.0)
{
radians += Angle.TWOPI;
}
alpha1 = new Angle()
{
Radians = radians
};
// eq. 21
radians = Math.Atan2(cosU1 * Math.Sin(lambda), (-sinU1cosU2 + cosU1sinU2 * Math.Cos(lambda))) + Math.PI;
if (radians < 0.0)
{
radians += Angle.TWOPI;
}
alpha2 = new Angle()
{
Radians = radians
};
}
distance = s;
azimuth = alpha1;
reverseAzimuth = alpha2;
}
