From bb5de3aa2ee118df78f0347cffd4e58f846dc1fb Mon Sep 17 00:00:00 2001 From: Mattias Andrée Date: Tue, 22 Oct 2024 22:22:41 +0200 Subject: ... MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mattias Andrée --- libtellurian_vincenty_inverse__.c | 161 +++++++++++++++++++++++++++++--------- 1 file changed, 126 insertions(+), 35 deletions(-) (limited to 'libtellurian_vincenty_inverse__.c') diff --git a/libtellurian_vincenty_inverse__.c b/libtellurian_vincenty_inverse__.c index c50752e..4d8d6ea 100644 --- a/libtellurian_vincenty_inverse__.c +++ b/libtellurian_vincenty_inverse__.c @@ -1,10 +1,9 @@ /* See LICENSE file for copyright and license details. */ #include "common.h" #ifndef TEST +#include // TODO remove -/* 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) @@ -34,42 +33,62 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati double sin_u1_sin_u2 = sin_u1 * sin_u2; double L = longitude2 - longitude1; + double lambda_prev; + int max_interations = 20; 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 */ + 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(a / b, a / b, -1.0); + 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); @@ -89,7 +108,7 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati 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); + *azimuth2_out = atan2(y, x); } /* @@ -106,5 +125,77 @@ libtellurian_vincenty_inverse__(double latitude1, double longitude1, double lati #else -TODO_TEST + + +# 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 -- cgit v1.2.3-70-g09d2