diff options
Diffstat (limited to '')
| -rw-r--r-- | libtellurian_end_point_radians.c | 162 |
1 files changed, 148 insertions, 14 deletions
diff --git a/libtellurian_end_point_radians.c b/libtellurian_end_point_radians.c index c512b59..036ac99 100644 --- a/libtellurian_end_point_radians.c +++ b/libtellurian_end_point_radians.c @@ -3,8 +3,6 @@ #ifndef TEST -/* 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) @@ -12,6 +10,7 @@ libtellurian_end_point_radians(double latitude1, double longitude1, double azimu /* * 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; @@ -38,23 +37,26 @@ 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; + int max_interations = 20; sigma = sigma_0; - /* { */ + do { + sigma_prev = sigma; - cos_2sigma_m = cos(fma(2.0, sigma1, sigma)); - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); + 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); + 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 */ + } while (fabs(sigma - sigma_prev) > 1e-14 && --max_interations); if (latitude2_out || azimuth2_out) { x0 = fma(cos_sigma * cos_alpha1, cos_u1, -sin_u1 * sin_alpha); @@ -82,5 +84,137 @@ libtellurian_end_point_radians(double latitude1, double longitude1, double azimu #else -TODO_TEST + + +# define PM LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE +# define PE LIBTELLURIAN_EQUATORIAL_CIRCUMFERENCE + +static int +approx(double a, double b) +{ + return fabs(a - b) <= 1e-8; +} + +static double +normal_angle(double u) +{ + u = fmod(u + 4.0 * M_PI, 2.0 * M_PI); + if (u > M_PI) + u -= 2.0 * M_PI; + return u; +} + +static void +end_point(double a, double b, double c, double d, + double *lat_out, double *lon_out, double *az_out) +{ + libtellurian_end_point_radians(a, b, c, d, lat_out, lon_out, az_out); + if (lat_out) { + *lat_out = normal_angle(*lat_out); + if (*lat_out > M_PI / 2) { + *lat_out = M_PI - *lat_out; + if (*lon_out) + *lon_out += M_PI; + } else if (*lat_out < -M_PI / 2) { + *lat_out = -M_PI - *lat_out; + if (*lon_out) + *lon_out += M_PI; + } + } + if (lon_out) + *lon_out = normal_angle(*lon_out); + if (az_out) + *az_out = normal_angle(*az_out); +} + +int +main(void) +{ + double lat, lon, az; + + end_point(0, 0, 0, 0, &lat, &lon, &az); + ASSERT(lat == 0); + ASSERT(lon == 0); + ASSERT(az == 0); + + end_point(0, 0, 3, 0, &lat, &lon, &az); + ASSERT(lat == 0); + ASSERT(lon == 0); + ASSERT(az == 3); + + end_point(0, 0, 6, 0, &lat, &lon, &az); + ASSERT(lat == 0); + ASSERT(lon == 0); + ASSERT(az == normal_angle(6)); + + end_point(0, 0, 0, PM / 4, &lat, &lon, &az); + ASSERT(approx(lat, D90)); + ASSERT(approx(lon, 0)); + + end_point(0, 0, 0, PM / 4, NULL, &lon, &az); + ASSERT(approx(lon, 0)); + + end_point(0, 0, 0, PM / 2, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D180)); + ASSERT(approx(az, D180)); + + end_point(0, 0, 0, PM / 2, &lat, NULL, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(az, D180)); + + end_point(0, 0, 0, PM / 2, &lat, &lon, NULL); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D180)); + + end_point(0, 0, 0, PM / 2, &lat, NULL, NULL); + ASSERT(approx(lat, 0)); + + end_point(0, 0, 0, PM / 2, NULL, NULL, NULL); + + end_point(0, 0, 0, PM / 2, NULL, NULL, &az); + ASSERT(approx(az, D180)); + + end_point(0, 0, 0, PM / 2, NULL, &lon, NULL); + ASSERT(approx(lon, D180)); + + end_point(0, 0, 0, PM, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, 0)); + ASSERT(approx(az, 0)); + + end_point(0, 0, 0, 2 * PM, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, 0)); + ASSERT(approx(az, 0)); + + end_point(0, 0, 0, 3 * PM / 2, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D180)); + ASSERT(approx(az, D180)); + + end_point(0, 0, D90, PE, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, 0)); + ASSERT(approx(az, D90)); + + end_point(0, 0, D90, PE / 8, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D180 / 4)); + ASSERT(approx(az, D90)); + + end_point(0, 0, D90, PE / 4, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D90)); + ASSERT(approx(az, D90)); + + end_point(0, 0, D90, PE / 2, &lat, &lon, &az); + ASSERT(approx(lat, 0)); + ASSERT(approx(lon, D180)); + ASSERT(approx(az, D90)); + + return 0; +} + + #endif |