diff options
Diffstat (limited to '')
| -rw-r--r-- | libtellurian_vincenty_inverse__.c | 104 |
1 files changed, 104 insertions, 0 deletions
diff --git a/libtellurian_vincenty_inverse__.c b/libtellurian_vincenty_inverse__.c new file mode 100644 index 0000000..746011f --- /dev/null +++ b/libtellurian_vincenty_inverse__.c @@ -0,0 +1,104 @@ +/* See LICENSE file for copyright and license details. */ +#include "common.h" + + +/* TODO complete the implementation */ + +void +libtellurian_vincenty_inverse__(double latitude1, double longitude1, double latitude2, double longitude2, + double *distance_out, double *azimuth1_out, double *azimuth2_out) +{ + /* + * Vincenty's formula for the "inverse problem" + */ + + double lambda, t, uu, A, B, sigma_minus_delta_sigma; + double x, y, cos_lambda, sin_lambda, sin_sigma, cos_sigma, sigma; + double sin_alpha, cos2_alpha, cos_2sigma_m, C, cos2_2sigma_m; + + double a = LIBTELLURIAN_EQUATORIAL_RADIUS; + double b = LIBTELLURIAN_POLAR_RADIUS; + double c = b / a; + double f = 1.0 - c; + + double u1 = atan(c * tan(latitude1)); + double u2 = atan(c * tan(latitude2)); + + double cos_u1 = cos(u1), sin_u1 = sin(u1); + double cos_u2 = cos(u2), sin_u2 = sin(u2); + + double cos_u1_sin_u2 = cos_u1 * sin_u2; + double sin_u1_cos_u2 = sin_u1 * cos_u2; + double cos_u1_cos_u2 = cos_u1 * cos_u2; + double sin_u1_sin_u2 = sin_u1 * sin_u2; + + double L = longitude2 - longitude1; + + lambda = L; + + /* { */ + + cos_lambda = cos(lambda); + sin_lambda = sin(lambda); + + y = cos_u2 * sin_lambda; + x = fma(-sin_u1_cos_u2, cos_lambda, cos_u1_sin_u2); + + sin_sigma = sqrt(fma(y, y, x * x)); + cos_sigma = fma(cos_u1_cos_u2, cos_lambda, sin_u1_sin_u2); + sigma = atan2(sin_sigma, cos_sigma); + + sin_alpha = (cos_u1_cos_u2 * sin_lambda) / sin_sigma; + cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0); + /* + * If sin σ = 0 the value of sin α is indeterminate. + * It represents an end point coincident with, or + * diametrically opposed to, the start point. + */ + + cos_2sigma_m = fma(-2.0 / cos2_alpha, sin_u1_sin_u2, cos_sigma); + C = 0.25 * f * cos2_alpha * fma(f, fma(-0.75, cos2_alpha, 1.0), 1.0); + + cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m; + t = fma(2.0, cos2_2sigma_m, -1.0); + t = fma(C * cos_sigma, t, cos_2sigma_m); + t = fma(C * sin_sigma, t, sigma); + lambda = fma(fma(C, f, -f) * sin_alpha, t, L); + + /* } repeat until lambda converges */ + + if (distance_out) { + uu = cos2_alpha * fma(a / b, a / b, -1.0); + A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0); + B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0); + + t = fma(4.0, cos2_2sigma_m, -3.0); + t *= fma(2.0 * sin_sigma, 2.0 * sin_sigma, -3.0); + t *= cos_2sigma_m * B / -6.0; + t = fma(cos_sigma, fma(2.0, cos2_2sigma_m, -1.0), t); + t = fma(t, B / 4.0, cos_2sigma_m); + sigma_minus_delta_sigma = fma(t, -B * sin_sigma, sigma); + + *distance_out = b * A * sigma_minus_delta_sigma; + } + + if (azimuth1_out) + *azimuth1_out = atan2(y, x); + + if (azimuth2_out) { + y = cos_u1 * sin_lambda; + x = fma(cos_u1_sin_u2, cos_lambda, -sin_u1_cos_u2); + *azimuth1_out = atan2(y, x); + } + + /* + * Between two nearly antipodal points, the iterative formula may fail to converge; + * this will occur when the first guess at λ as computed by the equation above is + * greater than π in absolute value. + */ + + /* + * https://en.wikipedia.org/wiki/Vincenty's_formulae#Vincenty's_modification + * https://en.wikipedia.org/wiki/Vincenty's_formulae#Nearly_antipodal_points + */ +} |