1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
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;
}
}
|
