mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
284 lines
No EOL
9.4 KiB
C++
284 lines
No EOL
9.4 KiB
C++
#include "vincentys_calculations.h"
|
|
#include "wgs84.h"
|
|
|
|
#define M_2PI (M_PI * 2)
|
|
|
|
/*
|
|
The code in this file was ported to C++ and slightly modified
|
|
from https://github.com/airbreather/Gavaghan.Geodesy/blob/master/Source/Gavaghan.Geodesy/GeodeticCalculator.cs
|
|
|
|
Original license :
|
|
---------------------------------------------------------------------------------------
|
|
Gavaghan.Geodesy by Mike Gavaghan
|
|
|
|
http://www.gavaghan.org/blog/free-source-code/geodesy-library-vincentys-formula/
|
|
|
|
This code may be freely used and modified on any personal or professional
|
|
project. It comes with no warranty.
|
|
|
|
BitCoin tips graciously accepted at 1FB63FYQMy7hpC2ANVhZ5mSgAZEtY1aVLf
|
|
---------------------------------------------------------------------------------------
|
|
*/
|
|
namespace geodetic
|
|
{
|
|
geodetic_coords_t vincentys_forward(geodetic_coords_t start, double initialBearing, double distance, double &finalBearing, double tolerance)
|
|
{
|
|
const double &a = WGS84::a;
|
|
const double &b = WGS84::b;
|
|
const double aSquared = a * a;
|
|
const double bSquared = b * b;
|
|
const double &f = WGS84::f;
|
|
|
|
start.toRads(); // Ensure
|
|
|
|
double phi1 = start.lat;
|
|
double alpha1 = initialBearing;
|
|
double cosAlpha1 = cos(alpha1);
|
|
double sinAlpha1 = sin(alpha1);
|
|
double s = distance * 1000; // To meters from Km
|
|
double tanU1 = (1.0 - f) * tan(phi1);
|
|
double cosU1 = 1.0 / sqrt(1.0 + tanU1 * tanU1);
|
|
double sinU1 = tanU1 * cosU1;
|
|
|
|
// eq. 1
|
|
double sigma1 = atan2(tanU1, cosAlpha1);
|
|
|
|
// eq. 2
|
|
double sinAlpha = cosU1 * sinAlpha1;
|
|
double sin2Alpha = sinAlpha * sinAlpha;
|
|
double cos2Alpha = 1 - sin2Alpha;
|
|
double uSquared = cos2Alpha * (aSquared - bSquared) / bSquared;
|
|
|
|
// eq. 3
|
|
double A = 1 + (uSquared / 16384) * (4096 + uSquared * (-768 + uSquared * (320 - 175 * uSquared)));
|
|
|
|
// eq. 4
|
|
double B = (uSquared / 1024) * (256 + uSquared * (-128 + uSquared * (74 - 47 * uSquared)));
|
|
|
|
// iterate until there is a negligible change in sigma
|
|
double deltaSigma;
|
|
double sOverbA = s / (b * A);
|
|
double sigma = sOverbA;
|
|
double sinSigma;
|
|
double prevSigma = sOverbA;
|
|
double sigmaM2;
|
|
double cosSigmaM2;
|
|
double cos2SigmaM2;
|
|
|
|
for (;;)
|
|
{
|
|
// eq. 5
|
|
sigmaM2 = 2.0 * sigma1 + sigma;
|
|
cosSigmaM2 = cos(sigmaM2);
|
|
cos2SigmaM2 = cosSigmaM2 * cosSigmaM2;
|
|
sinSigma = sin(sigma);
|
|
double cosSignma = cos(sigma);
|
|
|
|
// eq. 6
|
|
deltaSigma = B * sinSigma * (cosSigmaM2 + (B / 4.0) * (cosSignma * (-1 + 2 * cos2SigmaM2) - (B / 6.0) * cosSigmaM2 * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM2)));
|
|
|
|
// eq. 7
|
|
sigma = sOverbA + deltaSigma;
|
|
|
|
// break after converging to tolerance
|
|
if (abs(sigma - prevSigma) < tolerance)
|
|
break;
|
|
|
|
prevSigma = sigma;
|
|
}
|
|
|
|
sigmaM2 = 2.0 * sigma1 + sigma;
|
|
cosSigmaM2 = cos(sigmaM2);
|
|
cos2SigmaM2 = cosSigmaM2 * cosSigmaM2;
|
|
|
|
double cosSigma = cos(sigma);
|
|
sinSigma = sin(sigma);
|
|
|
|
// eq. 8
|
|
double sinU1sinSigma_cosU1cosSigmacosAlpha1 = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
|
|
double phi2 = atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1, (1.0 - f) * sqrt(sin2Alpha + (sinU1sinSigma_cosU1cosSigmacosAlpha1 * sinU1sinSigma_cosU1cosSigmacosAlpha1)));
|
|
|
|
// eq. 9
|
|
// This fixes the pole crossing defect spotted by Matt Feemster. When a path
|
|
// passes a pole and essentially crosses a line of latitude twice - once in
|
|
// each direction - the longitude calculation got messed up. Using Atan2
|
|
// instead of Atan fixes the defect. The change is in the next 3 lines.
|
|
// double tanLambda = sinSigma * sinAlpha1 / (cosU1 * cosSigma - sinU1*sinSigma*cosAlpha1);
|
|
// double lambda = atan(tanLambda);
|
|
double lambda = atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
|
|
|
|
// eq. 10
|
|
double C = (f / 16) * cos2Alpha * (4 + f * (4 - 3 * cos2Alpha));
|
|
|
|
// eq. 11
|
|
double L = lambda - (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cosSigmaM2 + C * cosSigma * (-1 + 2 * cos2SigmaM2)));
|
|
|
|
// eq. 12
|
|
double alpha2 = atan2(sinAlpha, -sinU1 * sinSigma + cosU1 * cosSigma * cosAlpha1);
|
|
|
|
// build result
|
|
finalBearing = alpha2;
|
|
|
|
return geodetic_coords_t(phi2, start.lon + L, start.alt, true);
|
|
}
|
|
|
|
geodetic_curve_t vincentys_inverse(geodetic_coords_t start, geodetic_coords_t end, double tolerance)
|
|
{
|
|
//
|
|
// All equation numbers refer back to Vincenty's publication:
|
|
// See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
|
|
//
|
|
|
|
// get constants
|
|
const double &a = WGS84::a;
|
|
const double &b = WGS84::b;
|
|
const double &f = WGS84::f;
|
|
|
|
start.toRads();
|
|
end.toRads();
|
|
|
|
// get parameters as radians
|
|
double phi1 = start.lat;
|
|
double lambda1 = start.lon;
|
|
double phi2 = end.lat;
|
|
double lambda2 = end.lon;
|
|
|
|
// calculations
|
|
double a2 = a * a;
|
|
double b2 = b * b;
|
|
double a2b2b2 = (a2 - b2) / b2;
|
|
|
|
double omega = lambda2 - lambda1;
|
|
|
|
double tanphi1 = tan(phi1);
|
|
double tanU1 = (1.0 - f) * tanphi1;
|
|
double U1 = atan(tanU1);
|
|
double sinU1 = sin(U1);
|
|
double cosU1 = cos(U1);
|
|
|
|
double tanphi2 = tan(phi2);
|
|
double tanU2 = (1.0 - f) * tanphi2;
|
|
double U2 = atan(tanU2);
|
|
double sinU2 = sin(U2);
|
|
double cosU2 = 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 = sin(lambda);
|
|
double coslambda = cos(lambda);
|
|
|
|
// eq. 14
|
|
double cosU1sinU2_sinU2cosU2coslambda = cosU1sinU2 - sinU1cosU2 * coslambda;
|
|
double sin2sigma = (cosU2 * sinlambda * cosU2 * sinlambda) + (cosU1sinU2_sinU2cosU2coslambda * cosU1sinU2_sinU2cosU2coslambda);
|
|
double sinsigma = sqrt(sin2sigma);
|
|
|
|
// eq. 15
|
|
double cossigma = sinU1sinU2 + (cosU1cosU2 * coslambda);
|
|
|
|
// eq. 16
|
|
sigma = atan2(sinsigma, cossigma);
|
|
|
|
// eq. 17 Careful! sin2sigma might be almost 0!
|
|
double sinalpha = (sin2sigma == 0) ? 0.0 : cosU1cosU2 * sinlambda / sinsigma;
|
|
double alpha = asin(sinalpha);
|
|
double cosalpha = 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)));
|
|
|
|
if (i < 2)
|
|
continue;
|
|
|
|
// see how much improvement we got
|
|
double change = abs((lambda - lambda0) / lambda);
|
|
|
|
if (change < tolerance)
|
|
{
|
|
converged = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// eq. 19
|
|
double s = b * A * (sigma - deltasigma);
|
|
double alpha1 = 0;
|
|
double alpha2 = 0;
|
|
|
|
// didn't converge? must be N/S
|
|
if (!converged)
|
|
{
|
|
if (phi1 > phi2)
|
|
{
|
|
alpha1 = 180 * DEG_TO_RAD;
|
|
alpha2 = 0 * DEG_TO_RAD;
|
|
}
|
|
else if (phi1 < phi2)
|
|
{
|
|
alpha1 = 0 * DEG_TO_RAD;
|
|
alpha2 = 180 * DEG_TO_RAD;
|
|
}
|
|
else
|
|
{
|
|
// alpha1 = Angle.NaN;
|
|
// alpha2 = Angle.NaN;
|
|
// logger->error("Error");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// eq. 20
|
|
alpha1 = atan2(cosU2 * sin(lambda), (cosU1sinU2 - sinU1cosU2 * cos(lambda)));
|
|
if (alpha1 < 0.0)
|
|
alpha1 += M_2PI;
|
|
|
|
// eq. 21
|
|
alpha2 = atan2(cosU1 * sin(lambda), (-sinU1cosU2 + cosU1sinU2 * cos(lambda))) + M_PI;
|
|
if (alpha2 < 0.0)
|
|
alpha2 += M_2PI;
|
|
}
|
|
|
|
if (alpha1 >= M_2PI)
|
|
alpha1 = alpha1 - M_2PI;
|
|
if (alpha2 >= M_2PI)
|
|
alpha2 = alpha2 - M_2PI;
|
|
|
|
return geodetic_curve_t(s, alpha1, alpha2, true);
|
|
}
|
|
}; |