From 3ce34980d7ba2bddbf3e9a1bd6f98cbc855bddc2 Mon Sep 17 00:00:00 2001 From: Mattias Andrée Date: Sat, 19 Oct 2024 18:21:54 +0200 Subject: First commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mattias Andrée --- libtellurian_end_point_radians.c | 79 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 libtellurian_end_point_radians.c (limited to 'libtellurian_end_point_radians.c') diff --git a/libtellurian_end_point_radians.c b/libtellurian_end_point_radians.c new file mode 100644 index 0000000..79e759f --- /dev/null +++ b/libtellurian_end_point_radians.c @@ -0,0 +1,79 @@ +/* See LICENSE file for copyright and license details. */ +#include "common.h" + + +/* TODO complete the implementation */ + +void +libtellurian_end_point_radians(double latitude1, double longitude1, double azimuth1, double distance, + double *latitude2_out, double *longitude2_out, double *azimuth2_out) +{ + /* + * Vincenty's formula for the "direct problem" + */ + double t, sigma, cos_2sigma_m, sin_sigma, cos_sigma; + double x, x0, y, C, L, v, lambda; + + 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 sin_u1 = sin(u1); + double cos_u1 = cos(u1); + double sin_alpha1 = sin(azimuth1); + double cos_alpha1 = cos(azimuth1); + + double sigma1 = atan2(tan(u1), cos_alpha1); + + double sin_alpha = cos_u1 * sin_alpha1; + double cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0); + + double uu = cos2_alpha * fma(a / b, a / b, -1.0); + + double A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0); + double B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0); + + double sigma_0 = distance / (b * A); + + sigma = sigma_0; + + /* { */ + + cos_2sigma_m = cos(fma(2.0, sigma1, sigma)); + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + + v = cos_sigma * fma(2 * cos_2sigma_m, cos_2sigma_m, -1.0); + t = fma(2.0 * cos_2sigma_m, 2.0 * cos_2sigma_m, -3.0); + t *= fma(2.0 * sin_sigma, 2.0 * sin_sigma, -3.0); + t = fma(B * cos_2sigma_m / -6.0, t, v); + t = fma(0.25 * B, t, cos_2sigma_m); + sigma = fma(B * sin_sigma, t, sigma_0); + + /* } repeat until sigma converges */ + + if (latitude2_out || azimuth2_out) + x0 = fma(cos_sigma * cos_alpha1, cos_u1, -sin_u1 * sin_alpha); + + if (latitude2_out) { + y = fma(sin_sigma * cos_alpha1, cos_u1, sin_u1 * cos_sigma); + x = sqrt(fma(sin_alpha, sin_alpha, x0 * x0)) * c; + *latitude2_out = atan2(y, x); + } + + if (azimuth2_out) + *azimuth2_out = atan2(sin_alpha, x0); + + if (longitude2_out) { + C = fma(fma(-0.75, cos2_alpha, 1.0), f, 1.0) * cos2_alpha * f / 4.0; + y = sin_sigma * sin_alpha1; + x = fma(sin_sigma * cos_alpha1, sin_u1, cos_u1 * cos_sigma); + lambda = atan2(y, x); + t = fma(C * sin_sigma, fma(C, v, cos_2sigma_m), sigma); + L = fma(fma(C, f, -f) * sin_alpha, t, lambda); + *longitude2_out = longitude1 + L; + } +} -- cgit v1.2.3-70-g09d2