diff options
| -rw-r--r-- | TODO | 51 | ||||
| -rw-r--r-- | common.h | 4 | ||||
| -rw-r--r-- | libtellurian.7 | 13 | ||||
| -rw-r--r-- | libtellurian.h | 32 | ||||
| -rw-r--r-- | libtellurian_azimuth.c | 37 | ||||
| -rw-r--r-- | libtellurian_azimuth_radians.c | 17 | ||||
| -rw-r--r-- | libtellurian_azimuthal_radius_radians.c | 5 | ||||
| -rw-r--r-- | libtellurian_distance.3 | 23 | ||||
| -rw-r--r-- | libtellurian_distance.c | 45 | ||||
| -rw-r--r-- | libtellurian_distance_radians.c | 31 | ||||
| -rw-r--r-- | libtellurian_effective_gravity_radians.c | 3 | ||||
| -rw-r--r-- | libtellurian_elevated_gravity_radians.c | 3 | ||||
| -rw-r--r-- | libtellurian_end_point.3 | 16 | ||||
| -rw-r--r-- | libtellurian_end_point_radians.c | 2 | ||||
| -rw-r--r-- | libtellurian_gaussian_radius_radians.c | 2 | ||||
| -rw-r--r-- | libtellurian_meridian_radius_radians.c | 2 | ||||
| -rw-r--r-- | libtellurian_sea_level_radians.c | 12 | ||||
| -rw-r--r-- | libtellurian_transverse_radius_radians.c | 2 | ||||
| -rw-r--r-- | libtellurian_vincenty_inverse__.c | 161 |
19 files changed, 400 insertions, 61 deletions
@@ -1 +1,52 @@ Finish libtellurian_vincenty_inverse__ +Document the details of NaN returns in libtellurian_distance.3 +Document what happens to the azimuths when the starting point is a pole. + +libtellurian_coarse_distance can be simplified to (d/2R)² +which would be an good alternative for simply sorting distances, +and it would still be easy to afterwards convert the values for +a selection of them into approximate distances. + +Add inverse function of libtellurian_effective_gravity_radians + +Add inverse function of libtellurian_elevated_gravity_radians + +And functions for converting between coordinate systems + Geodetic (h, φ, λ) : Angle between normal and equatorial plane + Inverse of Cartesian (see Cartesian for definition of N) + Iteration is required unless φ or h is known + λ = atan2(Y, X) + h = p / cos φ - N + φ = tan⁻¹(Zp⁻¹/(1 - e²N/(N + h))) + where p = √(X² + Y²) + https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_geodetic_coordinates + When h=0 + λ = atan2(Y, X) + φ = tan⁻¹(Zp⁻¹/(1 - e²)) + where p = √(X² + Y²) + wouldn't it easier to convert to geocentric intermittently + Geocentric ϕ : Angle from centre of earth + ϕ = tan⁻¹((1 - e²) tan φ) + Geometric : Arc length = geodetic + Parametric/reduced β : spherical angle resulting in same distance from polar axis as + β = tan⁻¹((1 - f) tan φ) + Ellipsoidal-harmonic + TODO + Rectifying μ + μ = πm(φ) / 2m(½π), where m(u) = a(1 - e²) ∫{0→u} √(1 - e² sin² v)⁻³ dv + Authalic ξ + ξ = sin⁻¹(q(φ) / q(½π)), where q(u) = ((1 - e²) sin u)/(1 - e² sin u) + (1 - e²) e⁻¹ tanh⁻¹ (e sin u) + q(½π) = 1 + (1 - e²) e⁻¹ tanh⁻¹ e + Conformal χ + χ = tan⁻¹ (sinh [sinh⁻¹ tan φ - e tanh⁻¹ (e sin φ)]) + Isometric ψ + ψ = sinh⁻¹ tan φ - e than⁻¹ (e sin φ) + Astronomical Φ : angle between equatorial plane and the true vertical direction + the true vertical direction, is the direction of gravity which is affect + byu the centrifugal acceleration in addition to the gravitational acceleration. + Cartesian (geocentric Cartesian) + X = (N + h) cos φ cos λ + Y = (N + h) cos φ sin λ + Z = (Nb²/a² + h) sin φ + where + N = a²/√(a² cos² φ + b² sin² φ) @@ -14,6 +14,10 @@ #define degrees(RAD) (180.0 / (double)M_PI * (RAD)) #define haversin(X) (fma(-0.5, cos(X), 0.5)) +#define geodetic(GEOCENTRIC)\ + atan(tan(GEOCENTRIC) * (LIBTELLURIAN_EQUATORIAL_RADIUS * LIBTELLURIAN_EQUATORIAL_RADIUS /\ + (LIBTELLURIAN_POLAR_RADIUS * LIBTELLURIAN_POLAR_RADIUS))) + void libtellurian_vincenty_inverse__(double latitude1, double longitude1, double latitude2, double longitude2, double *distance_out, double *azimuth1_out, double *azimuth2_out); diff --git a/libtellurian.7 b/libtellurian.7 index 0933aa9..ae3dff0 100644 --- a/libtellurian.7 +++ b/libtellurian.7 @@ -95,3 +95,16 @@ radius of curvature for some latitude. See .BR libtellurian.h (0) for a listing of available constants. +.SH NOTES +Users, and developers in particular, shall be aware that +this library uses GPS coordinates which are geocentric. +This means that the latitude is base one the angle from +the centre on the earth. There are a number of other +systems, most famously as a linear function of the +arc length, or more commonly in geodesy, the geodesic +latitude which is the enable between the local normal +from the surface and equatorial plane. Users should +take great care ensure the correct correct system (GPS) +is used, and developers should be careful designing +functions for the GPS coordinate system and be aware +the geodesic formulae usually use geodesic latitudes. diff --git a/libtellurian.h b/libtellurian.h index b294e9c..98309be 100644 --- a/libtellurian.h +++ b/libtellurian.h @@ -219,6 +219,15 @@ double libtellurian_coarse_distance_radians(double latitude1, double longitude1, * performed. If you have no need for an azimuth you can set * the corresponding output parameter to `NULL`, and the * function will not compute it. + * + * If the two points are they same, the distance will be 0 + * and the azimuths will be NaN, denoting that any direction + * can be taken. If the two points are antipodal, the distance + * will be `LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE` and the + * azimuths will be NaN denoting that are multiple directions + * to take, specifying either any (if the two points are the + * poles) or either north or south (and the two azimuths shall + * be the opposite of each other). */ LIBTELLURIAN_WUR__ double libtellurian_distance(double latitude1, double longitude1, @@ -247,6 +256,15 @@ double libtellurian_distance(double latitude1, double longitude1, * performed. If you have no need for an azimuth you can set * the corresponding output parameter to `NULL`, and the * function will not compute it. + * + * If the two points are they same, the distance will be 0 + * and the azimuths will be NaN, denoting that any direction + * can be taken. If the two points are antipodal, the distance + * will be `LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE` and the + * azimuths will be NaN denoting that are multiple directions + * to take, specifying either any (if the two points are the + * poles) or either north or south (and the two azimuths shall + * be the opposite of each other). */ LIBTELLURIAN_WUR__ double libtellurian_distance_radians(double latitude1, double longitude1, @@ -271,6 +289,13 @@ double libtellurian_distance_radians(double latitude1, double longitude1, * This function is identical to libtellurian_distance` * except it does not compute the distance between the * points + * + * If the two points are they same, the azimuths will be NaN, + * denoting that any direction can be taken. If the two points + * are antipodal, the azimuths will be NaN denoting that are + * multiple directions to take, specifying either any (if the + * two points are the poles) or either north or south (and + * the two azimuths shall be the opposite of each other). */ void libtellurian_azimuth(double latitude1, double longitude1, double latitude2, double longitude2, @@ -294,6 +319,13 @@ void libtellurian_azimuth(double latitude1, double longitude1, * * This function is identical to libtellurian_distance_radians` * except it does not compute the distance between the points + * + * If the two points are they same, the azimuths will be NaN, + * denoting that any direction can be taken. If the two points + * are antipodal, the azimuths will be NaN denoting that are + * multiple directions to take, specifying either any (if the + * two points are the poles) or either north or south (and + * the two azimuths shall be the opposite of each other). */ void libtellurian_azimuth_radians(double latitude1, double longitude1, double latitude2, double longitude2, diff --git a/libtellurian_azimuth.c b/libtellurian_azimuth.c index 4bfc743..c39596a 100644 --- a/libtellurian_azimuth.c +++ b/libtellurian_azimuth.c @@ -21,5 +21,40 @@ libtellurian_azimuth(double latitude1, double longitude1, #else -TODO_TEST + + +static int +check(double da, double db, double dc, double dd, + double ra, double rb, double rc, double rd) +{ + double az1_ref = 1, az2_ref = 2, az1 = 3, az2 = 4, az1b = 5, az2b = 6; + + libtellurian_azimuth_radians(ra, rb, rc, rd, &az1_ref, &az2_ref); + az1_ref *= 180 / M_PI; + az2_ref *= 180 / M_PI; + + libtellurian_azimuth(da, db, dc, dd, &az1, &az2); + libtellurian_azimuth(da, db, dc, dd, &az1b, NULL); + libtellurian_azimuth(da, db, dc, dd, NULL, &az2b); + libtellurian_azimuth(da, db, dc, dd, NULL, NULL); + return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b; +} + +int +main(void) +{ + double a1 = 1, a2 = 2; + + ASSERT(check(30, 45, 60, 90, D30, D45, D60, D90)); + ASSERT(check(30, 45, 60, 45, D30, D45, D60, D45)); + ASSERT(check(45, 30, 60, 45, D45, D30, D60, D45)); + ASSERT(check(45, 30, 30, 45, D45, D30, D30, D45)); + + libtellurian_azimuth(0, 0, 0, 90, &a1, &a2); + ASSERT(a1 == a2); + ASSERT(a1 == 90); + return 0; +} + + #endif diff --git a/libtellurian_azimuth_radians.c b/libtellurian_azimuth_radians.c index 18c954e..469eb45 100644 --- a/libtellurian_azimuth_radians.c +++ b/libtellurian_azimuth_radians.c @@ -17,9 +17,16 @@ libtellurian_azimuth_radians(double latitude1, double longitude1, #else -#if 1 -TODO_TEST -#else +static int +eq(double a, double b) +{ + if (isfinite(a) && isfinite(b)) + return a == b; + if (isnan(a) && isnan(b)) + return 1; + printf("Unexpected floating-point class\n"); + return 0; +} static int check(double a, double b, double c, double d) @@ -34,7 +41,7 @@ check(double a, double b, double c, double d) libtellurian_azimuth_radians(a, b, c, d, &az1b, NULL); libtellurian_azimuth_radians(a, b, c, d, NULL, &az2b); libtellurian_azimuth_radians(a, b, c, d, NULL, NULL); - return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b; + return eq(az1, az1_ref) && eq(az2, az2_ref) && eq(az1, az1b) && eq(az2, az2b); } int @@ -49,7 +56,5 @@ main(void) return 0; } -#endif - #endif diff --git a/libtellurian_azimuthal_radius_radians.c b/libtellurian_azimuthal_radius_radians.c index d58a903..e9433e3 100644 --- a/libtellurian_azimuthal_radius_radians.c +++ b/libtellurian_azimuthal_radius_radians.c @@ -6,8 +6,9 @@ double libtellurian_azimuthal_radius_radians(double latitude, double azimuth) { - double m = libtellurian_meridian_radius_radians(latitude); - double n = libtellurian_transverse_radius_radians(latitude); + double lat = geodetic(latitude); + double m = libtellurian_meridian_radius_radians(lat); + double n = libtellurian_transverse_radius_radians(lat); double c2 = cos(azimuth) * cos(azimuth); double s2 = sin(azimuth) * sin(azimuth); double x = c2 / m; diff --git a/libtellurian_distance.3 b/libtellurian_distance.3 index fc753af..d477f53 100644 --- a/libtellurian_distance.3 +++ b/libtellurian_distance.3 @@ -104,6 +104,29 @@ functions do not return any value. .SH ERRORS None. +.SH APPLICATION USAGE +The application should use +.BR libtellurian_azimuth () +or +.BR libtellurian_azimuth_radians () +whenever the distance will not be used by the +application, likewise, if +.I *azimuth1_out +will not be used +.I azimuth1_out +should be set to +.I NULL +and if +.I *azimuth2_out +will not be used +.I azimuth2_out +should be set to +.IR NULL . +Although this currently has almost no impact on +the application, future versions of the library +may select different methods for performing the +calculations depending on what output is enabled. + .SH NOTES The (forward) azimuths are defined as the direction you travelling when you travel diff --git a/libtellurian_distance.c b/libtellurian_distance.c index 250dddc..dec0edd 100644 --- a/libtellurian_distance.c +++ b/libtellurian_distance.c @@ -23,5 +23,48 @@ libtellurian_distance(double latitude1, double longitude1, #else -TODO_TEST + + +static int +check(double da, double db, double dc, double dd, + double ra, double rb, double rc, double rd) +{ + double az1_ref = 1, az2_ref = 2, az1 = 3, az2 = 4, az1b = 5, az2b = 6; + double s_ref, s1, s2, s3, s4; + + s_ref = libtellurian_distance_radians(ra, rb, rc, rd, &az1_ref, &az2_ref); + az1_ref *= 180 / M_PI; + az2_ref *= 180 / M_PI; + + s1 = libtellurian_distance(da, db, dc, dd, &az1, &az2); + s2 = libtellurian_distance(da, db, dc, dd, &az1b, NULL); + s3 = libtellurian_distance(da, db, dc, dd, NULL, &az2b); + s4 = libtellurian_distance(da, db, dc, dd, NULL, NULL); + ASSERT(s1 == s_ref); + ASSERT(s2 == s_ref); + ASSERT(s3 == s_ref); + ASSERT(s4 == s_ref); + return az1 == az1_ref && az2 == az2_ref && az1 == az1b && az2 == az2b; +} + +int +main(void) +{ + double a1 = 1, a2 = 2, s; + + ASSERT(check(30, 45, 60, 90, D30, D45, D60, D90)); + ASSERT(check(30, 45, 60, 45, D30, D45, D60, D45)); + ASSERT(check(45, 30, 60, 45, D45, D30, D60, D45)); + ASSERT(check(45, 30, 30, 45, D45, D30, D30, D45)); + + s = libtellurian_distance(0, 0, 0, 90, &a1, &a2); + fprintf(stderr, "%lg\n", s); + fprintf(stderr, "%lg\n", s / LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE); + (void) s; + ASSERT(a1 == a2); + ASSERT(a1 == 90); + return 0; +} + + #endif diff --git a/libtellurian_distance_radians.c b/libtellurian_distance_radians.c index d7c5547..7401edd 100644 --- a/libtellurian_distance_radians.c +++ b/libtellurian_distance_radians.c @@ -17,5 +17,34 @@ libtellurian_distance_radians(double latitude1, double longitude1, #else -TODO_TEST + + +int +main(void) +{ + double rs, ra1, ra2; + double s, a1, a2; + + libtellurian_vincenty_inverse__(1, 2, 3, 4, &rs, &ra1, &ra2); + + s = libtellurian_distance_radians(1, 2, 3, 4, &a1, &a2); + ASSERT(s == rs); + ASSERT(a1 == ra1); + ASSERT(a2 == ra2); + + s = libtellurian_distance_radians(1, 2, 3, 4, &a1, NULL); + ASSERT(s == rs); + ASSERT(a1 == ra1); + + s = libtellurian_distance_radians(1, 2, 3, 4, NULL, &a2); + ASSERT(s == rs); + ASSERT(a2 == ra2); + + s = libtellurian_distance_radians(1, 2, 3, 4, NULL, NULL); + ASSERT(s == rs); + + return 0; +} + + #endif diff --git a/libtellurian_effective_gravity_radians.c b/libtellurian_effective_gravity_radians.c index 54e2b8e..cb23246 100644 --- a/libtellurian_effective_gravity_radians.c +++ b/libtellurian_effective_gravity_radians.c @@ -9,7 +9,8 @@ double libtellurian_effective_gravity_radians(double gravity, double latitude) { - return fma(-K / gravity, cos(latitude) * cos(latitude), gravity); + double lat = geodetic(latitude); /* actual one is geodetic and one is geographical, there is no actual difference */ + return fma(-K / gravity, cos(lat) * cos(lat), gravity); } diff --git a/libtellurian_elevated_gravity_radians.c b/libtellurian_elevated_gravity_radians.c index 1c97f1a..edf9678 100644 --- a/libtellurian_elevated_gravity_radians.c +++ b/libtellurian_elevated_gravity_radians.c @@ -6,6 +6,7 @@ double libtellurian_elevated_gravity_radians(double gravity, double latitude, double altitude) { + double lat = geodetic(latitude); double a = LIBTELLURIAN_EQUATORIAL_RADIUS; double b = LIBTELLURIAN_POLAR_RADIUS; double omega = LIBTELLURIAN_ANGULAR_VELOCITY; @@ -15,7 +16,7 @@ libtellurian_elevated_gravity_radians(double gravity, double latitude, double al double neg_k1 = fma(-2.0, f + m, -2.0) / a; double k2 = 4.0 * f / a; double neg_k3 = -3.0 / (a * a); - double sin2_phi = sin(latitude) * sin(latitude); + double sin2_phi = sin(lat) * sin(lat); double s = fma(neg_k3, altitude, fma(k2, sin2_phi, neg_k1)); return fma(s * altitude, gravity, gravity); } diff --git a/libtellurian_end_point.3 b/libtellurian_end_point.3 index 3bf78b2..483c6b0 100644 --- a/libtellurian_end_point.3 +++ b/libtellurian_end_point.3 @@ -83,6 +83,22 @@ None. .SH ERRORS None. +.SH APPLICATION USAGE +If +.I *azimuth2_out +will not be used by the application +.I azimuth2_out +should be set to +.I NULL +and likwise for +.I latitude2_out +and +.IR longitude2_out . +Although this currently has almost no impact on +the application, future versions of the library +may select different methods for performing the +calculations depending on what output is enabled. + .SH NOTES The (forward) azimuths are defined as the direction you travelling when you travel diff --git a/libtellurian_end_point_radians.c b/libtellurian_end_point_radians.c index 036ac99..ff00f5c 100644 --- a/libtellurian_end_point_radians.c +++ b/libtellurian_end_point_radians.c @@ -37,7 +37,7 @@ libtellurian_end_point_radians(double latitude1, double longitude1, double azimu 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); - double sigma_prev = 0; + double sigma_prev; int max_interations = 20; sigma = sigma_0; diff --git a/libtellurian_gaussian_radius_radians.c b/libtellurian_gaussian_radius_radians.c index 02d0586..7b14ed5 100644 --- a/libtellurian_gaussian_radius_radians.c +++ b/libtellurian_gaussian_radius_radians.c @@ -11,7 +11,7 @@ libtellurian_gaussian_radius_radians(double latitude) double c = b / a; double neg_e2 = fma(c, c, -1.0); double neg_e2_plus_1 = c * c; - double s = sin(latitude); + double s = sin(geodetic(latitude)); double denom = fma(neg_e2, s * s, 1.0); return a * sqrt(neg_e2_plus_1) / denom; } diff --git a/libtellurian_meridian_radius_radians.c b/libtellurian_meridian_radius_radians.c index f550289..d5213c1 100644 --- a/libtellurian_meridian_radius_radians.c +++ b/libtellurian_meridian_radius_radians.c @@ -9,7 +9,7 @@ libtellurian_meridian_radius_radians(double latitude) double a = LIBTELLURIAN_EQUATORIAL_RADIUS; double b = LIBTELLURIAN_POLAR_RADIUS; double neg_e2 = fma(b / a, b / a, -1.0); - double s = sin(latitude); + double s = sin(geodetic(latitude)); return fma(a, neg_e2, a) / pow(fma(neg_e2, s * s, 1.0), 1.5); } diff --git a/libtellurian_sea_level_radians.c b/libtellurian_sea_level_radians.c index e536025..ec03985 100644 --- a/libtellurian_sea_level_radians.c +++ b/libtellurian_sea_level_radians.c @@ -6,15 +6,9 @@ double libtellurian_sea_level_radians(double latitude) { - double a = LIBTELLURIAN_EQUATORIAL_RADIUS; - double b = LIBTELLURIAN_POLAR_RADIUS; - double c = cos(latitude); - double s = sin(latitude); - double x = a * c * a; - double y = b * s * b; - double num = fma(x, x, y * y); - double denom = fma(x, c, y * s); - return sqrt(num / denom); + double x = cos(latitude) * LIBTELLURIAN_EQUATORIAL_RADIUS; + double y = sin(latitude) * LIBTELLURIAN_POLAR_RADIUS; + return sqrt(fma(x, x, y * y)); } diff --git a/libtellurian_transverse_radius_radians.c b/libtellurian_transverse_radius_radians.c index c4f80aa..3e4950e 100644 --- a/libtellurian_transverse_radius_radians.c +++ b/libtellurian_transverse_radius_radians.c @@ -9,7 +9,7 @@ libtellurian_transverse_radius_radians(double latitude) double a = LIBTELLURIAN_EQUATORIAL_RADIUS; double b = LIBTELLURIAN_POLAR_RADIUS; double neg_e2 = fma(b / a, b / a, -1.0); - double s = sin(latitude); + double s = sin(geodetic(latitude)); return a * pow(fma(neg_e2, s * s, 1.0), -0.5); } 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 <stdio.h> // 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 |