GeographicLib: Cartesian3.cpp Source File

GeographicLib 2.6

Loading...
Searching...
No Matches
Cartesian3.cpp
Go to the documentation of this file.
1/**
2 * \file Cartesian3.cpp
3 * \brief Implementation for GeographicLib::Triaxial::Cartesian3 class
4 *
5 * Copyright (c) Charles Karney (2025) <karney@alum.mit.edu> and licensed
6 * under the MIT/X11 License. For more information, see
7 * https://geographiclib.sourceforge.io/
8 **********************************************************************/
9
10#include <GeographicLib/Triaxial/Cartesian3.hpp>
11
12namespace GeographicLib {
13 namespace Triaxial {
14
15 using namespace std;
16
17 Cartesian3::Cartesian3(const Ellipsoid3& t)
18 : _t(t)
19 , _axes{_t.a(), _t.b(), _t.c()}
20 , _axes2{Math::sq(_t.a()), Math::sq(_t.b()), Math::sq(_t.c())}
21 , _linecc2{(_t.a() - _t.c()) * (_t.a() + _t.c()),
22 (_t.b() - _t.c()) * (_t.b() + _t.c()), 0}
23 , _norm(0, 1)
24 , _uni(0, 1)
25 {}
26
27 Cartesian3::Cartesian3(real a, real b, real c)
28 : Cartesian3(Ellipsoid3(a, b, c))
29 {}
30
31 Cartesian3::Cartesian3(real b, real e2, real k2, real kp2)
32 : Cartesian3(Ellipsoid3(b, e2, k2, kp2))
33 {}
34
35 template<int n>
36 pair<Math::real, Math::real> Cartesian3::funp<n>::operator()(real p)
37 const {
38 real fp = 0, fv = -1, fcorr = 0;
39 for (int k = 0; k < 3; ++k) {
40 if (_r[k] == 0) continue;
41 real g = _r[k] / (p + _l[k]);
42 if constexpr (n == 2) g *= g;
43 real ga = round(g/_d) * _d, gb = g - ga;
44 fv = fv + ga; fcorr = fcorr + gb;
45 fp = fp - n * g / (p + _l[k]);
46 }
47 return pair<real, real>(fv + fcorr, fp);
48 }
49
50 Math::real Cartesian3::cartsolve(const function<pair<real, real>(real)>& f,
51 real p0, real pscale) {
52 // Solve
53 // f(p) = 0
54 // Initial guess is p0; pscale is a scale factor for p; scale factor for f
55 // = 1. This assumes that there's a single solution with p >= 0 and that
56 // for p > 0, f' < 0 and f'' > 0
57 const real eps = numeric_limits<real>::epsilon(),
58 tol = Math::sq(cbrt(eps)), tol2 = Math::sq(tol),
59 ptol = pscale * sqrt(eps);
60 real p = p0;
61 real od = -1;
62 for (int i = 0;
63 i < maxit_ ||
64 (throw_ && (throw GeographicLib::GeographicErr
65 ("Convergence failure Cartesian3::cartsolve"), false));
66 ++i) {
67 pair<real, real> fx = f(p);
68 real fv = fx.first, fp = fx.second;
69 // We're done if f(p) <= 0 on initial guess; this can happens when z = 0.
70 // However, since Newton converges from below, any negative f(p)
71 // indicates convergence.
72 if (!(fv > tol2)) break;
73 real d = -fv/fp; // d is positive
74 p = p + d;
75 // converged if fv <= 8*eps (after first iteration) or
76 // d <= max(eps, |p|) * tol and d <= od.
77 // N.B. d is always positive.
78 if ( (fv <= 8 * eps || d <= fmax(ptol, p) * tol) && d <= od )
79 // The condition d <= od means that this won't trip on the first
80 // iteration
81 break;
82 od = d;
83 }
84 return p;
85 }
86
87 Math::real Cartesian3::cubic(vec3 R2) const {
88 // Solve sum(R2[i]/(z + lineq2[i]), i,0,2) - 1 = 0 with lineq2[2] = 0.
89 // This has three real roots with just one satisifying q >= 0.
90 // Express as a cubic equation z^3 + a*z^2 + b*z + c = 0.
91 real c = - _linecc2[0]*_linecc2[1] * R2[2],
92 b = _linecc2[0]*_linecc2[1]
93 - (_linecc2[1] * R2[0] + _linecc2[0] * R2[1] +
94 (_linecc2[0] + _linecc2[1]) * R2[2]),
95 a = _linecc2[0] + _linecc2[1] - (R2[0] + R2[1] + R2[2]);
96 bool recip = b > 0;
97 if (recip) {
98 // If b positive there a cancellation in p = (3*b - a^2) / 3, so
99 // transform to a polynomial in 1/t. The resulting coefficients are
100 real ax = b/c, bx = a/c, cx = 1/c;
101 a = ax; b = bx; c = cx;
102 }
103 // Reduce cubic to w^3 + p*w + q = 0, where z = w - a/3.
104 // See https://dlmf.nist.gov/1.11#iii
105 real p = (3*b - Math::sq(a)) / 3,
106 q = (2*a*Math::sq(a) - 9*a*b + 27*c) / 27;
107
108 // Now switch to https://dlmf.nist.gov/4.43
109 // We have 3 real roots, so 4*p^3 + 27*q^2 <= 0
110 real A = sqrt(fmax(real(0), -4*p/3)),
111 alp = atan2(q, sqrt(fmax(real(0),
112 -(4*p*Math::sq(p)/27 + Math::sq(q)))))/3;
113 // alp is in [-pi/3, pi/3]
114 // z = A*sin(alp + 2*pi/3 * k) - a/3 for k = -1, 0, 1
115 // for the single positive solution we pick k = 1 which gives the
116 // algebraically largest result
117 real t = A/2 * (cos(alp) * sqrt(real(3)) - sin(alp)) - a/3;
118
119 return recip ? 1/t : t;
120 }
121
122 void Cartesian3::carttoellip(vec3 R, Angle& bet, Angle& omg, real& H) const {
123 // tol2 = eps^(4/3)
124 real tol2 = Math::sq(Math::sq(cbrt(numeric_limits<real>::epsilon())));
125 vec3 R2 = {Math::sq(R[0]), Math::sq(R[1]), Math::sq(R[2])};
126 real qmax = R2[0] + R2[1] + R2[2],
127 qmin = fmax(fmax(R2[2], R2[1] + R2[2] - _linecc2[1]),
128 R2[0] + R2[1] + R2[2] - _linecc2[0]),
129 q = qmin;
130 do { // Executed once (provides the ability to break)
131 const funp<1> f(R2, _linecc2);
132 pair<real, real> fx = f(q);
133 if (!( fx.first > tol2 ))
134 break; // negative means converged
135 q = fmax(qmin, fmin(qmax, cubic(R2)));
136 fx = f(q);
137 if (!( fabs(fx.first) > tol2 ))
138 break; // test abs(fv) here
139 q = fmax(qmin, q - fx.first/fx.second);
140 q = cartsolve(f, q, Math::sq(b()));
141 } while (false);
142 vec3 axes = {sqrt(_linecc2[0] + q), sqrt(_linecc2[1] + q), sqrt(q)};
143 _t.cart2toellipint(R, bet, omg, axes);
144 H = axes[2] - c();
145 }
146
147 void Cartesian3::elliptocart(Angle bet, Angle omg, real H, vec3& R) const {
148 vec3 ax;
149 real shift = H * (2*c() + H);
150 for (int k = 0; k < 2; ++k)
151 ax[k] = sqrt(_axes2[k] + shift);
152 ax[2] = c() + H;
153 real tx = hypot(_t.k() * bet.c(), _t.kp()),
154 tz = hypot(_t.k(), _t.kp() * omg.s());
155 R = { ax[0] * omg.c() * tx,
156 ax[1] * bet.c() * omg.s(),
157 ax[2] * bet.s() * tz };
158 }
159
160 template<int n>
161 void Cartesian3::cart2togeneric(vec3 R, ang& phi, ang& lam, bool alt) const {
162 static_assert(n >= 0 && n <= 2, "Bad coordinate conversion");
163 if constexpr (n == 2) {
164 R[0] /= _axes2[0];
165 R[1] /= _axes2[1];
166 R[2] /= _axes2[2];
167 } else if constexpr (n == 1) {
168 R[0] /= _axes[0];
169 R[1] /= _axes[1];
170 R[2] /= _axes[2];
171 } // else n == 0, R is unchanged
172 roty(R, alt ? 1 : 0);
173 // nr = [-R[2], R[1], R[0]]
174 phi = ang(R[2], hypot(R[0], R[1]));
175 lam = ang(R[1], R[0]); // ang{0, 0} -> 0
176 }
177
178 template<int n>
179 void Cartesian3::generictocart2(ang phi, ang lam, vec3& R, bool alt) const {
180 static_assert(n >= 0 && n <= 2, "Bad coordinate conversion");
181 R = {phi.c() * lam.c(), phi.c() * lam.s(), phi.s()};
182 roty(R, alt ? -1 : 0);
183 if constexpr (n == 2) {
184 R[0] *= _axes2[0];
185 R[1] *= _axes2[1];
186 R[2] *= _axes2[2];
187 } else if constexpr (n == 1) {
188 R[0] *= _axes[0];
189 R[1] *= _axes[1];
190 R[2] *= _axes[2];
191 } // else n == 0, R is unchanged
192 if constexpr (n != 1) {
193 real d = Math::hypot3(R[0] / _axes[0], R[1] / _axes[1], R[2] / _axes[2]);
194 R[0] /= d; R[1] /= d; R[2] /= d;
195 } // else n == 1, d = 1 and R is already on the surface of the ellipsoid
196 }
197
198 template<int n>
199 Angle Cartesian3::meridianplane(ang lam, bool alt) const {
200 if constexpr (n == 2)
201 return lam.modang(_axes2[alt ? 2 : 0]/_axes2[1]);
202 else if constexpr (n == 1)
203 return lam.modang(_axes[alt ? 2 : 0]/_axes[1]);
204 else { // n == 0
205 (void) alt; // Visual Studio 15 complains about used alt
206 return lam;
207 }
208 }
209
210 void Cartesian3::cardinaldir(vec3 R, ang merid, vec3& N, vec3& E,
211 bool alt) const {
212 roty(R, alt ? 1 : 0);
213 int i0 = alt ? 2 : 0, i1 = 1, i2 = alt ? 0 : 2;
214 vec3 up = {R[0] / _axes2[i0], R[1] / _axes2[i1],
215 R[2] / _axes2[i2]};
216 Ellipsoid3::normvec(up);
217 N = { -R[0]*R[2] / _axes2[i2], -R[1]*R[2] / _axes2[i2],
218 Math::sq(R[0]) / _axes2[i0] + Math::sq(R[1]) / _axes2[i1]};
219 if (R[0] == 0 && R[1] == 0) {
220 real s = copysign(real(1), -R[2]);
221 N = {s*merid.c(), s*merid.s(), 0};
222 } else
223 Ellipsoid3::normvec(N);
224 // E = N x up
225 E = { N[1]*up[2] - N[2]*up[1], N[2]*up[0] - N[0]*up[2],
226 N[0]*up[1] - N[1]*up[0]};
227 roty(E, alt ? -1 : 0);
228 roty(N, alt ? -1 : 0);
229 }
230
231 template<int n>
232 void Cartesian3::cart2togeneric(vec3 R, vec3 V,
233 ang& phi, ang& lam, ang& zet,
234 bool alt) const {
235 cart2togeneric<n>(R, phi, lam, alt);
236 vec3 N, E;
237 cardinaldir(R, meridianplane<n>(lam, alt), N, E, alt);
238 zet = ang(V[0]*E[0] + V[1]*E[1] + V[2]*E[2],
239 V[0]*N[0] + V[1]*N[1] + V[2]*N[2]);
240 }
241
242 template<int n>
243 void Cartesian3::generictocart2(ang phi, ang lam, ang zet,
244 vec3& R, vec3&V, bool alt) const {
245 generictocart2<n>(phi, lam, R, alt);
246 vec3 N, E;
247 cardinaldir(R, meridianplane<n>(lam, alt), N, E, alt);
248 V = {zet.c() * N[0] + zet.s() * E[0],
249 zet.c() * N[1] + zet.s() * E[1],
250 zet.c() * N[2] + zet.s() * E[2]};
251 }
252
253 void Cartesian3::cart2toany(vec3 R,
254 coord coordout, Angle& lat, Angle& lon) const {
255 bool alt = coordout > ELLIPSOIDAL;
256 switch (coordout) {
257 case GEODETIC:
258 case GEODETIC_X:
259 cart2togeneric<2>(R, lat, lon, alt); break;
260 case PARAMETRIC:
261 case PARAMETRIC_X:
262 cart2togeneric<1>(R, lat, lon, alt); break;
263 case GEOCENTRIC:
264 case GEOCENTRIC_X:
265 cart2togeneric<0>(R, lat, lon, alt); break;
266 case ELLIPSOIDAL:
267 _t.cart2toellip(R, lat, lon); break;
268 default:
269 throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordout));
270 }
271 }
272
273 void Cartesian3::anytocart2(coord coordin, Angle lat, Angle lon,
274 vec3& R) const {
275 bool alt = coordin > ELLIPSOIDAL;
276 switch (coordin) {
277 case GEODETIC:
278 case GEODETIC_X:
279 generictocart2<2>(lat, lon, R, alt); break;
280 case PARAMETRIC:
281 case PARAMETRIC_X:
282 generictocart2<1>(lat, lon, R, alt); break;
283 case GEOCENTRIC:
284 case GEOCENTRIC_X:
285 generictocart2<0>(lat, lon, R, alt); break;
286 case ELLIPSOIDAL:
287 _t.elliptocart2(lat, lon, R); break;
288 default:
289 throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordin));
290 }
291 }
292
293 void Cartesian3::anytoany(coord coordin, Angle lat1, Angle lon1,
294 coord coordout, Angle& lat2, Angle& lon2) const {
295 vec3 R;
296 anytocart2(coordin, lat1, lon1, R);
297 cart2toany(R, coordout, lat2, lon2);
298 }
299
300 void Cartesian3::cart2toany(vec3 R, vec3 V, coord coordout,
301 Angle& lat, Angle& lon, Angle& azi) const {
302 bool alt = coordout > ELLIPSOIDAL;
303 switch (coordout) {
304 case GEODETIC:
305 case GEODETIC_X:
306 cart2togeneric<2>(R, V, lat, lon, azi, alt); break;
307 case PARAMETRIC:
308 case PARAMETRIC_X:
309 cart2togeneric<1>(R, V, lat, lon, azi, alt); break;
310 case GEOCENTRIC:
311 case GEOCENTRIC_X:
312 cart2togeneric<0>(R, V, lat, lon, azi, alt); break;
313 case ELLIPSOIDAL:
314 _t.cart2toellip(R, V, lat, lon, azi); break;
315 default:
316 throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordout));
317 }
318 }
319
320 void Cartesian3::anytocart2(coord coordin, Angle lat, Angle lon, Angle azi,
321 vec3& R, vec3& V) const {
322 bool alt = coordin > ELLIPSOIDAL;
323 switch (coordin) {
324 case GEODETIC:
325 case GEODETIC_X:
326 generictocart2<2>(lat, lon, azi, R, V, alt); break;
327 case PARAMETRIC:
328 case PARAMETRIC_X:
329 generictocart2<1>(lat, lon, azi, R, V, alt); break;
330 case GEOCENTRIC:
331 case GEOCENTRIC_X:
332 generictocart2<0>(lat, lon, azi, R, V, alt); break;
333 case ELLIPSOIDAL:
334 _t.elliptocart2(lat, lon, azi, R, V); break;
335 default:
336 throw GeographicErr("Bad Cartesian3::coord value " + to_string(coordin));
337 }
338 }
339
340 void Cartesian3::carttoany(vec3 R, coord coordout,
341 Angle& lat, Angle& lon, real& h) const {
342 switch (coordout) {
343 case ELLIPSOIDAL: carttoellip(R, lat, lon, h); break;
344 default:
345 vec3 R2;
346 carttocart2(R, R2, h);
347 cart2toany(R2, coordout, lat, lon);
348 }
349 }
350
351 void Cartesian3::anytocart(coord coordin, Angle lat, Angle lon, real h,
352 vec3& R) const {
353 switch (coordin) {
354 case ELLIPSOIDAL: elliptocart(lat, lon, h, R); break;
355 default:
356 vec3 R2;
357 anytocart2(coordin, lat, lon, R2);
358 cart2tocart(R2, h, R);
359 }
360 }
361
362 void Cartesian3::cart2tocart(vec3 R2, real h, vec3& R) const {
363 vec3 Rn = {R2[0] / _axes2[0], R2[1] / _axes2[1], R2[2] / _axes2[2]};
364 real d = h / Math::hypot3(Rn[0], Rn[1], Rn[2]);
365 R = R2;
366 for (int k = 0; k < 3; ++k)
367 R[k] += Rn[k] * d;
368 }
369
370 void Cartesian3::carttocart2(vec3 R, vec3& R2, real& h) const {
371 const real eps = numeric_limits<real>::epsilon(), ztol = b() * eps/8;
372 for (int k = 0; k < 3; ++k)
373 if (fabs(R[k]) <= ztol) R[k] = copysign(real(0), R[k]);
374 vec3 s = {R[0] * _axes[0], R[1] * _axes[1], R[2] * _axes[2]};
375 real p = fmax(fmax(fabs(s[2]), hypot(s[1], s[2]) - _linecc2[1]),
376 Math::hypot3(s[0], s[1], s[2]) - _linecc2[0]);
377 const funp<2> f(s, _linecc2);
378 p = cartsolve(f, p, Math::sq(b()));
379 R2 = R;
380 for (int k = 0; k < 3; ++k)
381 R2[k] *= _axes2[k] / (p + _linecc2[k]);
382 // Deal with case p == 0 (when R2[2] is indeterminate).
383 if (p == 0) {
384 if (_linecc2[0] == 0) // sphere
385 R2[0] = R[0];
386 if (_linecc2[1] == 0) // sphere or prolate
387 R2[1] = R[1];
388 R2[2] = _axes[2] * R[2] *
389 sqrt(1 - Math::sq(R2[0]/_axes[0]) + Math::sq(R2[1]/_axes[1]));
390 }
391 h = (p - _axes2[2]) * Math::hypot3(R2[0] / _axes2[0],
392 R2[1] / _axes2[1],
393 R2[2] / _axes2[2]);
394 }
395
396 } // namespace Triaxial
397} // namespace GeographicLib
Cartesian3.hpp
Header for GeographicLib::Triaxial::Cartesian3 class.
ang
GeographicLib::Angle ang
Definition Geod3Solve.cpp:26
real
GeographicLib::Math::real real
Definition Geod3Solve.cpp:25
GeographicLib::AngleT::c
T c() const
Definition Angle.hpp:153
GeographicLib::AngleT::s
T s() const
Definition Angle.hpp:149
GeographicLib::AngleT::modang
AngleT modang(T m) const
Definition Angle.hpp:711
GeographicLib::GeographicErr
Exception handling for GeographicLib.
Definition Constants.hpp:344
GeographicLib::Math
Mathematical functions needed by GeographicLib.
Definition Math.hpp:82
GeographicLib::Math::sq
static T sq(T x)
Definition Math.hpp:209
GeographicLib::Math::hypot3
static T hypot3(T x, T y, T z)
Definition Math.cpp:285
GeographicLib::Math::real
double real
Definition Math.hpp:115
GeographicLib::Triaxial::Cartesian3::anytoany
void anytoany(coord coordin, Angle lat1, Angle lon1, coord coordout, Angle &lat2, Angle &lon2) const
Definition Cartesian3.cpp:293
GeographicLib::Triaxial::Cartesian3::Cartesian3
Cartesian3(const Ellipsoid3 &t)
Definition Cartesian3.cpp:17
GeographicLib::Triaxial::Cartesian3::cart2tocart
void cart2tocart(vec3 R2, real h, vec3 &R) const
Definition Cartesian3.cpp:362
GeographicLib::Triaxial::Cartesian3::anytocart2
void anytocart2(coord coordin, Angle lat, Angle lon, vec3 &R) const
Definition Cartesian3.cpp:273
GeographicLib::Triaxial::Cartesian3::vec3
Ellipsoid3::vec3 vec3
Definition Cartesian3.hpp:100
GeographicLib::Triaxial::Cartesian3::coord
coord
Definition Cartesian3.hpp:185
GeographicLib::Triaxial::Cartesian3::ELLIPSOIDAL
@ ELLIPSOIDAL
Definition Cartesian3.hpp:205
GeographicLib::Triaxial::Cartesian3::GEODETIC_X
@ GEODETIC_X
Definition Cartesian3.hpp:210
GeographicLib::Triaxial::Cartesian3::GEOCENTRIC
@ GEOCENTRIC
Definition Cartesian3.hpp:200
GeographicLib::Triaxial::Cartesian3::GEODETIC
@ GEODETIC
Definition Cartesian3.hpp:190
GeographicLib::Triaxial::Cartesian3::PARAMETRIC_X
@ PARAMETRIC_X
Definition Cartesian3.hpp:215
GeographicLib::Triaxial::Cartesian3::PARAMETRIC
@ PARAMETRIC
Definition Cartesian3.hpp:195
GeographicLib::Triaxial::Cartesian3::GEOCENTRIC_X
@ GEOCENTRIC_X
Definition Cartesian3.hpp:220
GeographicLib::Triaxial::Cartesian3::carttoany
void carttoany(vec3 R, coord coordout, Angle &lat, Angle &lon, real &h) const
Definition Cartesian3.cpp:340
GeographicLib::Triaxial::Cartesian3::carttocart2
void carttocart2(vec3 R, vec3 &R2, real &h) const
Definition Cartesian3.cpp:370
GeographicLib::Triaxial::Cartesian3::t
const Ellipsoid3 & t() const
Definition Cartesian3.hpp:543
GeographicLib::Triaxial::Cartesian3::anytocart
void anytocart(coord coordin, Angle lat, Angle lon, real h, vec3 &R) const
Definition Cartesian3.cpp:351
GeographicLib::Triaxial::Cartesian3::cart2toany
void cart2toany(vec3 R, coord coordout, Angle &lat, Angle &lon) const
Definition Cartesian3.cpp:253
GeographicLib::Triaxial::Ellipsoid3
A triaxial ellipsoid.
Definition Ellipsoid3.hpp:82
GeographicLib::Triaxial
Namespace for operations on triaxial ellipsoids.
Definition Cartesian3.cpp:13
GeographicLib
Namespace for GeographicLib.
Definition Accumulator.cpp:12
GeographicLib::Angle
AngleT< Math::real > Angle
Definition Angle.hpp:760
std
Definition NearestNeighbor.hpp:806