/* See LICENSE file for copyright and license details. */
#include "common.h"
#ifndef TEST
#include <stdio.h> // TODO remove
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;
double lambda_prev;
int max_interations = 20;
lambda = L;
do {
lambda_prev = lambda;
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;
if (!isfinite(sin_alpha)) {
fprintf(stderr, "BAD\n");
if (distance_out) {
/* Dot product of vectors is 0 if coincidental, but π if antipodal */
/* TODO it may be fast to see if latitude1+latitude2≋0 and longitude2-longitude1≋π */
t = sin(latitude1) * sin(latitude2);
t = fma(cos(latitude1) * cos(latitude2), cos(longitude2 - longitude1), t);
if (t > 0.0) {
fprintf(stderr, " COINCIDENTAL\n");
*distance_out = 0;
} else {
fprintf(stderr, " ANTIPODAL\n");
*distance_out = LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE;
}
}
if (azimuth1_out)
*azimuth1_out = nan("");
if (azimuth2_out)
*azimuth2_out = nan("");
return;
}
cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);
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);
} while (lambda != lambda_prev && --max_interations); /* repeat until lambda converges */
fprintf(stderr, "%lg => %lg -> %lg : %lg (%i)\n", L, lambda_prev, lambda, lambda - lambda_prev, max_interations);
if (distance_out) {
uu = cos2_alpha * fma(c, c, -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);
*azimuth2_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
*/
}
#else
# define PE LIBTELLURIAN_EQUATORIAL_CIRCUMFERENCE
# define PM LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE
static int
approx(double a, double e)
{
return fabs((a / e) - 1.0) <= 1.0e-8;
}
int
main(void)
{
double s, a1, a2;
#define RESET ((void)(s = 111, a1 = 222, a2 = 333))
libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, &a2);
libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, NULL);
libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, &a2);
libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, NULL);
libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, &a2);
libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, NULL);
libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, &a2);
libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, NULL);
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, &a2);
ASSERT(s == 0);
ASSERT(isnan(a1));
ASSERT(isnan(a2));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, &a2);
ASSERT(isnan(a1));
ASSERT(isnan(a2));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, &a2);
ASSERT(s == 0);
ASSERT(isnan(a2));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, &a2);
ASSERT(isnan(a2));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, NULL);
ASSERT(s == 0);
ASSERT(isnan(a1));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, NULL);
ASSERT(isnan(a1));
RESET;
libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, NULL);
ASSERT(s == 0);
libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, NULL);
fprintf(stderr, "--------\n");
RESET;
libtellurian_vincenty_inverse__(0, 0, D45, 0, &s, &a1, &a2);
ASSERT(approx(s, PM / 4));
ASSERT(isnan(a1));
ASSERT(isnan(a2));
return 0;
}
#endif
