GeographicLib: Geocentric.cpp Source File (original) (raw)
1
2
3
4
5
6
7
8
9
11
13
14 using namespace std;
15
17 : _a(a)
18 , _f(f)
19 , _e2(_f * (2 - _f))
20 , _e2m(Math::sq(1 - _f))
21 , _e2a(fabs(_e2))
22 , _e4a(Math::sq(_e2))
23 , _maxrad(2 * _a / numeric_limits::epsilon())
24 {
25 if (!(isfinite(_a) && _a > 0))
26 throw GeographicErr("Equatorial radius is not positive");
27 if (!(isfinite(_f) && _f < 1))
28 throw GeographicErr("Polar semi-axis is not positive");
29 }
30
35
36 void Geocentric::IntForward(real lat, real lon, real h,
38 real M[dim2_]) const {
39 real sphi, cphi, slam, clam;
43 Z = (_e2m * n + h) * sphi;
44 X = (n + h) * cphi;
45 Y = X * slam;
46 X *= clam;
47 if (M)
48 Rotation(sphi, cphi, slam, clam, M);
49 }
50
51 void Geocentric::IntReverse(real X, real Y, real Z,
53 real M[dim2_]) const {
55 R = hypot(X, Y),
56 slam = R != 0 ? Y / R : 0,
57 clam = R != 0 ? X / R : 1;
58 h = hypot(R, Z);
59 real sphi, cphi;
60 if (h > _maxrad) {
61
62
63
64
65
66
67 R = hypot(X/2, Y/2);
68 slam = R != 0 ? (Y/2) / R : 0;
69 clam = R != 0 ? (X/2) / R : 1;
70 real H = hypot(Z/2, R);
71 sphi = (Z/2) / H;
72 cphi = R / H;
73 } else if (_e4a == 0) {
74
75
76
77 real H = hypot(h == 0 ? 1 : Z, R);
78 sphi = (h == 0 ? 1 : Z) / H;
79 cphi = R / H;
80 h -= _a;
81 } else {
82
83
87 r = (p + q - _e4a) / 6;
88 if (_f < 0) swap(p, q);
89 if ( !(_e4a * q == 0 && r <= 0) ) {
91
92
93 S = _e4a * p * q / 4,
95 r3 = r * r2,
96 disc = S * (2 * r3 + S);
98 if (disc >= 0) {
99 real T3 = S + r3;
100
101
102
103 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc);
104
105 real T = cbrt(T3);
106
107 u += T + (T != 0 ? r2 / T : 0);
108 } else {
109
110 real ang = atan2(sqrt(-disc), -(S + r3));
111
112
113 u += 2 * r * cos(ang / 3);
114 }
116 v = sqrt(Math::sq(u) + _e4a * q),
117
118
119 uv = u < 0 ? _e4a * q / (v - u) : u + v,
120
121 w = fmax(real(0), _e2a * (uv - q) / (2 * v)),
122
123
124 k = uv / (sqrt(uv + Math::sq(w)) + w),
125 k1 = _f >= 0 ? k : k - _e2,
126 k2 = _f >= 0 ? k + _e2 : k,
127 d = k1 * R / k2,
128 H = hypot(Z/k1, R/k2);
129 sphi = (Z/k1) / H;
130 cphi = (R/k2) / H;
131 h = (1 - _e2m/k1) * hypot(d, Z);
132 } else {
133
134
135
136
137
138
140 zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
141 xx = sqrt( _f < 0 ? _e4a - p : p ),
142 H = hypot(zz, xx);
143 sphi = zz / H;
144 cphi = xx / H;
145 if (Z < 0) sphi = -sphi;
146 h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
147 }
148 }
151 if (M)
152 Rotation(sphi, cphi, slam, clam, M);
153 }
154
155 void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
156 real M[dim2_]) {
157
158
159
160
161
162
163
164
165 M[0] = -slam; M[3] = clam; M[6] = 0;
166
167 M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
168
169 M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
170 }
171
172}
Header for GeographicLib::Geocentric class.
GeographicLib::Math::real real
static const Geocentric & WGS84()
Definition Geocentric.cpp:31
Exception handling for GeographicLib.
Mathematical functions needed by GeographicLib.
static void sincosd(T x, T &sinx, T &cosx)
static T atan2d(T y, T x)
Namespace for GeographicLib.
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)