GeographicLib: Geodesic3.cpp Source File

GeographicLib 2.6

Loading...
Searching...
No Matches
Geodesic3.cpp
Go to the documentation of this file.
1/**
2 * \file Geodesic3.cpp
3 * \brief Implementation for GeographicLib::Triaxial::Geodesic3 class
4 *
5 * Copyright (c) Charles Karney (2024-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 <iostream>
11#include <iomanip>
12#include <GeographicLib/Triaxial/Geodesic3.hpp>
13
14namespace GeographicLib {
15 namespace Triaxial {
16
17 using namespace std;
18
19 Geodesic3::Geodesic3(const Ellipsoid3& t)
20 : _t(t)
21 , _umbalt(false)
22 , _ellipthresh(1/real(8))
23 {}
24
25 Geodesic3::Geodesic3(real a, real b, real c)
26 : Geodesic3(Ellipsoid3(a, b, c))
27 {}
28
29 Geodesic3::Geodesic3(real b, real e2, real k2, real kp2)
30 : Geodesic3(Ellipsoid3(b, e2, k2, kp2))
31 {}
32
33 GeodesicLine3
34 Geodesic3::Inverse(Angle bet1, Angle omg1, Angle bet2, Angle omg2,
35 real& s12, Angle& alp1, Angle& alp2) const {
36 using TL = GeodesicLine3;
37 string msg;
38 bet1.round();
39 omg1.round();
40 bet2.round();
41 omg2.round();
42
43 if (!_umbline)
44 // Initialize _umbline
45 _umbline = make_shared<GeodesicLine3>(GeodesicLine3(*this));
46
47 // In triaxial + oblate cases, [bet, omg] are initially put into [-90,90] x
48 // [-180,180]. For prolate case, maybe we instead put [bet, omg] into
49 // [-180,180] x [0,180].
50 // What about rotating coordinates to make
51 // oblate: omg1 = 0
52 // prolate: bet1 = -90
53 // this eliminates many of the special cases
54
55 ang bet10(bet1), omg10(omg1);
56 // If prolate put omg in [ 0, 180], bet in [-180, 180]
57 // else put bet in [-90, 90], omg in [-180, 180]
58 bool flip1 = t().AngNorm(bet1, omg1, prolate()),
59 flip2 = t().AngNorm(bet2, omg2, prolate());
60 bool swap12;
61 {
62 // For prolate, swap based on omg, switch 1 & 2 because poles are at
63 // 0/180, instead of +/-90.
64 ang tmp1(prolate() ? omg2 : bet1), tmp2(prolate() ? omg1 : bet2);
65 tmp1.setquadrant(0U); tmp2.setquadrant(0U);
66 ang tmp12 = tmp2 - tmp1; // |bet2| - |bet1|
67 swap12 = tmp12.s() > 0; // is |bet2| > |bet1|
68 if (!biaxial() && tmp12.s() == 0) {
69 // don't need to do this if biaxial
70 tmp1 = omg1; tmp2 = omg2;
71 tmp1.setquadrant(0U); tmp2.setquadrant(0U);
72 tmp12 = tmp2 - tmp1;
73 swap12 = tmp12.s() < 0; // is |omg2| < |omg1|
74 }
75 // N.B. No swapping if bet1 = +0 and bet2 = -0.
76 }
77 if (swap12) {
78 swap(bet1, bet2);
79 swap(omg1, omg2);
80 }
81 if (oblate()) {
82 // Rotate, subtracting omg1 from omg[12], so omg1 = 0
83 omg2 -= omg1;
84 omg2 = omg2.base();
85 omg1 = ang::cardinal(0);
86 } else if (prolate()) {
87 // Rotate, subtracting bet1 + 90 from bet[12], so bet1 = -90
88 bet2 -= bet1 + ang::cardinal(1);
89 bet2 = bet2.base();
90 bet1 = ang::cardinal(-1);
91 }
92
93 bool swapomg = swapomg_ &&
94 // Don't want the swap to make a new umbilical point
95 !(bet1.c() == 0 && omg2.s() == 0) &&
96 fabs(omg2.s()) < fabs(omg1.s());
97 if (swapomg) swap(omg1, omg2);
98 // Now |bet1| >= |bet2|
99 bool flipz = bet1.s() > 0;
100 if (flipz) { // Not needed for prolate, bet1 already -90
101 bet1.reflect(true);
102 bet2.reflect(true);
103 }
104 // Now bet1 <= 0
105 bool flipy = prolate() ? signbit(bet2.c()) :
106 signbit(omg1.s()) || (omg1.s() == 0 && signbit(omg2.s()));
107 if (flipy) {
108 if (prolate())
109 bet2.reflect(false, true);
110 else {
111 omg1.reflect(true);
112 omg2.reflect(true);
113 }
114 }
115 // For oblate omg2 >= 0, for prolate bet2 in [-90, 90]
116 bool flipx = signbit(omg1.c());
117 if (flipx) {
118 omg1.reflect(false, true);
119 omg2.reflect(false, true);
120 }
121 bool flipomg = bet2.c() == 0 && signbit(omg2.s());
122 // Eliminate coordinate ambiguity bet2 = +/90 and omg2 < 0 for point 2
123 // Point 1 is already done with flipy. (Maybe skip for oblate?)
124 if (flipomg) omg2.reflect(true);
125
126 bool umb1 = bet1.c() == 0 && omg1.s() == 0,
127 umb2 = bet2.c() == 0 && omg2.s() == 0;
128
129 // Now bet1 <= 0, bet1 <= bet2 <= -bet1, 0 <= omg1 <= 90
130 //
131 // Oblate: omg1 = 0, omg2 >= 0; rotation results in meridional geodesics
132 // following triaxial middle ellipse (omg2 = 0/180); minor ellipse folded
133 // into middle ellipse.
134 //
135 // Prolate: bet1 = -90, bet2 in [-90,90], omg2 >= 0; rotation results in
136 // meridional geodesics following triaxial middle ellipse (bet2 = +/-90);
137 // major ellipse folded into middle ellipse.
138 //
139 // Distinguish two general categories of solution
140 //
141 // A The geodesic follows one of the principal ellipses (this means, of
142 // course, that both points need to be on the same principal ellipse) and
143 // the initial and final azimuth are easily determined. The subcases
144 // are:
145 //
146 // a Minor ellipse, omg = +/-90: the ellipse is followed regardless of
147 // the points; however we treat this using B.d (and the solution along
148 // the ellipse is found on the first iteration).
149 // Oblate: this becomes the middle ellipse case, A.c.
150 // Prolate: same as triaxial case.
151 //
152 // b Major ellipse, bet = 0: the ellipse is followed provided that both
153 // points are close enough "equatorial geodesics". From point 1 we
154 // follow an equatorial geodesic +/- 180deg in arc distance (= psi
155 // variable).
156 // Oblate: same as triaxial case, except conjugate point is trivially
157 // found (but don't use this)
158 // Prolate: treated by middle ellipse cases A.c.2 and A.c.3.
159 //
160 // 1 If point 2 is within the resulting span of longitudes, the
161 // geodesic is equatorial (compute with ArcPos0 with the thr
162 // variable).
163 //
164 // 2 Otherwise, do search with strategy B.
165 //
166 // c Middle ellipse, bet = +/-90 or omg = 0,180: the ellipse is followed
167 // provided that both points are close enough "meridional geodesics".
168 // The four umbilical points divide the middle ellipse into 4 segments.
169 // Oblate: same as triaxial case, but geodesic always follows ellipse.
170 // Prolate: this becomes the major ellipse case, A.b
171 //
172 // There are several subcases:
173 //
174 // 1 opposite umbilical points: multiple shortest geodesic, two of
175 // which follow the middle ellipse. But it's more useful to return
176 // the one going through bet = 0, omg = 90. Then all the others can
177 // be generated by multipling tan(alp1) and tan(alp2) by a constant.
178 // Oblate/Prolate: opposite poles with tht12 = 180
179 //
180 // 2 bet1 = -90, bet2 = -90: geodesic follows ellipse (compute with
181 // ArcPos0). Points may be adjacent umbilical points.
182 // Oblate: same pole
183 // Prolate: meridional geodesic not crossing a pole
184 //
185 // 3 bet1 = -90, bet2 = 90: geodesic may follow the ellipse if if they
186 // are close enough. See case A.b for strategy. Points may be
187 // adjacent umbilical points.
188 // Oblate: opposite poles with tht12 < 180
189 // Prolate: meridional geodesic crossing a pole + opposite meridians,
190 // non-meridional
191 //
192 // 4 |bet2| != 90: geodesic follows the ellipse. Compute with Hybrid.
193 // Oblate: meridional geodesic
194 // Prolate: same (omg2 = 0) or opposite (omg2 = 180) poles
195 //
196 // B The geodesic does not follow a principal ellipse
197 //
198 // a point 1 is an umbilical point, gam = 0, so we know alp2
199 // Oblate: remaining meridional cases (one end a pole)
200 // Prolate: remaining meridional cases (one end a pole)
201 //
202 // b bet1 = -90: do a search with alp1 bracket = [-90+eps, 90-eps].
203 // Oblate: already treated by B.a
204 // Prolate: this treate the general case
205 //
206 // c omg1 = 0: do a search with alp1 bracket = [eps, 180-eps], or
207 // [-180+eps, -eps], depending on the sign of omg2.
208 // Oblate: this treats the general case
209 // Prolate: already handled by B.a
210 //
211 // d general case, bracket search by two neighboring umbilical
212 // directions. This treats case A.a.
213 // Oblate/Prolate: already handled by B.b or B.c
214
215 // Set up variables for search, "h" variables are for hybridalt
216 real fa = Math::NaN(), fb = Math::NaN();
217 ang alpa, alpb;
218
219 // and for the final result
220 ang bet2a, omg2a;
221
222 // Much of the initial logic for the inverse solution uses umbilical
223 // geodesics; use the cached value for this.
224 TL::fline lf = _umbline->_f;
225 TL::fline::fics fic;
226 TL::fline::disttx d{Math::NaN(), Math::NaN(), 0};
227
228 real aa = k2() * Math::sq(bet2.c()), bb = kp2() * Math::sq(omg2.s());
229
230 if constexpr (debug_)
231 cout << "COORDS " << real(bet1) << " " << real(omg1) << " "
232 << real(bet2) << " " << real(omg2) << "\n";
233 // bet1.setn(0); omg1.setn(0); bet2.setn(0); omg2.setn(0);
234 // flag for progress
235 bool done = false, backside = false;
236 if (bet1.c() * omg1.s() == 0 && bet2.c() * omg2.s() == 0) {
237 // Case A.c, both points on middle ellipse
238 if (umb1 && umb2 && bet2.s() > 0 && omg2.c() < 0) {
239 // Case A.c.1, process opposite umbilical points
240 // For oblate/prolate this gives 0/90
241 alp1 = biaxial() ? ang(k(), kp(), 0, true) :
242 ang(exp(lf.deltashift()/2), 1);
243 fic = TL::fline::fics(lf, bet1, omg1, alp1);
244 bool betp = k2() < kp2();
245 // if (biaxial()) betp = !betp;
246 // betp = !betp;
247 d = lf.ArcPos0(fic, ang::cardinal(2), bet2a, omg2a, alp2, betp);
248 if constexpr (debug_) msg = "A.c opposite umbilics";
249 backside = signbit(bet2a.c());
250 done = true;
251 } else if (bet1.c() == 0 && bet2.c() == 0) {
252 // Case A.c.{2,3}, bet1 = -90, bet2 = +/-90
253 if (bet2.s() < 0) {
254 // Case A.c.2, bet1 = bet2 = -90
255 // If oblate, bet1 = -90, omg1 = 0, need alp1 = omg2 to follow omg2
256 // meridian.
257 alp1 = ang::cardinal(oblate() ? 2 : 1);
258 fic = TL::fline::fics(lf, bet1, omg1, alp1);
259 ang omg12 = omg2 - omg1;
260 if (omg12.s() == 0 && omg12.c() < 0) {
261 // adjacent E/W umbilical points
262 // Should be able to get ArcPos0 to return this?
263 d = oblate() ?
264 TL::fline::disttx{ (biaxial() ? 1 : -1 ) * Math::pi()/2,
265 -Math::pi()/2, 0 } :
266 prolate() ?
267 TL::fline::disttx{ Math::pi()/2, -Math::pi()/2, 0 } :
268 TL::fline::disttx{ -BigValue(), BigValue(), 0 };
269 if constexpr (debug_) msg = "A.c.2 adjacent EW umbilics";
270 alp2 = ang::cardinal(prolate() ? 1 : 0);
271 } else {
272 d = lf.ArcPos0(fic, omg12.base(), bet2a, omg2a, alp2, false);
273 if constexpr (debug_) msg = "A.c.2 bet1/2 = -90";
274 }
275 // not needed
276 // if (omg2a.s() < 0) alp2.reflect(true);
277 done = true;
278 } else {
279 // Case A.c.3, bet1 = -90, bet2 = 90
280 // need to see how far apart the points are
281 // If point 1 is at [-90, 0], direction is 0 else -90.
282 // For prolate use -90.
283 alp1 = ang::cardinal(omg1.s() == 0 && !prolate() ? 0 : -1);
284 fic = TL::fline::fics(lf, bet1, omg1, alp1);
285 // If point 1 is [-90, 0] and point 2 is [90, 0]
286 if (omg1.s() == 0 && omg2.s() == 0) {
287 // adjacent N/S umbilical points
288 // Should be able to get ArcPos0 to return this?
289 d = biaxial() ?
290 TL::fline::disttx{ Math::pi()/2, -Math::pi()/2, 0 } :
291 TL::fline::disttx{ BigValue(), -BigValue(), 0 };
292 alp2 = ang::cardinal(oblate() ? 0 : 1);
293 if constexpr (debug_) msg = "A.c.3 adjacent NS umbilics";
294 done = true;
295 } else {
296 // FIX ME for oblate
297 if (omg1.s() == 0)
298 omg2a = ang::cardinal(2);
299 else
300 // Compute conjugate point along the middle ellipse
301 d = lf.ArcPos0(fic, ang::cardinal(2), bet2a, omg2a, alp2);
302 // XXX FIX HERE for prolate case -90 -1 90 177
303 omg2a -= omg2;
304 if (omg2a.s() >= -numeric_limits<real>::epsilon()/2) {
305 // Includes all cases where point 1 = [-90, 0], omg1.s() == 0.
306 ang omg12 = omg2 + omg1;
307 // FIX ME for oblate
308 d = lf.ArcPos0(fic, omg12.base(), bet2a, omg2a, alp2, false);
309 if (!biaxial() && signbit(omg2a.s()))
310 alp2.reflect(true);
311 if constexpr (debug_) msg = "A.c.3 bet1/2 = -/+90 meridional";
312 done = true;
313 } else {
314 alpa = ang::cardinal(-1) + ang::eps();
315 fa = omg2a.radians0();
316 (void) lf.ArcPos0(fic, ang::cardinal(-2), bet2a, omg2a, alp2);
317 omg2a -= omg2;
318 alpb = -alpa;
319 fb = omg2a.radians0();
320 if constexpr (debug_)
321 msg = "A.c.3 general bet1/2 = -/+90, non-meridional";
322 done = false; // A marker
323 }
324 if constexpr (debug_)
325 cout << "ALP/F "
326 << real(alpa) << " " << fa << " "
327 << real(alpb) << " " << fb << "\n";
328 }
329 }
330 } else {
331 // Case A.c.4, other meridional cases, invoke Hybrid with the following
332 // value of alp1
333 // If oblate, bet1 = -90, omg1 = 0, need alp1 = omg2 to follow omg2
334 // meridian.
335 // If prolate, bet1 = -90, omg1 = 0, need alp1 = -bet2 to follow bet2
336 // meridian.
337 alp1 = oblate() ? omg2 :
338 (prolate() ? -bet2 :
339 ang::cardinal(bet1.c() == 0 ?
340 // TODO: CHECK omg2.c() < 1 test; CHANGE TO < 0
341 (omg2.c() < 0 ? 1 :
342 (omg1.s() == 0 && !prolate() ? 0 : -1)) :
343 (omg2.c() > 0 ? 0 : 2)));
344 fic = TL::fline::fics(lf, bet1, omg1, alp1);
345 d = prolate() ?
346 lf.ArcPos0(fic, (omg2-omg1).base(), bet2a, omg2a, alp2, false) :
347 lf.Hybrid(fic, bet2, bet2a, omg2a, alp2);
348 if (prolate() && signbit(bet2a.c()))
349 alp2.reflect(true,true);
350 if constexpr (debug_) msg = "A.c.4 other meridional";
351 backside = signbit(bet2a.c());
352 done = true;
353 }
354 } else if (bet1.s() == 0 && bet2.s() == 0) {
355 // Case A.b, both points on equator
356 ang omg12 = (omg2 - omg1).base();
357 int E = signbit(omg12.s()) ? -1 : 1; // Fix #1 for triaxial sphere
358 // set direction for probe as +/-90 based on sign of omg12
359 alp1 = ang::cardinal(E);
360 bet1.reflect(true);
361 lf = TL::fline(this->t(), gamma(bet1, omg1, alp1));
362 fic = TL::fline::fics(lf, bet1, omg1, alp1);
363 (void) lf.ArcPos0(fic, ang::cardinal(2), bet2a, omg2a, alp2);
364 omg2a -= omg2;
365 if (E * omg2a.s() >= -numeric_limits<real>::epsilon()/2) {
366 // geodesic follows the equator
367 d = lf.ArcPos0(fic, omg12.flipsign(E), bet2a, omg2a, alp2, false);
368 if constexpr (debug_) msg = "A.b.1 bet1/2 = 0 equatorial";
369 done = true;
370 } else {
371 // geodesic does not follow the equator
372 alpb = ang::cardinal(-1) - ang::eps();
373 alpa = -alpb;
374 (E > 0 ? fa : fb) = omg2a.radians0();
375 (void) lf.ArcPos0(fic, ang::cardinal(-2), bet2a, omg2a, alp2);
376 omg2a -= omg2;
377 (E > 0 ? fb : fa) = omg2a.radians0();
378 if constexpr (debug_) msg = "A.b.2 general bet1/2 = 0 non-equatorial";
379 done = false; // A marker
380 }
381 } else if (umb1) {
382 // Case B.a, umbilical point to general point
383 alp2 = ang(kp() * omg2.s(), k() * bet2.c());
384 // RETHINK THIS. If we know alp2, we can compute delta. This should be
385 // enough to find alp1.
386 fic = TL::fline::fics(lf, bet2, omg2, alp2);
387 // bool betp = k2() > kp2(); // This could be betb = !prolate();
388 bool betp = aa > bb;
389 real delta = (lf.transpolar() ? -1 : 1) * fic.delta;
390 alp1 = oblate() ?
391 // For oblate
392 // delta =
393 // atan2(bet1.s() * fabs(alp1.s()), bet0.c() * alp1.c())) - omg1
394 // Here omg1 = -90 (because of pi/2 shift in ext. vs int. omg)
395 // bet1.s() = -1, bet0.c() = 1, alp1.s() > 0, so
396 // alp1 = 90 - delta
397 ang::cardinal(1) - ang::radians(delta) :
398 (prolate() ?
399 // For prolate
400 // delta =
401 // bet1 - atan2(omg1.s() * fabs(alp1.c()), omg0.c() * alp1.s())
402 // Here bet1 = -90, omg1.s() = -1, alp1.c() > 0, omg0.c() = 1
403 // delta = bet1 + atan2(alp1.c(), alp1.s()) = -90 + 90 - alp1
404 // alp1 = -delta
405 -ang::radians(delta) :
406 // For triaxial case at an umbilic point
407 // delta = f.deltashift()/2 - log(fabs(alp1.t()));
408 // alp1.t() = exp(f.deltashift()/2 - delta)
409 ang(exp(lf.deltashift()/2 - delta), 1));
410 fic = TL::fline::fics(lf, bet1, omg1, alp1);
411 d = lf.ArcPos0(fic, (betp ? bet2 - bet1 : omg2 - omg1).base(),
412 bet2a, omg2a, alp2, betp);
413 if constexpr (debug_) msg = "B.a umbilic to general";
414 done = true;
415 } else if (bet1.c() == 0) {
416 // Case B.b, bet1 = -90 to general point
417 // subsumed by B.a for oblate
418 // general handling for prolate
419 if (!signbit(omg2.s())) {
420 alpa = ang::cardinal(-1) + ang::eps();
421 alpb = -alpa;
422 fa = -omg2.radians();
423 fb = (ang::cardinal(2) - omg2).radians0();
424 } else {
425 alpa = ang::cardinal(1) + ang::eps();
426 alpb = -alpa;
427 fa = (ang::cardinal(2) - omg2).radians0();
428 fb = -omg2.radians();
429 }
430 if constexpr (debug_) msg = "B.b general bet1 = -90";
431 done = false; // A marker
432 } else if (omg1.s() == 0) {
433 // Case B.c, omg1 = 0 to general point
434 // subsumed by B.a for prolate
435 // general handling of oblate
436 if (omg2.s() > 0) {
437 alpa = ang::eps();
438 alpb = ang::cardinal(2) - alpa;
439 fa = -omg2.radians0();
440 fb = (ang::cardinal(2)-omg2).radians0();
441 } else {
442 // alpb = -ang::eps();
443 // alpa = ang::cardinal(-2) - alpb;
444 alpa = ang(-numeric_limits<real>::epsilon()/(1<<20), -1, 0, true);
445 alpb = ang(-numeric_limits<real>::epsilon()/(1<<20), 1, 0, true);
446 fa = (ang::cardinal(2)-omg2).radians0();
447 fb = -omg2.radians0();
448 }
449 if constexpr (debug_) msg = "B.c general omg1 = 0";
450 done = false; // A marker
451 } else {
452 // Case B.d, general case
453 real f[4];
454 alpa = ang( kp() * fabs(omg1.s()), k() * fabs(bet1.c()));
455 alpb = alpa;
456
457 fic = TL::fline::fics(lf, bet1, omg1, alpb);
458 unsigned qb = 0U, qa = 3U; // qa = qb - 1 (mod 4)
459 if constexpr (debug_) msg = "B.d general";
460 for (; !done && qb <= 4U; ++qb, ++qa) {
461 if (qb) {
462 alpb.setquadrant(qb);
463 fic.setquadrant(lf, qb);
464 }
465 if (qb < 4U) {
466 f[qb] = lf.Hybrid0(fic, bet2, omg2);
467 if constexpr (debug_)
468 cout << "f[qb] " << qb << " " << f[qb] << "\n";
469 if (fabs(f[qb]) < 2*numeric_limits<real>::epsilon()) {
470 alp1 = alpb;
471 d = lf.Hybrid(fic, bet2, bet2a, omg2a, alp2);
472 if constexpr (debug_) msg = "B.d accidental umbilic";
473 backside = signbit(bet2a.c()); // qb == 1U || qb == 2U;
474 done = true;
475 break;
476 }
477 }
478 if (qb && (f[qa & 3U] < 0 && f[qb & 3U] > 0) &&
479 // Fix #2 for triaxial sphere
480 // Expect f[qb] - f[qa] <= pi. The following condition catches
481 // cases where f[qb] = pi and f[qa] = -pi. This can happen with e2
482 // == 0 and bet2 == - bet1. Here "4" is a standin for pi+eps.
483 f[qb & 3U] - f[qa & 3U] < 4) {
484 break;
485 }
486 }
487 if constexpr (debug_)
488 cout << "fDD " << done << " " << qa << " " << qb << " "
489 << f[qa & 3U] << " " << f[qb & 3U] << "\n";
490 if (!done) {
491 fa = f[qa & 3U]; fb = f[qb & 3U];
492 alpa.setquadrant(qa);
493 done = false; // A marker
494 }
495 }
496
497 int countn = 0, countb = 0;
498 if (!done) {
499 // Iterative search for the solution
500 if constexpr (debug_)
501 cout << "X " << done << " " << msg << "\n";
502 alp1 = findroot(
503 [this, &bet1, &omg1, &bet2, &omg2]
504 (const ang& alp) -> real
505 {
506 return HybridA(bet1, omg1, alp, bet2, omg2, true);
507 },
508 alpa, alpb,
509 fa, fb,
510 &countn, &countb);
511 if constexpr (debug_)
512 cout << "ALP1 " << real(alp1) << "\n";
513 lf = TL::fline(this->t(), gamma(bet1, omg1, alp1));
514 fic = TL::fline::fics(lf, bet1, omg1, alp1);
515 // Let aa = k2() * Math::sq(bet2.c()), bb = kp2() * Math::sq(omg2.s());
516 // Figure sin/cos alp2 and call lf.Hybrid with betp = |salp2| <
517 // |calp2|. For !transpolar
518 // gammax = aa * salp2^2 - bb * calp2^2
519 // salp2^2 = (bb + gammax) / (aa + bb)
520 // and use betp = salp2^2 < 1/2
521 // For transpolar
522 // gammax = - aa * salp2^2 + bb * calp2^2
523 // salp2^2 = (bb + gammax) / (aa + bb)
524 // and use betp = salp2^2 < 1/2
525 // For transpolar
526 // gammax = bb * calp2^2 - aa * salp2^2
527 // calp2^2 = (aa + gammax) / (aa + bb)
528 // and use betp = calp2^2 > 1/2
529 bool betp = true;
530 if (hybridalt_ && (swapomg_ || omg1.s() <= fabs(omg2.s())))
531 betp = (lf.transpolar() ?
532 2 * (aa + lf.gammax()) > (aa + bb) :
533 2 * (bb + lf.gammax()) < (aa + bb));
534 d = lf.Hybrid(fic, betp ? bet2 : omg2, bet2a, omg2a, alp2, betp);
535 // Don't set backside for !betp and bet2.c() == 0. Not sure why.
536 backside = (betp || bet2.c() != 0) && signbit(bet2a.c());
537 }
538
539 if (!biaxial() && backside) alp2.reflect(true, true);
540 alp2.round();
541
542 TL::gline lg(this->t(), lf.gm());
543 TL::gline::gics gic(lg, fic);
544 s12 = lg.dist(gic, d);
545
546 if constexpr (debug_)
547 cout << "FLIPS "
548 << flip1 << flip2 << swap12 << swapomg
549 << flipz << flipy << flipx << flipomg
550 << lf.transpolar() << "\n";
551 if constexpr (debug_)
552 cout << "A "
553 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
554 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
555 // Undo switches in reverse order flipomg flipx flipy flipz swapomg swap12
556 // flip2 flip1
557 if (flipomg) {
558 omg2.reflect(true);
559 alp2.reflect(true, true);
560 }
561
562 if constexpr (debug_)
563 cout << "B "
564 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
565 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
566 if (flipx) {
567 omg1.reflect(false, true);
568 omg2.reflect(false, true);
569 alp1.reflect(true);
570 alp2.reflect(true);
571 }
572
573 if constexpr (debug_)
574 cout << "C "
575 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
576 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
577 if (flipy) {
578 if (prolate()) {
579 bet2.reflect(false, true);
580 alp1.reflect(false, true);
581 alp2.reflect(false, true);
582 } else {
583 omg1.reflect(true);
584 // This was omg2.reflect(true, true); (by mistake?)
585 omg2.reflect(true);
586 alp1.reflect(true);
587 alp2.reflect(true);
588 }
589 }
590
591 if constexpr (debug_)
592 cout << "D "
593 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
594 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
595 if (flipz) {
596 bet1.reflect(true);
597 bet2.reflect(true);
598 alp1.reflect(false, true);
599 alp2.reflect(false, true);
600 }
601
602 if constexpr (debug_)
603 cout << "E "
604 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
605 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
606 if (swapomg) {
607 swap(omg1, omg2);
608 if (lf.transpolar()) {
609 real
610 calp1 = copysign(hypot(k()/kp()*bet1.c(), lf.nu()), alp1.c()),
611 calp2 = copysign(hypot(k()/kp()*bet2.c(), lf.nu()), alp2.c()),
612 salp1 = (lf.nu() < lf.nup() ?
613 (omg1.s() - lf.nu()) * (omg1.s() + lf.nu()) :
614 (lf.nup() - omg1.c()) * (lf.nup() + omg1.c())),
615 salp2 = (lf.nu() < lf.nup() ?
616 (omg2.s() - lf.nu()) * (omg2.s() + lf.nu()) :
617 (lf.nup() - omg2.c()) * (lf.nup() + omg2.c()));
618 salp1 = -copysign(signbit(salp1) ? 0 : sqrt(salp1), alp2.s());
619 salp2 = -copysign(signbit(salp2) ? 0 : sqrt(salp2), alp1.s());
620 alp1 = ang(salp1, calp1);
621 alp2 = ang(salp2, calp2);
622 } else {
623 real
624 salp1 = -copysign(hypot(kp()/k()*omg1.s(), lf.nu()), alp1.s()),
625 salp2 = -copysign(hypot(kp()/k()*omg2.s(), lf.nu()), alp2.s()),
626 calp1 = (lf.nu() < lf.nup() ?
627 (bet1.c() - lf.nu()) * (bet1.c() + lf.nu()) :
628 (lf.nup() - bet1.s()) * (lf.nup() + bet1.s())),
629 calp2 = (lf.nu() < lf.nup() ?
630 (bet2.c() - lf.nu()) * (bet2.c() + lf.nu()) :
631 (lf.nup() - bet2.s()) * (lf.nup() + bet2.s()));
632 calp1 = copysign(signbit(calp1) ? 0 : sqrt(calp1), alp1.c());
633 calp2 = copysign(signbit(calp2) ? 0 : sqrt(calp2), alp2.c());
634 alp1 = ang(salp1, calp1);
635 alp2 = ang(salp2, calp2);
636 }
637 }
638 if (swap12) {
639 swap(bet1, bet2);
640 swap(omg1, omg2);
641 swap(alp1, alp2);
642 swap(umb1, umb2);
643 // points not swapped if umb1 == true
644 alp1 += ang::cardinal(2);
645 if (umb2 && !biaxial())
646 alp2 += ang::cardinal((signbit(alp2.s()) ? -1 : 1) * bet2.s());
647 else
648 alp2 += ang::cardinal(2);
649 }
650
651 if constexpr (debug_)
652 cout << "F "
653 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
654 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
655 if (flip1) t().Flip(bet1, omg1, alp1);
656 if (flip2) t().Flip(bet2, omg2, alp2);
657 alp1.setn(); alp2.setn();
658 if constexpr (debug_)
659 cout << "G "
660 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
661 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
662
663 fic = TL::fline::fics(lf, bet10, omg10, alp1);
664 gic = TL::gline::gics(lg, fic);
665 gic.s13 = signbit(s12) ? 0 : s12;
666
667 // clang needs std::move instead of move.
668 return TL(std::move(lf), std::move(fic), std::move(lg), std::move(gic));
669 }
670
671 Math::real Geodesic3::HybridA(ang bet1, ang omg1, ang alp1,
672 ang bet2a, ang omg2b,
673 bool betp) const {
674 ang b1{bet1}, o1{omg1}, a1{alp1};
675 // a1 -= ang(1e-8);
676 gamblk gam = gamma(b1, o1, a1);
677 GeodesicLine3::fline l(this->t(), gam);
678 GeodesicLine3::fline::fics ic(l, b1, o1, a1);
679 real dang = l.Hybrid0(ic, bet2a, omg2b, betp);
680 return dang;
681 }
682
683 // Solve f(alp1) = 0 where alp1 is an azimuth and f(alp1) is the difference
684 // in lontitude on bet2 and the target longitude.
685 Angle Geodesic3::findroot(const function<real(const ang&)>& f,
686 ang xa, ang xb,
687 real fa, real fb,
688 int* countn, int* countb) {
689 // Implement root finding method of Chandrupatla (1997)
690 // https://doi.org/10.1016/s0965-9978(96)00051-8
691 // Here we follow Scherer (2013), Section 6.1.7.3
692 // https://doi.org/10.1007/978-3-319-00401-3
693
694 // Here the independent variable is an ang, but the computations on this
695 // variable essentially involve its conversion to radians. There's no need
696 // to worry about the angle wrapping around because (xb-xa).radians() is in
697 // (0,pi).
698
699 // require xa and xb to be normalized (the result is normalized)
700 // require fa and fb to have opposite signs
701
702 ang xm; // The return value
703 int cntn = 0, cntb = 0;
704 bool trip = false;
705 const bool debug = debug_;
706 // 25 iterations with line 498534 of testset.txt
707 // 43 -2 -43 141
708 // This is a near conjugate case m12 = 0.0003857
709 // 27 iterations with line 360115 of testpro.txt
710 // -75 29 75 -169
711 // Converge failures with line 40045 of testsph[bc]
712 // echo 80 -90 -80 90 | ./Geod3Solve -e 1 0 2 1 -i
713 // echo 80 -90 -80 90 | ./Geod3Solve -e 1 0 1 2 -i
714 //
715 // Offset for debugging output
716 real x0 = real(0);
717 // If fa and fb have the same signs, assume that the root is at one of the
718 // endpoints if corresponding f is small. Otherwise, it's an error.
719 if (fa * fb >= 0) {
720 if constexpr (debug)
721 cout << "FA FB " << fa/numeric_limits<real>::epsilon() << " "
722 << fb/numeric_limits<real>::epsilon() << " " << (fa == fb) << "\n";
723 if (fa == fb && fabs(fa) <= 512*numeric_limits<real>::epsilon())
724 // If both fa and fb have the same sign and are small (but not too
725 // small!), return mean of the endpoints. This is the case of
726 // antipodal points on a triaxial sphere, case A.c.3 general bet1/2 =
727 // -/+90, non-meridional. The mean angle corresponds to the "minor"
728 // ellipse where the call to Hybrid (to compute alp2) gives a well
729 // defined result.
730 return ang(xa.s() + xb.s(), xa.c() + xb.c());
731 else if (fmin(fabs(fa), fabs(fb)) > 2*numeric_limits<real>::epsilon())
732 // neither endpoint small enough
733 throw GeographicLib::GeographicErr
734 ("Bad inputs Geodesic3::findroot");
735 else
736 // return best endpoint
737 return fabs(fa) < fabs(fb) ? xa : xb;
738 }
739 // tp = 1 - t
740 for (real t = 1/real(2), tp = t, ab = 0, ft = 0, fc = 0;
741 cntn < maxit_ ||
742 (throw_ && (throw GeographicLib::GeographicErr
743 ("Convergence failure Geodesic3::findroot"), false));) {
744 ang xt = 2*t == 1 ?
745 ang(xa.s() + xb.s(), xa.c() + xb.c()) :
746 (t < tp ? xa - ang::radians(t * ab) :
747 xb + ang::radians(tp * ab)),
748 xc;
749 if (trip) {
750 xm = xt;
751 if constexpr (debug)
752 cout << "BREAKA\n";
753 break;
754 }
755 ++cntn;
756 ft = f(xt);
757 if constexpr (debug)
758 cout << "H " << cntn << " " << real(xt)-x0 << " " << ft << "\n";
759 if (!(fabs(ft) >= numeric_limits<real>::epsilon())) {
760 xm = xt;
761 if constexpr (debug)
762 cout << "BREAKB\n";
763 break;
764 }
765 if (signbit(ft) == signbit(fa)) {
766 xc = xa; xa = xt;
767 fc = fa; fa = ft;
768 } else {
769 xc = xb; xb = xa; xa = xt;
770 fc = fb; fb = fa; fa = ft;
771 }
772 xm = fabs(fb) < fabs(fa) ? xb : xa;
773 // ordering is b - a - c
774 ab = (xa-xb).radians0();
775 real
776 ca = (xc-xa).radians0(),
777 cb = ca+ab,
778 // Scherer has a fabs(cb). This should be fabs(ab).
779 tl = numeric_limits<real>::epsilon() / fabs(ab);
780 // Backward tests to deal with NaNs
781 if constexpr (debug)
782 cout << "R " << cntn << " " << ab << " " << cb << "\n";
783 trip = !(2 * tl < 1);
784 if (trip && debug)
785 cout << "TRIP " << ab << "\n";
786 // Increase the amount away from the boundary to make the next iteration.
787 // Otherwise we get two equal values of f near the boundary and a
788 // bisection is triggered.
789 tl = fmin(1/real(32), 16*tl);
790 real
791 xi = ab / cb,
792 xip = ca / cb, // 1 - xi
793 phi = (fa-fb) / (fc-fb),
794 phip = (fc-fa) / (fc-fb); // 1 - phi
795 if (!trip && Math::sq(phip) < xip && Math::sq(phi) < xi) {
796 t = fa/(fb-fa) * fc/(fb-fc) - ca/ab * fa/(fc-fa) * fb/(fc-fb);
797 if constexpr (debug)
798 cout << "J1 " << cntn << " " << t << " " << 1 - t << "\n";
799 // This equation matches the pseudocode in Scherer. His Eq (6.40)
800 // reads t = fa/(fb-fa) * fc/(fb-fc) + ca/cb * fc/(fc-fa) * fb/(fb-fa);
801 // this is wrong.
802 tp = fb/(fb-fa) * fc/(fc-fa) + cb/ab * fa/(fc-fa) * fb/(fc-fb);
803 t = fmax(tl, t);
804 tp = fmax(tl, tp);
805 // t = fmin(1 - tl, fmax(tl, t));
806 // tp = fmin(1 - tl, fmax(tl, tp));
807 } else {
808 t = tp = 1/real(2);
809 ++cntb;
810 if constexpr (debug)
811 cout << "J2 " << cntn << " " << t << " " << 1 - t << "\n";
812 }
813 }
814 if (countn) *countn += cntn;
815 if (countb) *countb += cntb;
816 return xm;
817 }
818
819 Geodesic3::gamblk::gamblk(const Geodesic3& tg,
820 ang bet, ang omg, ang alp) {
821 real a = tg.k() * bet.c() * alp.s(), b = tg.kp() * omg.s() * alp.c();
822 gamma = (a - b) * (a + b);
823 // This direct test case
824 // -30 -86 58.455576621187896848 -1.577754271270003
825 // fails badly with reverse direct if gamma is not set to zero here.
826 // Neighboring values of alp as double are
827 // 58.455576621187890, 58.455576621187895, 58.455576621187900
828 // 30 86 90 180
829 // dgam/dalp = 2*alp.c()*alp.s() * hypot(tg.k * bet.c(), tg.kp * omg.s())
830 real maxdiff = 0;
831 if (!(tg.k2() == 0 || tg.kp2() == 0)) {
832 // Force small gamma to zero for triaxial case
833 real
834 alpdiff = 2 * alp.c() * alp.s()
835 * (tg.k2() * Math::sq(bet.c())+tg.kp2() * Math::sq(omg.s())),
836 betdiff = -2 * bet.c() * bet.s() * tg.k2() * Math::sq(alp.s()),
837 omgdiff = -2 * omg.c() * omg.s() * tg.kp2() * Math::sq(alp.c());
838 maxdiff = fmax( fabs(alpdiff), fmax( fabs(betdiff), fabs(omgdiff) ) );
839 }
840 if (fabs(gamma) <= 2 * maxdiff * numeric_limits<real>::epsilon()) {
841 // Set gamma = 0 if a change of alp, bet, or omg by epsilon would include
842 // gamma = 0.
843 gamma = 0;
844 // If (_umbalt and not oblate) or prolate, set gamma = -0
845 if ((tg.umbalt() && tg.kp2() > 0) || tg.k2() == 0) gamma = -gamma;
846 }
847 transpolar = signbit(gamma);
848 gammax = fabs(gamma);
849 kx2 = !transpolar ? tg.k2() : tg.kp2();
850 kxp2 = transpolar ? tg.k2() : tg.kp2();
851 kx = !transpolar ? tg.k() : tg.kp();
852 kxp = transpolar ? tg.k() : tg.kp();
853 // gammap = sqrt(kx2 - gammax)
854 real gammap =
855 (!transpolar ?
856 hypot(kx * hypot(bet.s(), alp.c()*bet.c()),
857 kxp * omg.s()*alp.c()) :
858 hypot(kxp * bet.c()*alp.s(),
859 kx * hypot(omg.c(), alp.s()*omg.s())));
860 // for gam == 0, we have nu = 0, nup = 1
861 nu = sqrt(gammax) / kx;
862 nup = gammap / kx;
863 }
864
865 Geodesic3::gamblk::gamblk(const Geodesic3& tg, bool neg)
866 : transpolar(neg)
867 , gamma(transpolar ? -real(0) : real(0))
868 , nu(0)
869 , nup(1)
870 , gammax(0)
871 , kx2(!transpolar ? tg.k2() : tg.kp2())
872 , kxp2(transpolar ? tg.k2() : tg.kp2())
873 , kx(!transpolar ? tg.k() : tg.kp())
874 , kxp(transpolar ? tg.k() : tg.kp())
875 {}
876
877 Geodesic3::gamblk Geodesic3::gamma(ang bet, ang omg, ang alp) const {
878 return gamblk(*this, bet, omg, alp);
879 }
880
881 GeodesicLine3 Geodesic3::Line(Angle bet1, Angle omg1, Angle alp1) const {
882 return GeodesicLine3(*this, bet1, omg1, alp1);
883 }
884
885 GeodesicLine3 Geodesic3::Direct(Angle bet1, Angle omg1, Angle alp1, real s12,
886 Angle& bet2, Angle& omg2, Angle& alp2)
887 const {
888 GeodesicLine3 l(*this, bet1, omg1, alp1);
889 l.Position(s12, bet2, omg2, alp2);
890 return l;
891 }
892
893 GeodesicLine3 Geodesic3::Inverse(real bet1, real omg1, real bet2, real omg2,
894 real& s12, real& alp1, real& alp2) const {
895 ang alp1a, alp2a;
896 GeodesicLine3 l = Inverse(ang(bet1), ang(omg1), ang(bet2), ang(omg2),
897 s12, alp1a, alp2a);
898 alp1 = real(alp1a); alp2 = real(alp2a);
899 return l;
900 }
901
902 GeodesicLine3 Geodesic3::Line(real bet1, real omg1, real alp1) const {
903 return Line(ang(bet1), ang(omg1), ang(alp1));
904 }
905
906 GeodesicLine3 Geodesic3::Direct(real bet1, real omg1, real alp1, real s12,
907 real& bet2, real& omg2, real& alp2)
908 const {
909 ang bet2a, omg2a, alp2a;
910 GeodesicLine3 l = Direct(ang(bet1), ang(omg1), ang(alp1), s12,
911 bet2a, omg2a, alp2a);
912 bet2 = real(bet2a); omg2 = real(omg2a); alp2 = real(alp2a);
913 return l;
914 }
915
916 } // namespace Triaxial
917} // namespace GeographicLib
ang
GeographicLib::Angle ang
Definition Geod3Solve.cpp:26
real
GeographicLib::Math::real real
Definition Geod3Solve.cpp:25
Geodesic3.hpp
Header for GeographicLib::Triaxial::Geodesic3 class.
GeographicLib::AngleT< Math::real >::eps
static AngleT eps()
GeographicLib::AngleT::base
AngleT base() const
Definition Angle.hpp:642
GeographicLib::AngleT::c
T c() const
Definition Angle.hpp:153
GeographicLib::AngleT< Math::real >::cardinal
static AngleT cardinal(Math::real q)
GeographicLib::AngleT::flipsign
AngleT flipsign(T mult) const
Definition Angle.hpp:705
GeographicLib::AngleT::s
T s() const
Definition Angle.hpp:149
GeographicLib::AngleT::round
AngleT & round()
Definition Angle.hpp:636
GeographicLib::AngleT::radians
static AngleT radians(T rad)
Definition Angle.hpp:534
GeographicLib::AngleT::radians0
T radians0() const
Definition Angle.hpp:549
GeographicLib::AngleT< Math::real >::radians
Math::real radians() const
GeographicLib::AngleT::setn
AngleT & setn(T n=0)
Definition Angle.hpp:662
GeographicLib::AngleT::setquadrant
AngleT & setquadrant(unsigned q)
Definition Angle.hpp:682
GeographicLib::AngleT::reflect
AngleT & reflect(bool flips, bool flipc=false, bool swapp=false)
Definition Angle.hpp:696
GeographicLib::Math::sq
static T sq(T x)
Definition Math.hpp:209
GeographicLib::Math::pi
static T pi()
Definition Math.hpp:187
GeographicLib::Math::NaN
static T NaN()
Definition Math.cpp:301
GeographicLib::Math::real
double real
Definition Math.hpp:115
GeographicLib::Triaxial::Ellipsoid3
A triaxial ellipsoid.
Definition Ellipsoid3.hpp:82
GeographicLib::Triaxial::Ellipsoid3::AngNorm
static bool AngNorm(Angle &bet, Angle &omg, Angle &alp, bool alt=false)
Definition Ellipsoid3.hpp:247
GeographicLib::Triaxial::Geodesic3
The solution of the geodesic problem for a triaxial ellipsoid.
Definition Geodesic3.hpp:65
GeographicLib::Triaxial::Geodesic3::Inverse
GeodesicLine3 Inverse(Angle bet1, Angle omg1, Angle bet2, Angle omg2, real &s12, Angle &alp1, Angle &alp2) const
Definition Geodesic3.cpp:34
GeographicLib::Triaxial::Geodesic3::Geodesic3
Geodesic3(const Ellipsoid3 &t=Ellipsoid3{})
Definition Geodesic3.cpp:19
GeographicLib::Triaxial::Geodesic3::Direct
GeodesicLine3 Direct(Angle bet1, Angle omg1, Angle alp1, real s12, Angle &bet2, Angle &omg2, Angle &alp2) const
Definition Geodesic3.cpp:885
GeographicLib::Triaxial::Geodesic3::Line
GeodesicLine3 Line(Angle bet1, Angle omg1, Angle alp1) const
Definition Geodesic3.cpp:881
GeographicLib::Triaxial::Geodesic3::t
const Ellipsoid3 & t() const
Definition Geodesic3.hpp:187
GeographicLib::Triaxial::GeodesicLine3
The direct geodesic problem for a triaxial ellipsoid.
Definition GeodesicLine3.hpp:54
GeographicLib::Triaxial::GeodesicLine3::Position
void Position(real s12, Angle &bet2, Angle &omg2, Angle &alp2) const
Definition GeodesicLine3.cpp:71
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::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
Definition NearestNeighbor.hpp:819