00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "GeographicLib/Geodesic.hpp"
00030
00031 #define GEOGRAPHICLIB_GEODESIC_CPP "$Id: Geodesic.cpp 6827 2010-05-20 19:56:18Z karney $"
00032
00033 RCSID_DECL(GEOGRAPHICLIB_GEODESIC_CPP)
00034 RCSID_DECL(GEOGRAPHICLIB_GEODESIC_HPP)
00035
00036 namespace GeographicLib {
00037
00038 using namespace std;
00039
00040
00041
00042
00043 const Math::real Geodesic::eps2 = sqrt(numeric_limits<real>::min());
00044 const Math::real Geodesic::tol0 = numeric_limits<real>::epsilon();
00045 const Math::real Geodesic::tol1 = 100 * tol0;
00046 const Math::real Geodesic::tol2 = sqrt(numeric_limits<real>::epsilon());
00047 const Math::real Geodesic::xthresh = 1000 * tol2;
00048
00049 Geodesic::Geodesic(real a, real r)
00050 : _a(a)
00051 , _r(r)
00052 , _f(_r != 0 ? 1 / _r : 0)
00053 , _f1(1 - _f)
00054 , _e2(_f * (2 - _f))
00055 , _ep2(_e2 / sq(_f1))
00056 , _n(_f / ( 2 - _f))
00057 , _b(_a * _f1)
00058 , _etol2(tol2 / max(real(0.1), sqrt(abs(_e2))))
00059 {
00060 if (!(_a > 0))
00061 throw GeographicErr("Major radius is not positive");
00062 }
00063
00064 const Geodesic Geodesic::WGS84(Constants::WGS84_a(), Constants::WGS84_r());
00065
00066 Math::real Geodesic::SinSeries(real sinx, real cosx,
00067 const real c[], int n) throw() {
00068
00069
00070
00071 real
00072 ar = 2 * (cosx - sinx) * (cosx + sinx),
00073 y0 = n & 1 ? c[n--] : 0, y1 = 0;
00074
00075 while (n) {
00076
00077 y1 = ar * y0 - y1 + c[n--];
00078 y0 = ar * y1 - y0 + c[n--];
00079 }
00080 return 2 * sinx * cosx * y0;
00081 }
00082
00083 GeodesicLine Geodesic::Line(real lat1, real lon1, real azi1) const throw() {
00084 return GeodesicLine(*this, lat1, lon1, azi1);
00085 }
00086
00087 Math::real Geodesic::Direct(real lat1, real lon1, real azi1, real s12,
00088 real& lat2, real& lon2, real& azi2, real& m12,
00089 bool arcmode) const throw() {
00090 GeodesicLine l(*this, lat1, lon1, azi1);
00091 return l.Position(s12, lat2, lon2, azi2, m12, arcmode);
00092 }
00093
00094 Math::real Geodesic::Inverse(real lat1, real lon1, real lat2, real lon2,
00095 real& s12, real& azi1, real& azi2, real& m12)
00096 const throw() {
00097 lon1 = AngNormalize(lon1);
00098 real lon12 = AngNormalize(AngNormalize(lon2) - lon1);
00099
00100
00101 lon12 = AngRound(lon12);
00102
00103 int lonsign = lon12 >= 0 ? 1 : -1;
00104 lon12 *= lonsign;
00105 if (lon12 == 180)
00106 lonsign = 1;
00107
00108 lat1 = AngRound(lat1);
00109 lat2 = AngRound(lat2);
00110
00111 int swapp = abs(lat1) >= abs(lat2) ? 1 : -1;
00112 if (swapp < 0) {
00113 lonsign *= -1;
00114 swap(lat1, lat2);
00115 }
00116
00117 int latsign = lat1 < 0 ? 1 : -1;
00118 lat1 *= latsign;
00119 lat2 *= latsign;
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132 real phi, sbet1, cbet1, sbet2, cbet2;
00133
00134 phi = lat1 * Constants::degree();
00135
00136 sbet1 = _f1 * sin(phi);
00137 cbet1 = lat1 == -90 ? eps2 : cos(phi);
00138 SinCosNorm(sbet1, cbet1);
00139
00140 phi = lat2 * Constants::degree();
00141
00142 sbet2 = _f1 * sin(phi);
00143 cbet2 = abs(lat2) == 90 ? eps2 : cos(phi);
00144 SinCosNorm(sbet2, cbet2);
00145
00146 real
00147 lam12 = lon12 * Constants::degree(),
00148 slam12 = lon12 == 180 ? 0 : sin(lam12),
00149 clam12 = cos(lam12);
00150
00151 real sig12, calp1, salp1, calp2, salp2;
00152
00153 real C1a[nC1 + 1], C2a[nC2 + 1], C3a[nC3];
00154
00155 bool meridian = lat1 == -90 || slam12 == 0;
00156
00157 if (meridian) {
00158
00159
00160
00161
00162 calp1 = clam12; salp1 = slam12;
00163 calp2 = 1; salp2 = 0;
00164
00165 real
00166
00167 ssig1 = sbet1, csig1 = calp1 * cbet1,
00168 ssig2 = sbet2, csig2 = calp2 * cbet2;
00169
00170
00171 sig12 = atan2(max(csig1 * ssig2 - ssig1 * csig2, real(0)),
00172 csig1 * csig2 + ssig1 * ssig2);
00173 {
00174 real dummy;
00175 Lengths(_n, sig12, ssig1, csig1, ssig2, csig2,
00176 cbet1, cbet2, s12, m12, dummy, C1a, C2a);
00177 }
00178
00179
00180
00181
00182
00183
00184
00185 if (sig12 < 1 || m12 >= 0) {
00186 m12 *= _a;
00187 s12 *= _b;
00188 sig12 /= Constants::degree();
00189 } else
00190
00191 meridian = false;
00192 }
00193
00194 if (!meridian &&
00195 sbet1 == 0 &&
00196
00197 (_f <= 0 || lam12 <= Constants::pi() - _f * Constants::pi())) {
00198
00199
00200 calp1 = calp2 = 0; salp1 = salp2 = 1;
00201 s12 = _a * lam12;
00202 m12 = _b * sin(lam12 / _f1);
00203 sig12 = lon12 / _f1;
00204
00205 } else if (!meridian) {
00206
00207
00208
00209
00210
00211 sig12 = InverseStart(sbet1, cbet1, sbet2, cbet2,
00212 lam12,
00213 salp1, calp1, salp2, calp2,
00214 C1a, C2a);
00215
00216 if (sig12 >= 0) {
00217
00218 s12 = m12 = sig12 * _a * sqrt(1 - _e2 * sq(cbet1));
00219 sig12 /= Constants::degree();
00220 } else {
00221
00222
00223 real ssig1, csig1, ssig2, csig2, eps;
00224 real ov = 0;
00225 unsigned numit = 0;
00226 for (unsigned trip = 0; numit < maxit; ++numit) {
00227 real dv;
00228 real v = Lambda12(sbet1, cbet1, sbet2, cbet2, salp1, calp1,
00229 salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
00230 eps, trip < 1, dv, C1a, C2a, C3a) - lam12;
00231
00232 if (abs(v) <= eps2 || !(trip < 1)) {
00233 if (abs(v) > max(tol1, ov))
00234 numit = maxit;
00235 break;
00236 }
00237 real
00238 dalp1 = -v/dv;
00239 real
00240 sdalp1 = sin(dalp1), cdalp1 = cos(dalp1),
00241 nsalp1 = salp1 * cdalp1 + calp1 * sdalp1;
00242 calp1 = calp1 * cdalp1 - salp1 * sdalp1;
00243 salp1 = max(real(0), nsalp1);
00244 SinCosNorm(salp1, calp1);
00245
00246
00247
00248
00249
00250
00251 if (abs(v) < tol1 || sq(v) < ov * tol0) ++trip;
00252 ov = abs(v);
00253 }
00254
00255 {
00256 real dummy;
00257 Lengths(eps, sig12, ssig1, csig1, ssig2, csig2,
00258 cbet1, cbet2, s12, m12, dummy, C1a, C2a);
00259 }
00260 m12 *= _a;
00261 s12 *= _b;
00262 sig12 /= Constants::degree();
00263
00264 if (numit >= maxit) {
00265
00266 s12 *= -1; sig12 *= -1; m12 *= -1;
00267 salp1 *= -1; calp1 *= -1;
00268 salp2 *= -1; calp2 *= -1;
00269 }
00270 }
00271 }
00272
00273
00274 if (swapp < 0) {
00275 swap(salp1, salp2);
00276 swap(calp1, calp2);
00277 }
00278
00279
00280 azi1 = 0 - atan2(- swapp * lonsign * salp1,
00281 + swapp * latsign * calp1) / Constants::degree();
00282 azi2 = 0 - atan2(- swapp * lonsign * salp2,
00283 + swapp * latsign * calp2) / Constants::degree();
00284
00285
00286 return sig12;
00287 }
00288
00289 void Geodesic::Lengths(real eps, real sig12,
00290 real ssig1, real csig1, real ssig2, real csig2,
00291 real cbet1, real cbet2,
00292 real& s12b, real& m12a, real& m0,
00293
00294 real C1a[], real C2a[]) const throw() {
00295
00296
00297 C1f(eps, C1a);
00298 C2f(eps, C2a);
00299 real
00300 A1m1 = A1m1f(eps),
00301 AB1 = (1 + A1m1) * (SinSeries(ssig2, csig2, C1a, nC1) -
00302 SinSeries(ssig1, csig1, C1a, nC1)),
00303 A2m1 = A2m1f(eps),
00304 AB2 = (1 + A2m1) * (SinSeries(ssig2, csig2, C2a, nC2) -
00305 SinSeries(ssig1, csig1, C2a, nC2));
00306 m0 = A1m1 - A2m1;
00307
00308
00309
00310 m12a = (sqrt(1 - _e2 * sq(cbet2)) * (csig1 * ssig2) -
00311 sqrt(1 - _e2 * sq(cbet1)) * (ssig1 * csig2))
00312 - _f1 * csig1 * csig2 * ( m0 * sig12 + (AB1 - AB2) );
00313
00314 s12b = (1 + A1m1) * sig12 + AB1;
00315 }
00316
00317 Math::real Geodesic::Astroid(real x, real y) throw() {
00318
00319
00320 real k;
00321 real
00322 p = sq(x),
00323 q = sq(y),
00324 r = (p + q - 1) / 6;
00325 if ( !(q == 0 && r <= 0) ) {
00326 real
00327
00328
00329 S = p * q / 4,
00330 r2 = sq(r),
00331 r3 = r * r2,
00332
00333
00334 disc = S * (S + 2 * r3);
00335 real u = r;
00336 if (disc >= 0) {
00337 real T3 = S + r3;
00338
00339
00340
00341 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc);
00342
00343 real T = Math::cbrt(T3);
00344
00345 u += T + (T != 0 ? r2 / T : 0);
00346 } else {
00347
00348 real ang = atan2(sqrt(-disc), -(S + r3));
00349
00350
00351 u += 2 * r * cos(ang / 3);
00352 }
00353 real
00354 v = sqrt(sq(u) + q),
00355
00356 uv = u < 0 ? q / (v - u) : u + v,
00357 w = (uv - q) / (2 * v);
00358
00359
00360 k = uv / (sqrt(uv + sq(w)) + w);
00361 } else {
00362
00363
00364 k = 0;
00365 }
00366 return k;
00367 }
00368
00369 Math::real Geodesic::InverseStart(real sbet1, real cbet1,
00370 real sbet2, real cbet2,
00371 real lam12,
00372 real& salp1, real& calp1,
00373
00374 real& salp2, real& calp2,
00375
00376 real C1a[], real C2a[]) const throw() {
00377
00378
00379
00380 real
00381 sig12 = -1,
00382
00383 sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
00384 cbet12 = cbet2 * cbet1 + sbet2 * sbet1,
00385 sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
00386
00387 bool shortline = cbet12 >= 0 && sbet12 < real(0.5) &&
00388 lam12 <= Constants::pi() / 6;
00389 real
00390 omg12 = shortline ? lam12 / sqrt(1 - _e2 * sq(cbet1)) : lam12,
00391 somg12 = sin(omg12), comg12 = cos(omg12);
00392
00393 salp1 = cbet2 * somg12;
00394 calp1 = comg12 >= 0 ?
00395 sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12) :
00396 sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
00397
00398 real
00399 ssig12 = Math::hypot(salp1, calp1),
00400 csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
00401
00402 if (shortline && ssig12 < _etol2) {
00403
00404 salp2 = cbet1 * somg12;
00405 calp2 = (sbet12 - cbet1 * sbet2 * sq(somg12) / (1 + comg12));
00406
00407 sig12 = atan2(ssig12, csig12);
00408 } else if (csig12 >= 0 ||
00409 ssig12 >= 3 * abs(_f) * Constants::pi() * sq(cbet1)) {
00410
00411
00412 } else {
00413
00414
00415 real x, y, lamscale, betscale;
00416 if (_f >= 0) {
00417
00418 {
00419 real
00420 mu = sq(sbet1),
00421 k2 = mu * _ep2,
00422 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
00423 lamscale = _f * cbet1 * A3f(_f, eps) * Constants::pi();
00424 }
00425 betscale = lamscale * cbet1;
00426
00427 x = (lam12 - Constants::pi()) / lamscale;
00428 y = sbet12a / betscale;
00429 } else {
00430
00431 real
00432 cbet12a = cbet2 * cbet1 - sbet2 * sbet1,
00433 bet12a = atan2(sbet12a, cbet12a);
00434 real m0, dummy;
00435
00436
00437 Lengths(_n, Constants::pi() + bet12a, sbet1, -cbet1, sbet2, cbet2,
00438 cbet1, cbet2, dummy, x, m0, C1a, C2a);
00439 x = -1 + x/(_f1 * cbet1 * cbet2 * m0 * Constants::pi());
00440 betscale = x < -real(0.01) ? sbet12a / x :
00441 -_f * sq(cbet1) * Constants::pi();
00442 lamscale = betscale / cbet1;
00443 y = (lam12 - Constants::pi()) / lamscale;
00444 }
00445
00446 if (y > -tol1 && x > -1 - xthresh) {
00447
00448 if (_f >= 0) {
00449 salp1 = min(real( 1), -x); calp1 = - sqrt(1 - sq(salp1));
00450 } else {
00451 calp1 = max(real(-1), x); salp1 = sqrt(1 - sq(calp1));
00452 }
00453 } else {
00454
00455
00456
00457 real k = Astroid(x, y);
00458
00459 real
00460 omg12a = lamscale * ( _f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k ),
00461 somg12 = sin(omg12a), comg12 = -cos(omg12a);
00462
00463 salp1 = cbet2 * somg12;
00464 calp1 = sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
00465 }
00466 }
00467 SinCosNorm(salp1, calp1);
00468 return sig12;
00469 }
00470
00471 Math::real Geodesic::Lambda12(real sbet1, real cbet1, real sbet2, real cbet2,
00472 real salp1, real calp1,
00473 real& salp2, real& calp2,
00474 real& sig12,
00475 real& ssig1, real& csig1,
00476 real& ssig2, real& csig2,
00477 real& eps, bool diffp, real& dlam12,
00478
00479 real C1a[], real C2a[], real C3a[]) const
00480 throw() {
00481
00482 if (sbet1 == 0 && calp1 == 0)
00483
00484
00485 calp1 = -eps2;
00486
00487 real
00488
00489 salp0 = salp1 * cbet1,
00490 calp0 = Math::hypot(calp1, salp1 * sbet1);
00491
00492 real somg1, comg1, somg2, comg2, omg12, lam12, mu, k2;
00493
00494
00495 ssig1 = sbet1; somg1 = salp0 * sbet1;
00496 csig1 = comg1 = calp1 * cbet1;
00497 SinCosNorm(ssig1, csig1);
00498 SinCosNorm(somg1, comg1);
00499
00500
00501
00502
00503
00504 salp2 = cbet2 != cbet1 ? salp0 / cbet2 : salp1;
00505
00506
00507
00508
00509 calp2 = cbet2 != cbet1 || abs(sbet2) != -sbet1 ?
00510 sqrt(sq(calp1 * cbet1) + (cbet1 < -sbet1 ?
00511 (cbet2 - cbet1) * (cbet1 + cbet2) :
00512 (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
00513 abs(calp1);
00514
00515
00516 ssig2 = sbet2; somg2 = salp0 * sbet2;
00517 csig2 = comg2 = calp2 * cbet2;
00518 SinCosNorm(ssig2, csig2);
00519 SinCosNorm(somg2, comg2);
00520
00521
00522 sig12 = atan2(max(csig1 * ssig2 - ssig1 * csig2, real(0)),
00523 csig1 * csig2 + ssig1 * ssig2);
00524
00525
00526 omg12 = atan2(max(comg1 * somg2 - somg1 * comg2, real(0)),
00527 comg1 * comg2 + somg1 * somg2);
00528 real B312, h0;
00529 mu = sq(calp0);
00530 k2 = mu * _ep2;
00531 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
00532 C3f(_f, eps, C3a);
00533 B312 = (SinSeries(ssig2, csig2, C3a, nC3-1) -
00534 SinSeries(ssig1, csig1, C3a, nC3-1));
00535 h0 = -_f * A3f(_f, eps),
00536 lam12 = omg12 + salp0 * h0 * (sig12 + B312);
00537
00538 if (diffp) {
00539 if (calp2 == 0)
00540 dlam12 = - 2 * sqrt(1 - _e2 * sq(cbet1)) / sbet1;
00541 else {
00542 real dummy1, dummy2;
00543 Lengths(eps, sig12, ssig1, csig1, ssig2, csig2,
00544 cbet1, cbet2, dummy1, dlam12, dummy2, C1a, C2a);
00545 dlam12 /= calp2 * cbet2;
00546 }
00547 }
00548
00549 return lam12;
00550 }
00551
00552 GeodesicLine::GeodesicLine(const Geodesic& g,
00553 real lat1, real lon1, real azi1) throw()
00554 : _a(g._a)
00555 , _r(g._r)
00556 , _b(g._b)
00557 , _f1(g._f1)
00558 {
00559 azi1 = Geodesic::AngNormalize(azi1);
00560
00561 azi1 = Geodesic::AngRound(azi1);
00562 lon1 = Geodesic::AngNormalize(lon1);
00563 _lat1 = lat1;
00564 _lon1 = lon1;
00565 _azi1 = azi1;
00566
00567 real
00568 alp1 = azi1 * Constants::degree(),
00569
00570
00571 salp1 = azi1 == -180 ? 0 : sin(alp1),
00572 calp1 = azi1 == 90 ? 0 : cos(alp1);
00573 real cbet1, sbet1, phi;
00574 phi = lat1 * Constants::degree();
00575
00576 sbet1 = _f1 * sin(phi);
00577 cbet1 = abs(lat1) == 90 ? Geodesic::eps2 : cos(phi);
00578 Geodesic::SinCosNorm(sbet1, cbet1);
00579
00580
00581 _salp0 = salp1 * cbet1;
00582
00583
00584 _calp0 = Math::hypot(calp1, salp1 * sbet1);
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594 _ssig1 = sbet1; _somg1 = _salp0 * sbet1;
00595 _csig1 = _comg1 = sbet1 != 0 || calp1 != 0 ? cbet1 * calp1 : 1;
00596 Geodesic::SinCosNorm(_ssig1, _csig1);
00597 Geodesic::SinCosNorm(_somg1, _comg1);
00598
00599 real mu = sq(_calp0);
00600 _k2 = mu * g._ep2;
00601 real eps = _k2 / (2 * (1 + sqrt(1 + _k2)) + _k2);
00602 _A1m1 = Geodesic::A1m1f(eps);
00603 _A2m1 = Geodesic::A2m1f(eps);
00604
00605 Geodesic::C1f(eps, _C1a);
00606 Geodesic::C1pf(eps, _C1pa);
00607 Geodesic::C2f(eps, _C2a);
00608 Geodesic::C3f(g._f, eps, _C3a);
00609
00610 _B11 = Geodesic::SinSeries(_ssig1, _csig1, _C1a, nC1);
00611 _B21 = Geodesic::SinSeries(_ssig1, _csig1, _C2a, nC2);
00612 {
00613 real s = sin(_B11), c = cos(_B11);
00614
00615 _stau1 = _ssig1 * c + _csig1 * s;
00616 _ctau1 = _csig1 * c - _ssig1 * s;
00617 }
00618
00619
00620
00621 _A3c = -g._f * _salp0 * Geodesic::A3f(g._f, eps);
00622 _B31 = Geodesic::SinSeries(_ssig1, _csig1, _C3a, nC3-1);
00623 }
00624
00625 Math::real GeodesicLine::Position(real s12, real& lat2, real& lon2,
00626 real& azi2, real& m12, bool arcmode)
00627 const throw() {
00628 if (!Init())
00629
00630 return 0;
00631
00632 real sig12, ssig12, csig12, B12 = 0;
00633 if (arcmode) {
00634
00635 sig12 = s12 * Constants::degree();
00636 real s12a = abs(s12);
00637 s12a -= 180 * floor(s12a / 180);
00638 ssig12 = s12a == 0 ? 0 : sin(sig12);
00639 csig12 = s12a == 90 ? 0 : cos(sig12);
00640 } else {
00641
00642 real
00643 tau12 = s12 / (_b * (1 + _A1m1)),
00644 s = sin(tau12),
00645 c = cos(tau12);
00646
00647 B12 = - Geodesic::SinSeries(_stau1 * c + _ctau1 * s,
00648 _ctau1 * c - _stau1 * s,
00649 _C1pa, nC1p);
00650 sig12 = tau12 - (B12 - _B11);
00651 ssig12 = sin(sig12);
00652 csig12 = cos(sig12);
00653 }
00654
00655 real omg12, lam12, lon12;
00656 real ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2;
00657
00658 ssig2 = _ssig1 * csig12 + _csig1 * ssig12;
00659 csig2 = _csig1 * csig12 - _ssig1 * ssig12;
00660 if (arcmode)
00661 B12 = Geodesic::SinSeries(ssig2, csig2, _C1a, nC1);
00662
00663 sbet2 = _calp0 * ssig2;
00664
00665 cbet2 = Math::hypot(_salp0, _calp0 * csig2);
00666 if (cbet2 == 0)
00667
00668 cbet2 = csig2 = Geodesic::eps2;
00669
00670 somg2 = _salp0 * ssig2; comg2 = csig2;
00671
00672 salp2 = _salp0; calp2 = _calp0 * csig2;
00673
00674 omg12 = atan2(somg2 * _comg1 - comg2 * _somg1,
00675 comg2 * _comg1 + somg2 * _somg1);
00676 lam12 = omg12 + _A3c *
00677 ( sig12 + (Geodesic::SinSeries(ssig2, csig2, _C3a, nC3-1) - _B31));
00678 lon12 = lam12 / Constants::degree();
00679
00680
00681 lon12 = lon12 - 360 * floor(lon12/360 + real(0.5));
00682 lat2 = atan2(sbet2, _f1 * cbet2) / Constants::degree();
00683 lon2 = Geodesic::AngNormalize(_lon1 + lon12);
00684
00685 azi2 = 0 - atan2(-salp2, calp2) / Constants::degree();
00686
00687 real
00688 B22 = Geodesic::SinSeries(ssig2, csig2, _C2a, nC2),
00689 AB1 = (1 + _A1m1) * (B12 - _B11),
00690 AB2 = (1 + _A2m1) * (B22 - _B21);
00691
00692
00693 m12 = _b * ((sqrt(1 + _k2 * sq( ssig2)) * (_csig1 * ssig2) -
00694 sqrt(1 + _k2 * sq(_ssig1)) * (_ssig1 * csig2))
00695 - _csig1 * csig2 * ( (_A1m1 - _A2m1) * sig12 + (AB1 - AB2) ));
00696 if (arcmode)
00697 s12 = _b * ((1 + _A1m1) * sig12 + AB1);
00698
00699 return arcmode ? s12 : sig12 / Constants::degree();
00700 }
00701
00702 void GeodesicLine::Scale(real a12, real& M12, real& M21) const throw() {
00703 if (!Init())
00704
00705 return;
00706 real sig12 = a12 * Constants::degree(), ssig12, csig12;
00707 {
00708 real a12a = abs(a12);
00709 a12a -= 180 * floor(a12a / 180);
00710 ssig12 = a12a == 0 ? 0 : sin(sig12);
00711 csig12 = a12a == 90 ? 0 : cos(sig12);
00712 }
00713
00714 real
00715 ssig2 = _ssig1 * csig12 + _csig1 * ssig12,
00716 csig2 = _csig1 * csig12 - _ssig1 * ssig12,
00717 ssig1sq = sq(_ssig1),
00718 ssig2sq = sq( ssig2),
00719 w1 = sqrt(1 + _k2 * ssig1sq),
00720 w2 = sqrt(1 + _k2 * ssig2sq),
00721 B12 = Geodesic::SinSeries(ssig2, csig2, _C1a, nC1),
00722 B22 = Geodesic::SinSeries(ssig2, csig2, _C2a, nC2),
00723 AB1 = (1 + _A1m1) * (B12 - _B11),
00724 AB2 = (1 + _A2m1) * (B22 - _B21),
00725 J12 = (_A1m1 - _A2m1) * sig12 + (AB1 - AB2);
00726 M12 = csig12 + (_k2 * (ssig2sq - ssig1sq) * ssig2/ (w1 + w2)
00727 - csig2 * J12) * _ssig1 / w1;
00728 M21 = csig12 - (_k2 * (ssig2sq - ssig1sq) * _ssig1/ (w1 + w2)
00729 - _csig1 * J12) * ssig2 / w2;
00730 }
00731
00732
00733
00734
00735 Math::real Geodesic::A1m1f(real eps) throw() {
00736 real
00737 eps2 = sq(eps),
00738 t;
00739 switch (nA1/2) {
00740 case 0:
00741 t = 0;
00742 break;
00743 case 1:
00744 t = eps2/4;
00745 break;
00746 case 2:
00747 t = eps2*(eps2+16)/64;
00748 break;
00749 case 3:
00750 t = eps2*(eps2*(eps2+4)+64)/256;
00751 break;
00752 case 4:
00753 t = eps2*(eps2*(eps2*(25*eps2+64)+256)+4096)/16384;
00754 break;
00755 default:
00756 STATIC_ASSERT(nA1 >= 0 && nA1 <= 8, "Bad value of nA1");
00757 t = 0;
00758 }
00759 return (t + eps) / (1 - eps);
00760 }
00761
00762
00763 void Geodesic::C1f(real eps, real c[]) throw() {
00764 real
00765 eps2 = sq(eps),
00766 d = eps;
00767 switch (nC1) {
00768 case 0:
00769 break;
00770 case 1:
00771 c[1] = -d/2;
00772 break;
00773 case 2:
00774 c[1] = -d/2;
00775 d *= eps;
00776 c[2] = -d/16;
00777 break;
00778 case 3:
00779 c[1] = d*(3*eps2-8)/16;
00780 d *= eps;
00781 c[2] = -d/16;
00782 d *= eps;
00783 c[3] = -d/48;
00784 break;
00785 case 4:
00786 c[1] = d*(3*eps2-8)/16;
00787 d *= eps;
00788 c[2] = d*(eps2-2)/32;
00789 d *= eps;
00790 c[3] = -d/48;
00791 d *= eps;
00792 c[4] = -5*d/512;
00793 break;
00794 case 5:
00795 c[1] = d*((6-eps2)*eps2-16)/32;
00796 d *= eps;
00797 c[2] = d*(eps2-2)/32;
00798 d *= eps;
00799 c[3] = d*(9*eps2-16)/768;
00800 d *= eps;
00801 c[4] = -5*d/512;
00802 d *= eps;
00803 c[5] = -7*d/1280;
00804 break;
00805 case 6:
00806 c[1] = d*((6-eps2)*eps2-16)/32;
00807 d *= eps;
00808 c[2] = d*((64-9*eps2)*eps2-128)/2048;
00809 d *= eps;
00810 c[3] = d*(9*eps2-16)/768;
00811 d *= eps;
00812 c[4] = d*(3*eps2-5)/512;
00813 d *= eps;
00814 c[5] = -7*d/1280;
00815 d *= eps;
00816 c[6] = -7*d/2048;
00817 break;
00818 case 7:
00819 c[1] = d*(eps2*(eps2*(19*eps2-64)+384)-1024)/2048;
00820 d *= eps;
00821 c[2] = d*((64-9*eps2)*eps2-128)/2048;
00822 d *= eps;
00823 c[3] = d*((72-9*eps2)*eps2-128)/6144;
00824 d *= eps;
00825 c[4] = d*(3*eps2-5)/512;
00826 d *= eps;
00827 c[5] = d*(35*eps2-56)/10240;
00828 d *= eps;
00829 c[6] = -7*d/2048;
00830 d *= eps;
00831 c[7] = -33*d/14336;
00832 break;
00833 case 8:
00834 c[1] = d*(eps2*(eps2*(19*eps2-64)+384)-1024)/2048;
00835 d *= eps;
00836 c[2] = d*(eps2*(eps2*(7*eps2-18)+128)-256)/4096;
00837 d *= eps;
00838 c[3] = d*((72-9*eps2)*eps2-128)/6144;
00839 d *= eps;
00840 c[4] = d*((96-11*eps2)*eps2-160)/16384;
00841 d *= eps;
00842 c[5] = d*(35*eps2-56)/10240;
00843 d *= eps;
00844 c[6] = d*(9*eps2-14)/4096;
00845 d *= eps;
00846 c[7] = -33*d/14336;
00847 d *= eps;
00848 c[8] = -429*d/262144;
00849 break;
00850 default:
00851 STATIC_ASSERT(nC1 >= 0 && nC1 <= 8, "Bad value of nC1");
00852 }
00853 }
00854
00855
00856 void Geodesic::C1pf(real eps, real c[]) throw() {
00857 real
00858 eps2 = sq(eps),
00859 d = eps;
00860 switch (nC1p) {
00861 case 0:
00862 break;
00863 case 1:
00864 c[1] = d/2;
00865 break;
00866 case 2:
00867 c[1] = d/2;
00868 d *= eps;
00869 c[2] = 5*d/16;
00870 break;
00871 case 3:
00872 c[1] = d*(16-9*eps2)/32;
00873 d *= eps;
00874 c[2] = 5*d/16;
00875 d *= eps;
00876 c[3] = 29*d/96;
00877 break;
00878 case 4:
00879 c[1] = d*(16-9*eps2)/32;
00880 d *= eps;
00881 c[2] = d*(30-37*eps2)/96;
00882 d *= eps;
00883 c[3] = 29*d/96;
00884 d *= eps;
00885 c[4] = 539*d/1536;
00886 break;
00887 case 5:
00888 c[1] = d*(eps2*(205*eps2-432)+768)/1536;
00889 d *= eps;
00890 c[2] = d*(30-37*eps2)/96;
00891 d *= eps;
00892 c[3] = d*(116-225*eps2)/384;
00893 d *= eps;
00894 c[4] = 539*d/1536;
00895 d *= eps;
00896 c[5] = 3467*d/7680;
00897 break;
00898 case 6:
00899 c[1] = d*(eps2*(205*eps2-432)+768)/1536;
00900 d *= eps;
00901 c[2] = d*(eps2*(4005*eps2-4736)+3840)/12288;
00902 d *= eps;
00903 c[3] = d*(116-225*eps2)/384;
00904 d *= eps;
00905 c[4] = d*(2695-7173*eps2)/7680;
00906 d *= eps;
00907 c[5] = 3467*d/7680;
00908 d *= eps;
00909 c[6] = 38081*d/61440;
00910 break;
00911 case 7:
00912 c[1] = d*(eps2*((9840-4879*eps2)*eps2-20736)+36864)/73728;
00913 d *= eps;
00914 c[2] = d*(eps2*(4005*eps2-4736)+3840)/12288;
00915 d *= eps;
00916 c[3] = d*(eps2*(8703*eps2-7200)+3712)/12288;
00917 d *= eps;
00918 c[4] = d*(2695-7173*eps2)/7680;
00919 d *= eps;
00920 c[5] = d*(41604-141115*eps2)/92160;
00921 d *= eps;
00922 c[6] = 38081*d/61440;
00923 d *= eps;
00924 c[7] = 459485*d/516096;
00925 break;
00926 case 8:
00927 c[1] = d*(eps2*((9840-4879*eps2)*eps2-20736)+36864)/73728;
00928 d *= eps;
00929 c[2] = d*(eps2*((120150-86171*eps2)*eps2-142080)+115200)/368640;
00930 d *= eps;
00931 c[3] = d*(eps2*(8703*eps2-7200)+3712)/12288;
00932 d *= eps;
00933 c[4] = d*(eps2*(1082857*eps2-688608)+258720)/737280;
00934 d *= eps;
00935 c[5] = d*(41604-141115*eps2)/92160;
00936 d *= eps;
00937 c[6] = d*(533134-2200311*eps2)/860160;
00938 d *= eps;
00939 c[7] = 459485*d/516096;
00940 d *= eps;
00941 c[8] = 109167851*d/82575360;
00942 break;
00943 default:
00944 STATIC_ASSERT(nC1p >= 0 && nC1p <= 8, "Bad value of nC1p");
00945 }
00946 }
00947
00948
00949 Math::real Geodesic::A2m1f(real eps) throw() {
00950 real
00951 eps2 = sq(eps),
00952 t;
00953 switch (nA2/2) {
00954 case 0:
00955 t = 0;
00956 break;
00957 case 1:
00958 t = eps2/4;
00959 break;
00960 case 2:
00961 t = eps2*(9*eps2+16)/64;
00962 break;
00963 case 3:
00964 t = eps2*(eps2*(25*eps2+36)+64)/256;
00965 break;
00966 case 4:
00967 t = eps2*(eps2*(eps2*(1225*eps2+1600)+2304)+4096)/16384;
00968 break;
00969 default:
00970 STATIC_ASSERT(nA2 >= 0 && nA2 <= 8, "Bad value of nA2");
00971 t = 0;
00972 }
00973 return t * (1 - eps) - eps;
00974 }
00975
00976
00977 void Geodesic::C2f(real eps, real c[]) throw() {
00978 real
00979 eps2 = sq(eps),
00980 d = eps;
00981 switch (nC2) {
00982 case 0:
00983 break;
00984 case 1:
00985 c[1] = d/2;
00986 break;
00987 case 2:
00988 c[1] = d/2;
00989 d *= eps;
00990 c[2] = 3*d/16;
00991 break;
00992 case 3:
00993 c[1] = d*(eps2+8)/16;
00994 d *= eps;
00995 c[2] = 3*d/16;
00996 d *= eps;
00997 c[3] = 5*d/48;
00998 break;
00999 case 4:
01000 c[1] = d*(eps2+8)/16;
01001 d *= eps;
01002 c[2] = d*(eps2+6)/32;
01003 d *= eps;
01004 c[3] = 5*d/48;
01005 d *= eps;
01006 c[4] = 35*d/512;
01007 break;
01008 case 5:
01009 c[1] = d*(eps2*(eps2+2)+16)/32;
01010 d *= eps;
01011 c[2] = d*(eps2+6)/32;
01012 d *= eps;
01013 c[3] = d*(15*eps2+80)/768;
01014 d *= eps;
01015 c[4] = 35*d/512;
01016 d *= eps;
01017 c[5] = 63*d/1280;
01018 break;
01019 case 6:
01020 c[1] = d*(eps2*(eps2+2)+16)/32;
01021 d *= eps;
01022 c[2] = d*(eps2*(35*eps2+64)+384)/2048;
01023 d *= eps;
01024 c[3] = d*(15*eps2+80)/768;
01025 d *= eps;
01026 c[4] = d*(7*eps2+35)/512;
01027 d *= eps;
01028 c[5] = 63*d/1280;
01029 d *= eps;
01030 c[6] = 77*d/2048;
01031 break;
01032 case 7:
01033 c[1] = d*(eps2*(eps2*(41*eps2+64)+128)+1024)/2048;
01034 d *= eps;
01035 c[2] = d*(eps2*(35*eps2+64)+384)/2048;
01036 d *= eps;
01037 c[3] = d*(eps2*(69*eps2+120)+640)/6144;
01038 d *= eps;
01039 c[4] = d*(7*eps2+35)/512;
01040 d *= eps;
01041 c[5] = d*(105*eps2+504)/10240;
01042 d *= eps;
01043 c[6] = 77*d/2048;
01044 d *= eps;
01045 c[7] = 429*d/14336;
01046 break;
01047 case 8:
01048 c[1] = d*(eps2*(eps2*(41*eps2+64)+128)+1024)/2048;
01049 d *= eps;
01050 c[2] = d*(eps2*(eps2*(47*eps2+70)+128)+768)/4096;
01051 d *= eps;
01052 c[3] = d*(eps2*(69*eps2+120)+640)/6144;
01053 d *= eps;
01054 c[4] = d*(eps2*(133*eps2+224)+1120)/16384;
01055 d *= eps;
01056 c[5] = d*(105*eps2+504)/10240;
01057 d *= eps;
01058 c[6] = d*(33*eps2+154)/4096;
01059 d *= eps;
01060 c[7] = 429*d/14336;
01061 d *= eps;
01062 c[8] = 6435*d/262144;
01063 break;
01064 default:
01065 STATIC_ASSERT(nC2 >= 0 && nC2 <= 8, "Bad value of nC2");
01066 }
01067 }
01068
01069
01070 Math::real Geodesic::A3f(real f, real eps) throw() {
01071 real
01072 del = (f - eps) / (1 - eps),
01073
01074
01075 nu2 = sq( del != 0 ? eps / del : 1 );
01076 real g;
01077 switch (nA3) {
01078 case 0:
01079 g = 0;
01080 break;
01081 case 1:
01082 g = 1;
01083 break;
01084 case 2:
01085 g = 1;
01086 break;
01087 case 3:
01088 g = 1;
01089 break;
01090 case 4:
01091 g = (16-nu2*del*sq(del))/16;
01092 break;
01093 case 5:
01094 g = (del*(-sq(nu2)*del-4*nu2)*sq(del)+64)/64;
01095 break;
01096 case 6:
01097 g = (del*(del*(nu2*del-sq(nu2))-4*nu2)*sq(del)+64)/64;
01098 break;
01099 case 7:
01100 g = (del*(del*(del*(nu2*((2-nu2)*nu2+2)*del+2*nu2)-2*sq(nu2))-8*nu2)*
01101 sq(del)+128)/128;
01102 break;
01103 case 8:
01104 g = (del*(del*(del*(del*(nu2*(nu2*(15*nu2+14)+24)*del+nu2*((32-16*nu2)*
01105 nu2+32))+32*nu2)-32*sq(nu2))-128*nu2)*sq(del)+2048)/2048;
01106 break;
01107 default:
01108 STATIC_ASSERT(nA3 >= 0 && nA3 <= 8, "Bad value of nA3");
01109 g = 0;
01110 }
01111 return (2 - f)/(2 - del) * g;
01112 }
01113
01114
01115 void Geodesic::C3f(real f, real eps, real c[]) throw() {
01116 real
01117 del = (f - eps) / (1 - eps),
01118
01119
01120 nu2 = sq( del != 0 ? eps / del : 1 );
01121 real d = eps;
01122 switch (nC3) {
01123 case 0:
01124 break;
01125 case 1:
01126 break;
01127 case 2:
01128 c[1] = d/4;
01129 break;
01130 case 3:
01131 c[1] = d*(2-del)/8;
01132 d *= eps;
01133 c[2] = d/16;
01134 break;
01135 case 4:
01136 c[1] = d*(del*((-nu2-4)*del-8)+16)/64;
01137 d *= eps;
01138 c[2] = d*(4-3*del)/64;
01139 d *= eps;
01140 c[3] = 5*d/192;
01141 break;
01142 case 5:
01143 c[1] = d*(del*(del*((-nu2-4)*del-2*nu2-8)-16)+32)/128;
01144 d *= eps;
01145 c[2] = d*(del*((-nu2-2)*del-6)+8)/128;
01146 d *= eps;
01147 c[3] = d*(10-9*del)/384;
01148 d *= eps;
01149 c[4] = 7*d/512;
01150 break;
01151 case 6:
01152 c[1] = d*(del*(del*(del*(((1-nu2)*nu2-2)*del-nu2-4)-2*nu2-8)-16)+32)/128;
01153 d *= eps;
01154 c[2] = d*(del*((-del-2*nu2-4)*del-12)+16)/256;
01155 d *= eps;
01156 c[3] = d*(del*((-7*nu2-8)*del-36)+40)/1536;
01157 d *= eps;
01158 c[4] = d*(7-7*del)/512;
01159 d *= eps;
01160 c[5] = 21*d/2560;
01161 break;
01162 case 7:
01163 c[1] = d*(del*(del*(del*(del*((nu2*(3*nu2+8)-8)*del+(8-8*nu2)*nu2-16)-
01164 8*nu2-32)-16*nu2-64)-128)+256)/1024;
01165 d *= eps;
01166 c[2] = d*(del*(del*(del*((10-7*nu2)*nu2*del-8)-16*nu2-32)-96)+128)/2048;
01167 d *= eps;
01168 c[3] = d*(del*(del*(2*nu2*del-7*nu2-8)-36)+40)/1536;
01169 d *= eps;
01170 c[4] = d*(del*((-3*nu2-2)*del-14)+14)/1024;
01171 d *= eps;
01172 c[5] = d*(42-45*del)/5120;
01173 d *= eps;
01174 c[6] = 11*d/2048;
01175 break;
01176 case 8:
01177 c[1] = d*(del*(del*(del*(del*(del*((nu2*((80-61*nu2)*nu2+80)-64)*del+
01178 nu2*(48*nu2+128)-128)+(128-128*nu2)*nu2-256)-128*nu2-512)-256*nu2-
01179 1024)-2048)+4096)/16384;
01180 d *= eps;
01181 c[2] = d*(del*(del*(del*(del*((nu2*(37*nu2+48)+16)*del+(80-56*nu2)*nu2)-
01182 64)-128*nu2-256)-768)+1024)/16384;
01183 d *= eps;
01184 c[3] = d*(del*(del*(del*(((140-91*nu2)*nu2+48)*del+64*nu2)-224*nu2-256)-
01185 1152)+1280)/49152;
01186 d *= eps;
01187 c[4] = d*(del*(del*((23*nu2+10)*del-48*nu2-32)-224)+224)/16384;
01188 d *= eps;
01189 c[5] = d*(del*((-165*nu2-60)*del-720)+672)/81920;
01190 d *= eps;
01191 c[6] = d*(88-99*del)/16384;
01192 d *= eps;
01193 c[7] = 429*d/114688;
01194 break;
01195 default:
01196 STATIC_ASSERT(nC3 >= 0 && nC3 <= 8, "Bad value of nC3");
01197 }
01198 }
01199 }