GeographicLib 2.7

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
14 namespace 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 // Fix for
379 //
380 // echo 0 20 0 -50 | Geod3Solve -i -t 1.5 1 0.5
381 //
382 // With an ellipsoid this eccentric (a > 2*c?), the E and W conjugate
383 // points from a given point on the equator can be less than 180deg
384 // apart. So the assumption that relevant angle differences always lie
385 // in [-180,180], and so can be computed with radians0(), fails.
386 //
387 // In this case the E/W conjugate points are at omega = 104.3 and
388 // -40.6. So that fa, the differenc in omega, should be -205.7d =
389 // 104.3-360-(-50). This is reduced to [-180,180] by radians0() and
390 // becomes +154.3d. Fix by checking signs on fa and fb.
391 //
392 // findroot which calls HybridA also reduces the differences in omega
393 // to [-180,180]. But the first iteration in HybridA uses the
394 // bisecting alpha which will result (??? TO CHECK) is a omega
395 // difference "in range".
396 //
397 // If this turns out to be false, we'll need to rethink. Perhaps use
398 // omg1 + 180 instead of omg2 as the base value. This would
399 // necessitate a change in the signature for findroot. So hold off on
400 // this for now.
401 //
402 // The Octave routine triaxial.distance also failed for equatorial
403 // geodesics with this ellipsoid. But here the problem was accepting
404 // the equatorial geodesic becase m12 >= 0. In fact there may be two
405 // intervening conjugate points with an ellipsoid this eccentric.
406 // Failure case is t.distance([0,20],[0,-175]).
407 if (fa > 0) fa -= 2*Math::pi();
408 if (fb < 0) fb += 2*Math::pi();
409 if constexpr (debug_) msg = "A.b.2 general bet1/2 = 0 non-equatorial";
410 done = false; // A marker
411 }
412 } else if (umb1) {
413 // Case B.a, umbilical point to general point
414 alp2 = ang(kp() * omg2.s(), k() * bet2.c());
415 // RETHINK THIS. If we know alp2, we can compute delta. This should be
416 // enough to find alp1.
417 fic = TL::fline::fics(lf, bet2, omg2, alp2);
418 // bool betp = k2() > kp2(); // This could be betb = !prolate();
419 bool betp = aa > bb;
420 real delta = (lf.transpolar() ? -1 : 1) * fic.delta;
421 alp1 = oblate() ?
422 // For oblate
423 // delta =
424 // atan2(bet1.s() * fabs(alp1.s()), bet0.c() * alp1.c())) - omg1
425 // Here omg1 = -90 (because of pi/2 shift in ext. vs int. omg)
426 // bet1.s() = -1, bet0.c() = 1, alp1.s() > 0, so
427 // alp1 = 90 - delta
428 ang::cardinal(1) - ang::radians(delta) :
429 (prolate() ?
430 // For prolate
431 // delta =
432 // bet1 - atan2(omg1.s() * fabs(alp1.c()), omg0.c() * alp1.s())
433 // Here bet1 = -90, omg1.s() = -1, alp1.c() > 0, omg0.c() = 1
434 // delta = bet1 + atan2(alp1.c(), alp1.s()) = -90 + 90 - alp1
435 // alp1 = -delta
436 -ang::radians(delta) :
437 // For triaxial case at an umbilic point
438 // delta = f.deltashift()/2 - log(fabs(alp1.t()));
439 // alp1.t() = exp(f.deltashift()/2 - delta)
440 ang(exp(lf.deltashift()/2 - delta), 1));
441 fic = TL::fline::fics(lf, bet1, omg1, alp1);
442 d = lf.ArcPos0(fic, (betp ? bet2 - bet1 : omg2 - omg1).base(),
443 bet2a, omg2a, alp2, betp);
444 if constexpr (debug_) msg = "B.a umbilic to general";
445 done = true;
446 } else if (bet1.c() == 0) {
447 // Case B.b, bet1 = -90 to general point
448 // subsumed by B.a for oblate
449 // general handling for prolate
450 if (!signbit(omg2.s())) {
451 alpa = ang::cardinal(-1) + ang::eps();
452 alpb = -alpa;
453 fa = -omg2.radians();
454 fb = (ang::cardinal(2) - omg2).radians0();
455 } else {
456 alpa = ang::cardinal(1) + ang::eps();
457 alpb = -alpa;
458 fa = (ang::cardinal(2) - omg2).radians0();
459 fb = -omg2.radians();
460 }
461 if constexpr (debug_) msg = "B.b general bet1 = -90";
462 done = false; // A marker
463 } else if (omg1.s() == 0) {
464 // Case B.c, omg1 = 0 to general point
465 // subsumed by B.a for prolate
466 // general handling of oblate
467 if (omg2.s() > 0) {
468 alpa = ang::eps();
469 alpb = ang::cardinal(2) - alpa;
470 fa = -omg2.radians0();
471 fb = (ang::cardinal(2)-omg2).radians0();
472 } else {
473 // alpb = -ang::eps();
474 // alpa = ang::cardinal(-2) - alpb;
475 alpa = ang(-numeric_limits<real>::epsilon()/(1<<20), -1, 0, true);
476 alpb = ang(-numeric_limits<real>::epsilon()/(1<<20), 1, 0, true);
477 fa = (ang::cardinal(2)-omg2).radians0();
478 fb = -omg2.radians0();
479 }
480 if constexpr (debug_) msg = "B.c general omg1 = 0";
481 done = false; // A marker
482 } else {
483 // Case B.d, general case
484 real f[4];
485 alpa = ang( kp() * fabs(omg1.s()), k() * fabs(bet1.c()));
486 alpb = alpa;
487
488 fic = TL::fline::fics(lf, bet1, omg1, alpb);
489 unsigned qb = 0U, qa = 3U; // qa = qb - 1 (mod 4)
490 if constexpr (debug_) msg = "B.d general";
491 for (; !done && qb <= 4U; ++qb, ++qa) {
492 if (qb) {
493 alpb.setquadrant(qb);
494 fic.setquadrant(lf, qb);
495 }
496 if (qb < 4U) {
497 f[qb] = lf.Hybrid0(fic, bet2, omg2);
498 if constexpr (debug_)
499 cout << "f[qb] " << qb << " " << f[qb] << "\n";
500 if (fabs(f[qb]) < 2*numeric_limits<real>::epsilon()) {
501 alp1 = alpb;
502 d = lf.Hybrid(fic, bet2, bet2a, omg2a, alp2);
503 if constexpr (debug_) msg = "B.d accidental umbilic";
504 backside = signbit(bet2a.c()); // qb == 1U || qb == 2U;
505 done = true;
506 break;
507 }
508 }
509 if (qb && (f[qa & 3U] < 0 && f[qb & 3U] > 0) &&
510 // Fix #2 for triaxial sphere
511 // Expect f[qb] - f[qa] <= pi. The following condition catches
512 // cases where f[qb] = pi and f[qa] = -pi. This can happen with e2
513 // == 0 and bet2 == - bet1. Here "4" is a standin for pi+eps.
514 f[qb & 3U] - f[qa & 3U] < 4) {
515 break;
516 }
517 }
518 if constexpr (debug_)
519 cout << "fDD " << done << " " << qa << " " << qb << " "
520 << f[qa & 3U] << " " << f[qb & 3U] << "\n";
521 if (!done) {
522 fa = f[qa & 3U]; fb = f[qb & 3U];
523 alpa.setquadrant(qa);
524 done = false; // A marker
525 }
526 }
527
528 int countn = 0, countb = 0;
529 if (!done) {
530 // Iterative search for the solution
531 if constexpr (debug_)
532 cout << "X " << done << " " << msg << "\n";
533 alp1 = findroot(
534 [this, &bet1, &omg1, &bet2, &omg2]
535 (const ang& alp) -> real
536 {
537 return HybridA(bet1, omg1, alp, bet2, omg2, true);
538 },
539 alpa, alpb,
540 fa, fb,
541 &countn, &countb);
542 if constexpr (debug_)
543 cout << "ALP1 " << real(alp1) << "\n";
544 lf = TL::fline(this->t(), gamma(bet1, omg1, alp1));
545 fic = TL::fline::fics(lf, bet1, omg1, alp1);
546 // Let aa = k2() * Math::sq(bet2.c()), bb = kp2() * Math::sq(omg2.s());
547 // Figure sin/cos alp2 and call lf.Hybrid with betp = |salp2| <
548 // |calp2|. For !transpolar
549 // gammax = aa * salp2^2 - bb * calp2^2
550 // salp2^2 = (bb + gammax) / (aa + bb)
551 // and use betp = salp2^2 < 1/2
552 // For transpolar
553 // gammax = - aa * salp2^2 + bb * calp2^2
554 // salp2^2 = (bb + gammax) / (aa + bb)
555 // and use betp = salp2^2 < 1/2
556 // For transpolar
557 // gammax = bb * calp2^2 - aa * salp2^2
558 // calp2^2 = (aa + gammax) / (aa + bb)
559 // and use betp = calp2^2 > 1/2
560 bool betp = true;
561 if (hybridalt_ && (swapomg_ || omg1.s() <= fabs(omg2.s())))
562 betp = (lf.transpolar() ?
563 2 * (aa + lf.gammax()) > (aa + bb) :
564 2 * (bb + lf.gammax()) < (aa + bb));
565 d = lf.Hybrid(fic, betp ? bet2 : omg2, bet2a, omg2a, alp2, betp);
566 // Don't set backside for !betp and bet2.c() == 0. Not sure why.
567 backside = (betp || bet2.c() != 0) && signbit(bet2a.c());
568 }
569
570 if (!biaxial() && backside) alp2.reflect(true, true);
571 alp2.round();
572
573 TL::gline lg(this->t(), lf.gm());
574 TL::gline::gics gic(lg, fic);
575 s12 = lg.dist(gic, d);
576
577 if constexpr (debug_)
578 cout << "FLIPS "
579 << flip1 << flip2 << swap12 << swapomg
580 << flipz << flipy << flipx << flipomg
581 << lf.transpolar() << "\n";
582 if constexpr (debug_)
583 cout << "A "
584 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
585 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
586 // Undo switches in reverse order flipomg flipx flipy flipz swapomg swap12
587 // flip2 flip1
588 if (flipomg) {
589 omg2.reflect(true);
590 alp2.reflect(true, true);
591 }
592
593 if constexpr (debug_)
594 cout << "B "
595 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
596 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
597 if (flipx) {
598 omg1.reflect(false, true);
599 omg2.reflect(false, true);
600 alp1.reflect(true);
601 alp2.reflect(true);
602 }
603
604 if constexpr (debug_)
605 cout << "C "
606 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
607 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
608 if (flipy) {
609 if (prolate()) {
610 bet2.reflect(false, true);
611 alp1.reflect(false, true);
612 alp2.reflect(false, true);
613 } else {
614 omg1.reflect(true);
615 // This was omg2.reflect(true, true); (by mistake?)
616 omg2.reflect(true);
617 alp1.reflect(true);
618 alp2.reflect(true);
619 }
620 }
621
622 if constexpr (debug_)
623 cout << "D "
624 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
625 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
626 if (flipz) {
627 bet1.reflect(true);
628 bet2.reflect(true);
629 alp1.reflect(false, true);
630 alp2.reflect(false, true);
631 }
632
633 if constexpr (debug_)
634 cout << "E "
635 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
636 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
637 if (swapomg) {
638 swap(omg1, omg2);
639 if (lf.transpolar()) {
640 real
641 calp1 = copysign(hypot(k()/kp()*bet1.c(), lf.nu()), alp1.c()),
642 calp2 = copysign(hypot(k()/kp()*bet2.c(), lf.nu()), alp2.c()),
643 salp1 = (lf.nu() < lf.nup() ?
644 (omg1.s() - lf.nu()) * (omg1.s() + lf.nu()) :
645 (lf.nup() - omg1.c()) * (lf.nup() + omg1.c())),
646 salp2 = (lf.nu() < lf.nup() ?
647 (omg2.s() - lf.nu()) * (omg2.s() + lf.nu()) :
648 (lf.nup() - omg2.c()) * (lf.nup() + omg2.c()));
649 salp1 = -copysign(signbit(salp1) ? 0 : sqrt(salp1), alp2.s());
650 salp2 = -copysign(signbit(salp2) ? 0 : sqrt(salp2), alp1.s());
651 alp1 = ang(salp1, calp1);
652 alp2 = ang(salp2, calp2);
653 } else {
654 real
655 salp1 = -copysign(hypot(kp()/k()*omg1.s(), lf.nu()), alp1.s()),
656 salp2 = -copysign(hypot(kp()/k()*omg2.s(), lf.nu()), alp2.s()),
657 calp1 = (lf.nu() < lf.nup() ?
658 (bet1.c() - lf.nu()) * (bet1.c() + lf.nu()) :
659 (lf.nup() - bet1.s()) * (lf.nup() + bet1.s())),
660 calp2 = (lf.nu() < lf.nup() ?
661 (bet2.c() - lf.nu()) * (bet2.c() + lf.nu()) :
662 (lf.nup() - bet2.s()) * (lf.nup() + bet2.s()));
663 calp1 = copysign(signbit(calp1) ? 0 : sqrt(calp1), alp1.c());
664 calp2 = copysign(signbit(calp2) ? 0 : sqrt(calp2), alp2.c());
665 alp1 = ang(salp1, calp1);
666 alp2 = ang(salp2, calp2);
667 }
668 }
669 if (swap12) {
670 swap(bet1, bet2);
671 swap(omg1, omg2);
672 swap(alp1, alp2);
673 swap(umb1, umb2);
674 // points not swapped if umb1 == true
675 alp1 += ang::cardinal(2);
676 if (umb2 && !biaxial())
677 alp2 += ang::cardinal((signbit(alp2.s()) ? -1 : 1) * bet2.s());
678 else
679 alp2 += ang::cardinal(2);
680 }
681
682 if constexpr (debug_)
683 cout << "F "
684 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
685 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
686 if (flip1) t().Flip(bet1, omg1, alp1);
687 if (flip2) t().Flip(bet2, omg2, alp2);
688 alp1.setn(); alp2.setn();
689 if constexpr (debug_)
690 cout << "G "
691 << real(bet1) << " " << real(omg1) << " " << real(alp1) << " "
692 << real(bet2) << " " << real(omg2) << " " << real(alp2) << "\n";
693
694 fic = TL::fline::fics(lf, bet10, omg10, alp1);
695 gic = TL::gline::gics(lg, fic);
696 gic.s13 = signbit(s12) ? 0 : s12;
697
698 // clang needs std::move instead of move.
699 return TL(std::move(lf), std::move(fic), std::move(lg), std::move(gic));
700 }
701
702 Math::real Geodesic3::HybridA(ang bet1, ang omg1, ang alp1,
703 ang bet2a, ang omg2b,
704 bool betp) const {
705 ang b1{bet1}, o1{omg1}, a1{alp1};
706 // a1 -= ang(1e-8);
707 gamblk gam = gamma(b1, o1, a1);
708 GeodesicLine3::fline l(this->t(), gam);
709 GeodesicLine3::fline::fics ic(l, b1, o1, a1);
710 real dang = l.Hybrid0(ic, bet2a, omg2b, betp);
711 return dang;
712 }
713
714 // Solve f(alp1) = 0 where alp1 is an azimuth and f(alp1) is the difference
715 // in lontitude on bet2 and the target longitude.
716 Angle Geodesic3::findroot(const function<real(const ang&)>& f,
717 ang xa, ang xb,
718 real fa, real fb,
719 int* countn, int* countb) {
720 // Implement root finding method of Chandrupatla (1997)
721 // https://doi.org/10.1016/s0965-9978(96)00051-8
722 // Here we follow Scherer (2013), Section 6.1.7.3
723 // https://doi.org/10.1007/978-3-319-00401-3
724
725 // Here the independent variable is an ang, but the computations on this
726 // variable essentially involve its conversion to radians. There's no need
727 // to worry about the angle wrapping around because (xb-xa).radians() is in
728 // (0,pi).
729
730 // require xa and xb to be normalized (the result is normalized)
731 // require fa and fb to have opposite signs
732
733 ang xm; // The return value
734 int cntn = 0, cntb = 0;
735 bool trip = false;
736 const bool debug = debug_;
737 // 25 iterations with line 498534 of testset.txt
738 // 43 -2 -43 141
739 // This is a near conjugate case m12 = 0.0003857
740 // 27 iterations with line 360115 of testpro.txt
741 // -75 29 75 -169
742 // Converge failures with line 40045 of testsph[bc]
743 // echo 80 -90 -80 90 | ./Geod3Solve -e 1 0 2 1 -i
744 // echo 80 -90 -80 90 | ./Geod3Solve -e 1 0 1 2 -i
745 //
746 // Offset for debugging output
747 real x0 = real(0);
748 // If fa and fb have the same signs, assume that the root is at one of the
749 // endpoints if corresponding f is small. Otherwise, it's an error.
750 if (fa * fb >= 0) {
751 if constexpr (debug)
752 cout << "FA FB " << fa/numeric_limits<real>::epsilon() << " "
753 << fb/numeric_limits<real>::epsilon() << " " << (fa == fb) << "\n";
754 if (fa == fb && fabs(fa) <= 512*numeric_limits<real>::epsilon())
755 // If both fa and fb have the same sign and are small (but not too
756 // small!), return mean of the endpoints. This is the case of
757 // antipodal points on a triaxial sphere, case A.c.3 general bet1/2 =
758 // -/+90, non-meridional. The mean angle corresponds to the "minor"
759 // ellipse where the call to Hybrid (to compute alp2) gives a well
760 // defined result.
761 return ang(xa.s() + xb.s(), xa.c() + xb.c());
762 else if (fmin(fabs(fa), fabs(fb)) > 2*numeric_limits<real>::epsilon())
763 // neither endpoint small enough
764 throw GeographicLib::GeographicErr
765 ("Bad inputs Geodesic3::findroot");
766 else
767 // return best endpoint
768 return fabs(fa) < fabs(fb) ? xa : xb;
769 }
770 // tp = 1 - t
771 for (real t = 1/real(2), tp = t, ab = 0, ft = 0, fc = 0;
772 cntn < maxit_ ||
773 (throw_ && (throw GeographicLib::GeographicErr
774 ("Convergence failure Geodesic3::findroot"), false));) {
775 ang xt = 2*t == 1 ?
776 ang(xa.s() + xb.s(), xa.c() + xb.c()) :
777 (t < tp ? xa - ang::radians(t * ab) :
778 xb + ang::radians(tp * ab)),
779 xc;
780 if (trip) {
781 xm = xt;
782 if constexpr (debug)
783 cout << "BREAKA\n";
784 break;
785 }
786 ++cntn;
787 ft = f(xt);
788 if constexpr (debug)
789 cout << "H " << cntn << " " << real(xt)-x0 << " " << ft << "\n";
790 if (!(fabs(ft) >= numeric_limits<real>::epsilon())) {
791 xm = xt;
792 if constexpr (debug)
793 cout << "BREAKB\n";
794 break;
795 }
796 if (signbit(ft) == signbit(fa)) {
797 xc = xa; xa = xt;
798 fc = fa; fa = ft;
799 } else {
800 xc = xb; xb = xa; xa = xt;
801 fc = fb; fb = fa; fa = ft;
802 }
803 xm = fabs(fb) < fabs(fa) ? xb : xa;
804 // ordering is b - a - c
805 ab = (xa-xb).radians0();
806 real
807 ca = (xc-xa).radians0(),
808 cb = ca+ab,
809 // Scherer has a fabs(cb). This should be fabs(ab).
810 tl = numeric_limits<real>::epsilon() / fabs(ab);
811 // Backward tests to deal with NaNs
812 if constexpr (debug)
813 cout << "R " << cntn << " " << ab << " " << cb << "\n";
814 trip = !(2 * tl < 1);
815 if (trip && debug)
816 cout << "TRIP " << ab << "\n";
817 // Increase the amount away from the boundary to make the next iteration.
818 // Otherwise we get two equal values of f near the boundary and a
819 // bisection is triggered.
820 tl = fmin(1/real(32), 16*tl);
821 real
822 xi = ab / cb,
823 xip = ca / cb, // 1 - xi
824 phi = (fa-fb) / (fc-fb),
825 phip = (fc-fa) / (fc-fb); // 1 - phi
826 if (!trip && Math::sq(phip) < xip && Math::sq(phi) < xi) {
827 t = fa/(fb-fa) * fc/(fb-fc) - ca/ab * fa/(fc-fa) * fb/(fc-fb);
828 if constexpr (debug)
829 cout << "J1 " << cntn << " " << t << " " << 1 - t << "\n";
830 // This equation matches the pseudocode in Scherer. His Eq (6.40)
831 // reads t = fa/(fb-fa) * fc/(fb-fc) + ca/cb * fc/(fc-fa) * fb/(fb-fa);
832 // this is wrong.
833 tp = fb/(fb-fa) * fc/(fc-fa) + cb/ab * fa/(fc-fa) * fb/(fc-fb);
834 t = fmax(tl, t);
835 tp = fmax(tl, tp);
836 // t = fmin(1 - tl, fmax(tl, t));
837 // tp = fmin(1 - tl, fmax(tl, tp));
838 } else {
839 t = tp = 1/real(2);
840 ++cntb;
841 if constexpr (debug)
842 cout << "J2 " << cntn << " " << t << " " << 1 - t << "\n";
843 }
844 }
845 if (countn) *countn += cntn;
846 if (countb) *countb += cntb;
847 return xm;
848 }
849
850 Geodesic3::gamblk::gamblk(const Geodesic3& tg,
851 ang bet, ang omg, ang alp) {
852 real a = tg.k() * bet.c() * alp.s(), b = tg.kp() * omg.s() * alp.c();
853 gamma = (a - b) * (a + b);
854 // This direct test case
855 // -30 -86 58.455576621187896848 -1.577754271270003
856 // fails badly with reverse direct if gamma is not set to zero here.
857 // Neighboring values of alp as double are
858 // 58.455576621187890, 58.455576621187895, 58.455576621187900
859 // 30 86 90 180
860 // dgam/dalp = 2*alp.c()*alp.s() * hypot(tg.k * bet.c(), tg.kp * omg.s())
861 real maxdiff = 0;
862 if (!(tg.k2() == 0 || tg.kp2() == 0)) {
863 // Force small gamma to zero for triaxial case
864 real
865 alpdiff = 2 * alp.c() * alp.s()
866 * (tg.k2() * Math::sq(bet.c())+tg.kp2() * Math::sq(omg.s())),
867 betdiff = -2 * bet.c() * bet.s() * tg.k2() * Math::sq(alp.s()),
868 omgdiff = -2 * omg.c() * omg.s() * tg.kp2() * Math::sq(alp.c());
869 maxdiff = fmax( fabs(alpdiff), fmax( fabs(betdiff), fabs(omgdiff) ) );
870 }
871 if (fabs(gamma) <= 3 * maxdiff * numeric_limits<real>::epsilon()) {
872 // Set gamma = 0 if a change of alp, bet, or omg by epsilon would include
873 // gamma = 0.
874 gamma = 0;
875 // If (_umbalt and not oblate) or prolate, set gamma = -0
876 if ((tg.umbalt() && tg.kp2() > 0) || tg.k2() == 0) gamma = -gamma;
877 }
878 transpolar = signbit(gamma);
879 gammax = fabs(gamma);
880 kx2 = !transpolar ? tg.k2() : tg.kp2();
881 kxp2 = transpolar ? tg.k2() : tg.kp2();
882 kx = !transpolar ? tg.k() : tg.kp();
883 kxp = transpolar ? tg.k() : tg.kp();
884 // gammap = sqrt(kx2 - gammax)
885 real gammap =
886 (!transpolar ?
887 hypot(kx * hypot(bet.s(), alp.c()*bet.c()),
888 kxp * omg.s()*alp.c()) :
889 hypot(kxp * bet.c()*alp.s(),
890 kx * hypot(omg.c(), alp.s()*omg.s())));
891 // for gam == 0, we have nu = 0, nup = 1
892 nu = sqrt(gammax) / kx;
893 nup = gammap / kx;
894 }
895
896 Geodesic3::gamblk::gamblk(const Geodesic3& tg, bool neg)
897 : transpolar(neg)
898 , gamma(transpolar ? -real(0) : real(0))
899 , nu(0)
900 , nup(1)
901 , gammax(0)
902 , kx2(!transpolar ? tg.k2() : tg.kp2())
903 , kxp2(transpolar ? tg.k2() : tg.kp2())
904 , kx(!transpolar ? tg.k() : tg.kp())
905 , kxp(transpolar ? tg.k() : tg.kp())
906 {}
907
908 Geodesic3::gamblk Geodesic3::gamma(ang bet, ang omg, ang alp) const {
909 return gamblk(*this, bet, omg, alp);
910 }
911
912 GeodesicLine3 Geodesic3::Line(Angle bet1, Angle omg1, Angle alp1) const {
913 return GeodesicLine3(*this, bet1, omg1, alp1);
914 }
915
916 GeodesicLine3 Geodesic3::Direct(Angle bet1, Angle omg1, Angle alp1, real s12,
917 Angle& bet2, Angle& omg2, Angle& alp2)
918 const {
919 GeodesicLine3 l(*this, bet1, omg1, alp1);
920 l.Position(s12, bet2, omg2, alp2);
921 return l;
922 }
923
924 GeodesicLine3 Geodesic3::Inverse(real bet1, real omg1, real bet2, real omg2,
925 real& s12, real& alp1, real& alp2) const {
926 ang alp1a, alp2a;
927 GeodesicLine3 l = Inverse(ang(bet1), ang(omg1), ang(bet2), ang(omg2),
928 s12, alp1a, alp2a);
929 alp1 = real(alp1a); alp2 = real(alp2a);
930 return l;
931 }
932
933 GeodesicLine3 Geodesic3::Line(real bet1, real omg1, real alp1) const {
934 return Line(ang(bet1), ang(omg1), ang(alp1));
935 }
936
937 GeodesicLine3 Geodesic3::Direct(real bet1, real omg1, real alp1, real s12,
938 real& bet2, real& omg2, real& alp2)
939 const {
940 ang bet2a, omg2a, alp2a;
941 GeodesicLine3 l = Direct(ang(bet1), ang(omg1), ang(alp1), s12,
942 bet2a, omg2a, alp2a);
943 bet2 = real(bet2a); omg2 = real(omg2a); alp2 = real(alp2a);
944 return l;
945 }
946
947 } // namespace Triaxial
948} // 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:89
GeographicLib::Triaxial::Ellipsoid3::AngNorm
static bool AngNorm(Angle &bet, Angle &omg, Angle &alp, bool alt=false)
Definition Ellipsoid3.hpp:255
GeographicLib::Triaxial::Geodesic3
The solution of the geodesic problem for a triaxial ellipsoid.
Definition Geodesic3.hpp:71
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:916
GeographicLib::Triaxial::Geodesic3::Line
GeodesicLine3 Line(Angle bet1, Angle omg1, Angle alp1) const
Definition Geodesic3.cpp:912
GeographicLib::Triaxial::Geodesic3::t
const Ellipsoid3 & t() const
Definition Geodesic3.hpp:193
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