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 #include "geodesic.h"
00027 #include <math.h>
00028
00029 #define GEOGRAPHICLIB_GEODESIC_ORDER 6
00030 #define nC1 GEOGRAPHICLIB_GEODESIC_ORDER
00031 #define nC1p GEOGRAPHICLIB_GEODESIC_ORDER
00032 #define nC2 GEOGRAPHICLIB_GEODESIC_ORDER
00033 #define nA3 GEOGRAPHICLIB_GEODESIC_ORDER
00034 #define nA3x nA3
00035 #define nC3 GEOGRAPHICLIB_GEODESIC_ORDER
00036 #define nC3x ((nC3 * (nC3 - 1)) / 2)
00037 #define nC4 GEOGRAPHICLIB_GEODESIC_ORDER
00038 #define nC4x ((nC4 * (nC4 + 1)) / 2)
00039
00040 typedef double real;
00041 typedef int boolx;
00042
00043 static unsigned init = 0;
00044 static const int FALSE = 0;
00045 static const int TRUE = 1;
00046 static unsigned digits, maxit1, maxit2;
00047 static real epsilon, realmin, pi, degree, NaN,
00048 tiny, tol0, tol1, tol2, tolb, xthresh;
00049
00050 static void Init() {
00051 if (!init) {
00052 #if defined(__DBL_MANT_DIG__)
00053 digits = __DBL_MANT_DIG__;
00054 #else
00055 digits = 53;
00056 #endif
00057 #if defined(__DBL_EPSILON__)
00058 epsilon = __DBL_EPSILON__;
00059 #else
00060 epsilon = pow(0.5, digits - 1);
00061 #endif
00062 #if defined(__DBL_MIN__)
00063 realmin = __DBL_MIN__;
00064 #else
00065 realmin = pow(0.5, 1022);
00066 #endif
00067 #if defined(M_PI)
00068 pi = M_PI;
00069 #else
00070 pi = atan2(0.0, -1.0);
00071 #endif
00072 maxit1 = 20;
00073 maxit2 = maxit1 + digits + 10;
00074 tiny = sqrt(realmin);
00075 tol0 = epsilon;
00076
00077
00078
00079 tol1 = 200 * tol0;
00080 tol2 = sqrt(tol0);
00081
00082 tolb = tol0 * tol2;
00083 xthresh = 1000 * tol2;
00084 degree = pi/180;
00085 NaN = sqrt(-1.0);
00086 init = 1;
00087 }
00088 }
00089
00090 enum captype {
00091 CAP_NONE = 0U,
00092 CAP_C1 = 1U<<0,
00093 CAP_C1p = 1U<<1,
00094 CAP_C2 = 1U<<2,
00095 CAP_C3 = 1U<<3,
00096 CAP_C4 = 1U<<4,
00097 CAP_ALL = 0x1FU,
00098 OUT_ALL = 0x7F80U
00099 };
00100
00101 static real sq(real x) { return x * x; }
00102 static real log1px(real x) {
00103 volatile real
00104 y = 1 + x,
00105 z = y - 1;
00106
00107
00108
00109
00110 return z == 0 ? x : x * log(y) / z;
00111 }
00112
00113 static real atanhx(real x) {
00114 real y = fabs(x);
00115 y = log1px(2 * y/(1 - y))/2;
00116 return x < 0 ? -y : y;
00117 }
00118
00119 static real hypotx(real x, real y)
00120 { return sqrt(x * x + y * y); }
00121
00122 static real cbrtx(real x) {
00123 real y = pow(fabs(x), 1/(real)(3));
00124 return x < 0 ? -y : y;
00125 }
00126
00127 static real sumx(real u, real v, real* t) {
00128 volatile real s = u + v;
00129 volatile real up = s - v;
00130 volatile real vpp = s - up;
00131 up -= u;
00132 vpp -= v;
00133 *t = -(up + vpp);
00134
00135
00136
00137 return s;
00138 }
00139
00140 static real minx(real x, real y)
00141 { return x < y ? x : y; }
00142
00143 static real maxx(real x, real y)
00144 { return x > y ? x : y; }
00145
00146 static void swapx(real* x, real* y)
00147 { real t = *x; *x = *y; *y = t; }
00148
00149 static void SinCosNorm(real* sinx, real* cosx) {
00150 real r = hypotx(*sinx, *cosx);
00151 *sinx /= r;
00152 *cosx /= r;
00153 }
00154
00155 static real AngNormalize(real x)
00156 { return x >= 180 ? x - 360 : (x < -180 ? x + 360 : x); }
00157 static real AngNormalize2(real x)
00158 { return AngNormalize(fmod(x, (real)(360))); }
00159
00160 static real AngDiff(real x, real y) {
00161 real t, d = sumx(-x, y, &t);
00162 if ((d - (real)(180)) + t > (real)(0))
00163 d -= (real)(360);
00164 else if ((d + (real)(180)) + t <= (real)(0))
00165 d += (real)(360);
00166 return d + t;
00167 }
00168
00169 static real AngRound(real x) {
00170 const real z = 1/(real)(16);
00171 volatile real y = fabs(x);
00172
00173 y = y < z ? z - (z - y) : y;
00174 return x < 0 ? -y : y;
00175 }
00176
00177 static void A3coeff(struct geod_geodesic* g);
00178 static void C3coeff(struct geod_geodesic* g);
00179 static void C4coeff(struct geod_geodesic* g);
00180 static real SinCosSeries(boolx sinp,
00181 real sinx, real cosx,
00182 const real c[], int n);
00183 static void Lengths(const struct geod_geodesic* g,
00184 real eps, real sig12,
00185 real ssig1, real csig1, real dn1,
00186 real ssig2, real csig2, real dn2,
00187 real cbet1, real cbet2,
00188 real* ps12b, real* pm12b, real* pm0,
00189 boolx scalep, real* pM12, real* pM21,
00190
00191 real C1a[], real C2a[]);
00192 static real Astroid(real x, real y);
00193 static real InverseStart(const struct geod_geodesic* g,
00194 real sbet1, real cbet1, real dn1,
00195 real sbet2, real cbet2, real dn2,
00196 real lam12,
00197 real* psalp1, real* pcalp1,
00198
00199 real* psalp2, real* pcalp2,
00200
00201 real* pdnm,
00202
00203 real C1a[], real C2a[]);
00204 static real Lambda12(const struct geod_geodesic* g,
00205 real sbet1, real cbet1, real dn1,
00206 real sbet2, real cbet2, real dn2,
00207 real salp1, real calp1,
00208 real* psalp2, real* pcalp2,
00209 real* psig12,
00210 real* pssig1, real* pcsig1,
00211 real* pssig2, real* pcsig2,
00212 real* peps, real* pdomg12,
00213 boolx diffp, real* pdlam12,
00214
00215 real C1a[], real C2a[], real C3a[]);
00216 static real A3f(const struct geod_geodesic* g, real eps);
00217 static void C3f(const struct geod_geodesic* g, real eps, real c[]);
00218 static void C4f(const struct geod_geodesic* g, real eps, real c[]);
00219 static real A1m1f(real eps);
00220 static void C1f(real eps, real c[]);
00221 static void C1pf(real eps, real c[]);
00222 static real A2m1f(real eps);
00223 static void C2f(real eps, real c[]);
00224 static int transit(real lon1, real lon2);
00225 static void accini(real s[]);
00226 static void acccopy(const real s[], real t[]);
00227 static void accadd(real s[], real y);
00228 static real accsum(const real s[], real y);
00229 static void accneg(real s[]);
00230
00231 void geod_init(struct geod_geodesic* g, real a, real f) {
00232 if (!init) Init();
00233 g->a = a;
00234 g->f = f <= 1 ? f : 1/f;
00235 g->f1 = 1 - g->f;
00236 g->e2 = g->f * (2 - g->f);
00237 g->ep2 = g->e2 / sq(g->f1);
00238 g->n = g->f / ( 2 - g->f);
00239 g->b = g->a * g->f1;
00240 g->c2 = (sq(g->a) + sq(g->b) *
00241 (g->e2 == 0 ? 1 :
00242 (g->e2 > 0 ? atanhx(sqrt(g->e2)) : atan(sqrt(-g->e2))) /
00243 sqrt(fabs(g->e2))))/2;
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253 g->etol2 = 0.1 * tol2 /
00254 sqrt( maxx((real)(0.001), fabs(g->f)) * minx((real)(1), 1 - g->f/2) / 2 );
00255
00256 A3coeff(g);
00257 C3coeff(g);
00258 C4coeff(g);
00259 }
00260
00261 void geod_lineinit(struct geod_geodesicline* l,
00262 const struct geod_geodesic* g,
00263 real lat1, real lon1, real azi1, unsigned caps) {
00264 real alp1, cbet1, sbet1, phi, eps;
00265 l->a = g->a;
00266 l->f = g->f;
00267 l->b = g->b;
00268 l->c2 = g->c2;
00269 l->f1 = g->f1;
00270
00271 l->caps = (caps ? caps : GEOD_DISTANCE_IN | GEOD_LONGITUDE) |
00272 GEOD_LATITUDE | GEOD_AZIMUTH;
00273
00274
00275 azi1 = AngRound(AngNormalize(azi1));
00276 lon1 = AngNormalize(lon1);
00277 l->lat1 = lat1;
00278 l->lon1 = lon1;
00279 l->azi1 = azi1;
00280
00281 alp1 = azi1 * degree;
00282
00283
00284 l->salp1 = azi1 == -180 ? 0 : sin(alp1);
00285 l->calp1 = fabs(azi1) == 90 ? 0 : cos(alp1);
00286 phi = lat1 * degree;
00287
00288 sbet1 = l->f1 * sin(phi);
00289 cbet1 = fabs(lat1) == 90 ? tiny : cos(phi);
00290 SinCosNorm(&sbet1, &cbet1);
00291 l->dn1 = sqrt(1 + g->ep2 * sq(sbet1));
00292
00293
00294 l->salp0 = l->salp1 * cbet1;
00295
00296
00297 l->calp0 = hypotx(l->calp1, l->salp1 * sbet1);
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307 l->ssig1 = sbet1; l->somg1 = l->salp0 * sbet1;
00308 l->csig1 = l->comg1 = sbet1 != 0 || l->calp1 != 0 ? cbet1 * l->calp1 : 1;
00309 SinCosNorm(&l->ssig1, &l->csig1);
00310
00311
00312 l->k2 = sq(l->calp0) * g->ep2;
00313 eps = l->k2 / (2 * (1 + sqrt(1 + l->k2)) + l->k2);
00314
00315 if (l->caps & CAP_C1) {
00316 real s, c;
00317 l->A1m1 = A1m1f(eps);
00318 C1f(eps, l->C1a);
00319 l->B11 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C1a, nC1);
00320 s = sin(l->B11); c = cos(l->B11);
00321
00322 l->stau1 = l->ssig1 * c + l->csig1 * s;
00323 l->ctau1 = l->csig1 * c - l->ssig1 * s;
00324
00325
00326 }
00327
00328 if (l->caps & CAP_C1p)
00329 C1pf(eps, l->C1pa);
00330
00331 if (l->caps & CAP_C2) {
00332 l->A2m1 = A2m1f(eps);
00333 C2f(eps, l->C2a);
00334 l->B21 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C2a, nC2);
00335 }
00336
00337 if (l->caps & CAP_C3) {
00338 C3f(g, eps, l->C3a);
00339 l->A3c = -l->f * l->salp0 * A3f(g, eps);
00340 l->B31 = SinCosSeries(TRUE, l->ssig1, l->csig1, l->C3a, nC3-1);
00341 }
00342
00343 if (l->caps & CAP_C4) {
00344 C4f(g, eps, l->C4a);
00345
00346 l->A4 = sq(l->a) * l->calp0 * l->salp0 * g->e2;
00347 l->B41 = SinCosSeries(FALSE, l->ssig1, l->csig1, l->C4a, nC4);
00348 }
00349 }
00350
00351 real geod_genposition(const struct geod_geodesicline* l,
00352 boolx arcmode, real s12_a12,
00353 real* plat2, real* plon2, real* pazi2,
00354 real* ps12, real* pm12,
00355 real* pM12, real* pM21,
00356 real* pS12) {
00357 real lat2 = 0, lon2 = 0, azi2 = 0, s12 = 0,
00358 m12 = 0, M12 = 0, M21 = 0, S12 = 0;
00359
00360 real sig12, ssig12, csig12, B12 = 0, AB1 = 0;
00361 real omg12, lam12, lon12;
00362 real ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2;
00363 unsigned outmask =
00364 (plat2 ? GEOD_LATITUDE : 0U) |
00365 (plon2 ? GEOD_LONGITUDE : 0U) |
00366 (pazi2 ? GEOD_AZIMUTH : 0U) |
00367 (ps12 ? GEOD_DISTANCE : 0U) |
00368 (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
00369 (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
00370 (pS12 ? GEOD_AREA : 0U);
00371
00372 outmask &= l->caps & OUT_ALL;
00373 if (!( TRUE &&
00374 (arcmode || (l->caps & GEOD_DISTANCE_IN & OUT_ALL)) ))
00375
00376 return NaN;
00377
00378 if (arcmode) {
00379 real s12a;
00380
00381 sig12 = s12_a12 * degree;
00382 s12a = fabs(s12_a12);
00383 s12a -= 180 * floor(s12a / 180);
00384 ssig12 = s12a == 0 ? 0 : sin(sig12);
00385 csig12 = s12a == 90 ? 0 : cos(sig12);
00386 } else {
00387
00388 real
00389 tau12 = s12_a12 / (l->b * (1 + l->A1m1)),
00390 s = sin(tau12),
00391 c = cos(tau12);
00392
00393 B12 = - SinCosSeries(TRUE,
00394 l->stau1 * c + l->ctau1 * s,
00395 l->ctau1 * c - l->stau1 * s,
00396 l->C1pa, nC1p);
00397 sig12 = tau12 - (B12 - l->B11);
00398 ssig12 = sin(sig12); csig12 = cos(sig12);
00399 if (fabs(l->f) > 0.01) {
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421 real
00422 ssig2 = l->ssig1 * csig12 + l->csig1 * ssig12,
00423 csig2 = l->csig1 * csig12 - l->ssig1 * ssig12,
00424 serr;
00425 B12 = SinCosSeries(TRUE, ssig2, csig2, l->C1a, nC1);
00426 serr = (1 + l->A1m1) * (sig12 + (B12 - l->B11)) - s12_a12 / l->b;
00427 sig12 = sig12 - serr / sqrt(1 + l->k2 * sq(ssig2));
00428 ssig12 = sin(sig12); csig12 = cos(sig12);
00429
00430 }
00431 }
00432
00433
00434 ssig2 = l->ssig1 * csig12 + l->csig1 * ssig12;
00435 csig2 = l->csig1 * csig12 - l->ssig1 * ssig12;
00436 dn2 = sqrt(1 + l->k2 * sq(ssig2));
00437 if (outmask & (GEOD_DISTANCE | GEOD_REDUCEDLENGTH | GEOD_GEODESICSCALE)) {
00438 if (arcmode || fabs(l->f) > 0.01)
00439 B12 = SinCosSeries(TRUE, ssig2, csig2, l->C1a, nC1);
00440 AB1 = (1 + l->A1m1) * (B12 - l->B11);
00441 }
00442
00443 sbet2 = l->calp0 * ssig2;
00444
00445 cbet2 = hypotx(l->salp0, l->calp0 * csig2);
00446 if (cbet2 == 0)
00447
00448 cbet2 = csig2 = tiny;
00449
00450 somg2 = l->salp0 * ssig2; comg2 = csig2;
00451
00452 salp2 = l->salp0; calp2 = l->calp0 * csig2;
00453
00454 omg12 = atan2(somg2 * l->comg1 - comg2 * l->somg1,
00455 comg2 * l->comg1 + somg2 * l->somg1);
00456
00457 if (outmask & GEOD_DISTANCE)
00458 s12 = arcmode ? l->b * ((1 + l->A1m1) * sig12 + AB1) : s12_a12;
00459
00460 if (outmask & GEOD_LONGITUDE) {
00461 lam12 = omg12 + l->A3c *
00462 ( sig12 + (SinCosSeries(TRUE, ssig2, csig2, l->C3a, nC3-1)
00463 - l->B31));
00464 lon12 = lam12 / degree;
00465
00466
00467 lon12 = AngNormalize2(lon12);
00468 lon2 = AngNormalize(l->lon1 + lon12);
00469 }
00470
00471 if (outmask & GEOD_LATITUDE)
00472 lat2 = atan2(sbet2, l->f1 * cbet2) / degree;
00473
00474 if (outmask & GEOD_AZIMUTH)
00475
00476 azi2 = 0 - atan2(-salp2, calp2) / degree;
00477
00478 if (outmask & (GEOD_REDUCEDLENGTH | GEOD_GEODESICSCALE)) {
00479 real
00480 B22 = SinCosSeries(TRUE, ssig2, csig2, l->C2a, nC2),
00481 AB2 = (1 + l->A2m1) * (B22 - l->B21),
00482 J12 = (l->A1m1 - l->A2m1) * sig12 + (AB1 - AB2);
00483 if (outmask & GEOD_REDUCEDLENGTH)
00484
00485
00486 m12 = l->b * ((dn2 * (l->csig1 * ssig2) - l->dn1 * (l->ssig1 * csig2))
00487 - l->csig1 * csig2 * J12);
00488 if (outmask & GEOD_GEODESICSCALE) {
00489 real t = l->k2 * (ssig2 - l->ssig1) * (ssig2 + l->ssig1) / (l->dn1 + dn2);
00490 M12 = csig12 + (t * ssig2 - csig2 * J12) * l->ssig1 / l->dn1;
00491 M21 = csig12 - (t * l->ssig1 - l->csig1 * J12) * ssig2 / dn2;
00492 }
00493 }
00494
00495 if (outmask & GEOD_AREA) {
00496 real
00497 B42 = SinCosSeries(FALSE, ssig2, csig2, l->C4a, nC4);
00498 real salp12, calp12;
00499 if (l->calp0 == 0 || l->salp0 == 0) {
00500
00501 salp12 = salp2 * l->calp1 - calp2 * l->salp1;
00502 calp12 = calp2 * l->calp1 + salp2 * l->salp1;
00503
00504
00505
00506
00507 if (salp12 == 0 && calp12 < 0) {
00508 salp12 = tiny * l->calp1;
00509 calp12 = -1;
00510 }
00511 } else {
00512
00513
00514
00515
00516
00517
00518
00519
00520 salp12 = l->calp0 * l->salp0 *
00521 (csig12 <= 0 ? l->csig1 * (1 - csig12) + ssig12 * l->ssig1 :
00522 ssig12 * (l->csig1 * ssig12 / (1 + csig12) + l->ssig1));
00523 calp12 = sq(l->salp0) + sq(l->calp0) * l->csig1 * csig2;
00524 }
00525 S12 = l->c2 * atan2(salp12, calp12) + l->A4 * (B42 - l->B41);
00526 }
00527
00528 if (outmask & GEOD_LATITUDE)
00529 *plat2 = lat2;
00530 if (outmask & GEOD_LONGITUDE)
00531 *plon2 = lon2;
00532 if (outmask & GEOD_AZIMUTH)
00533 *pazi2 = azi2;
00534 if (outmask & GEOD_DISTANCE)
00535 *ps12 = s12;
00536 if (outmask & GEOD_REDUCEDLENGTH)
00537 *pm12 = m12;
00538 if (outmask & GEOD_GEODESICSCALE) {
00539 if (pM12) *pM12 = M12;
00540 if (pM21) *pM21 = M21;
00541 }
00542 if (outmask & GEOD_AREA)
00543 *pS12 = S12;
00544
00545 return arcmode ? s12_a12 : sig12 / degree;
00546 }
00547
00548 void geod_position(const struct geod_geodesicline* l, real s12,
00549 real* plat2, real* plon2, real* pazi2) {
00550 geod_genposition(l, FALSE, s12, plat2, plon2, pazi2, 0, 0, 0, 0, 0);
00551 }
00552
00553 real geod_gendirect(const struct geod_geodesic* g,
00554 real lat1, real lon1, real azi1,
00555 boolx arcmode, real s12_a12,
00556 real* plat2, real* plon2, real* pazi2,
00557 real* ps12, real* pm12, real* pM12, real* pM21,
00558 real* pS12) {
00559 struct geod_geodesicline l;
00560 unsigned outmask =
00561 (plat2 ? GEOD_LATITUDE : 0U) |
00562 (plon2 ? GEOD_LONGITUDE : 0U) |
00563 (pazi2 ? GEOD_AZIMUTH : 0U) |
00564 (ps12 ? GEOD_DISTANCE : 0U) |
00565 (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
00566 (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
00567 (pS12 ? GEOD_AREA : 0U);
00568
00569 geod_lineinit(&l, g, lat1, lon1, azi1,
00570
00571 outmask | (arcmode ? GEOD_NONE : GEOD_DISTANCE_IN));
00572 return geod_genposition(&l, arcmode, s12_a12,
00573 plat2, plon2, pazi2, ps12, pm12, pM12, pM21, pS12);
00574 }
00575
00576 void geod_direct(const struct geod_geodesic* g,
00577 real lat1, real lon1, real azi1,
00578 real s12,
00579 real* plat2, real* plon2, real* pazi2) {
00580 geod_gendirect(g, lat1, lon1, azi1, FALSE, s12, plat2, plon2, pazi2,
00581 0, 0, 0, 0, 0);
00582 }
00583
00584 real geod_geninverse(const struct geod_geodesic* g,
00585 real lat1, real lon1, real lat2, real lon2,
00586 real* ps12, real* pazi1, real* pazi2,
00587 real* pm12, real* pM12, real* pM21, real* pS12) {
00588 real s12 = 0, azi1 = 0, azi2 = 0, m12 = 0, M12 = 0, M21 = 0, S12 = 0;
00589 real lon12;
00590 int latsign, lonsign, swapp;
00591 real phi, sbet1, cbet1, sbet2, cbet2, s12x = 0, m12x = 0;
00592 real dn1, dn2, lam12, slam12, clam12;
00593 real a12 = 0, sig12, calp1 = 0, salp1 = 0, calp2 = 0, salp2 = 0;
00594
00595 real C1a[nC1 + 1], C2a[nC2 + 1], C3a[nC3];
00596 boolx meridian;
00597 real omg12 = 0;
00598
00599 unsigned outmask =
00600 (ps12 ? GEOD_DISTANCE : 0U) |
00601 (pazi1 || pazi2 ? GEOD_AZIMUTH : 0U) |
00602 (pm12 ? GEOD_REDUCEDLENGTH : 0U) |
00603 (pM12 || pM21 ? GEOD_GEODESICSCALE : 0U) |
00604 (pS12 ? GEOD_AREA : 0U);
00605
00606 outmask &= OUT_ALL;
00607
00608
00609
00610 lon12 = AngDiff(AngNormalize(lon1), AngNormalize(lon2));
00611
00612 lon12 = AngRound(lon12);
00613
00614 lonsign = lon12 >= 0 ? 1 : -1;
00615 lon12 *= lonsign;
00616
00617 lat1 = AngRound(lat1);
00618 lat2 = AngRound(lat2);
00619
00620 swapp = fabs(lat1) >= fabs(lat2) ? 1 : -1;
00621 if (swapp < 0) {
00622 lonsign *= -1;
00623 swapx(&lat1, &lat2);
00624 }
00625
00626 latsign = lat1 < 0 ? 1 : -1;
00627 lat1 *= latsign;
00628 lat2 *= latsign;
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641 phi = lat1 * degree;
00642
00643 sbet1 = g->f1 * sin(phi);
00644 cbet1 = lat1 == -90 ? tiny : cos(phi);
00645 SinCosNorm(&sbet1, &cbet1);
00646
00647 phi = lat2 * degree;
00648
00649 sbet2 = g->f1 * sin(phi);
00650 cbet2 = fabs(lat2) == 90 ? tiny : cos(phi);
00651 SinCosNorm(&sbet2, &cbet2);
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661 if (cbet1 < -sbet1) {
00662 if (cbet2 == cbet1)
00663 sbet2 = sbet2 < 0 ? sbet1 : -sbet1;
00664 } else {
00665 if (fabs(sbet2) == -sbet1)
00666 cbet2 = cbet1;
00667 }
00668
00669 dn1 = sqrt(1 + g->ep2 * sq(sbet1));
00670 dn2 = sqrt(1 + g->ep2 * sq(sbet2));
00671
00672 lam12 = lon12 * degree;
00673 slam12 = lon12 == 180 ? 0 : sin(lam12);
00674 clam12 = cos(lam12);
00675
00676 meridian = lat1 == -90 || slam12 == 0;
00677
00678 if (meridian) {
00679
00680
00681
00682
00683 real ssig1, csig1, ssig2, csig2;
00684 calp1 = clam12; salp1 = slam12;
00685 calp2 = 1; salp2 = 0;
00686
00687
00688 ssig1 = sbet1; csig1 = calp1 * cbet1;
00689 ssig2 = sbet2; csig2 = calp2 * cbet2;
00690
00691
00692 sig12 = atan2(maxx(csig1 * ssig2 - ssig1 * csig2, (real)(0)),
00693 csig1 * csig2 + ssig1 * ssig2);
00694 {
00695 real dummy;
00696 Lengths(g, g->n, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
00697 cbet1, cbet2, &s12x, &m12x, &dummy,
00698 (outmask & GEOD_GEODESICSCALE) != 0U, &M12, &M21, C1a, C2a);
00699 }
00700
00701
00702
00703
00704
00705
00706
00707 if (sig12 < 1 || m12x >= 0) {
00708 m12x *= g->b;
00709 s12x *= g->b;
00710 a12 = sig12 / degree;
00711 } else
00712
00713 meridian = FALSE;
00714 }
00715
00716 if (!meridian &&
00717 sbet1 == 0 &&
00718
00719 (g->f <= 0 || lam12 <= pi - g->f * pi)) {
00720
00721
00722 calp1 = calp2 = 0; salp1 = salp2 = 1;
00723 s12x = g->a * lam12;
00724 sig12 = omg12 = lam12 / g->f1;
00725 m12x = g->b * sin(sig12);
00726 if (outmask & GEOD_GEODESICSCALE)
00727 M12 = M21 = cos(sig12);
00728 a12 = lon12 / g->f1;
00729
00730 } else if (!meridian) {
00731
00732
00733
00734
00735
00736 real dnm = 0;
00737 sig12 = InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
00738 lam12,
00739 &salp1, &calp1, &salp2, &calp2, &dnm,
00740 C1a, C2a);
00741
00742 if (sig12 >= 0) {
00743
00744 s12x = sig12 * g->b * dnm;
00745 m12x = sq(dnm) * g->b * sin(sig12 / dnm);
00746 if (outmask & GEOD_GEODESICSCALE)
00747 M12 = M21 = cos(sig12 / dnm);
00748 a12 = sig12 / degree;
00749 omg12 = lam12 / (g->f1 * dnm);
00750 } else {
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763 real ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, eps = 0;
00764 unsigned numit = 0;
00765
00766 real salp1a = tiny, calp1a = 1, salp1b = tiny, calp1b = -1;
00767 boolx tripn, tripb;
00768 for (tripn = FALSE, tripb = FALSE; numit < maxit2; ++numit) {
00769
00770
00771 real dv,
00772 v = (Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
00773 &salp2, &calp2, &sig12, &ssig1, &csig1, &ssig2, &csig2,
00774 &eps, &omg12, numit < maxit1, &dv, C1a, C2a, C3a)
00775 - lam12);
00776
00777
00778 if (tripb || !(fabs(v) >= (tripn ? 8 : 2) * tol0)) break;
00779
00780 if (v > 0 && (numit > maxit1 || calp1/salp1 > calp1b/salp1b))
00781 { salp1b = salp1; calp1b = calp1; }
00782 else if (v < 0 && (numit > maxit1 || calp1/salp1 < calp1a/salp1a))
00783 { salp1a = salp1; calp1a = calp1; }
00784 if (numit < maxit1 && dv > 0) {
00785 real
00786 dalp1 = -v/dv;
00787 real
00788 sdalp1 = sin(dalp1), cdalp1 = cos(dalp1),
00789 nsalp1 = salp1 * cdalp1 + calp1 * sdalp1;
00790 if (nsalp1 > 0 && fabs(dalp1) < pi) {
00791 calp1 = calp1 * cdalp1 - salp1 * sdalp1;
00792 salp1 = nsalp1;
00793 SinCosNorm(&salp1, &calp1);
00794
00795
00796
00797 tripn = fabs(v) <= 16 * tol0;
00798 continue;
00799 }
00800 }
00801
00802
00803
00804
00805
00806
00807
00808
00809 salp1 = (salp1a + salp1b)/2;
00810 calp1 = (calp1a + calp1b)/2;
00811 SinCosNorm(&salp1, &calp1);
00812 tripn = FALSE;
00813 tripb = (fabs(salp1a - salp1) + (calp1a - calp1) < tolb ||
00814 fabs(salp1 - salp1b) + (calp1 - calp1b) < tolb);
00815 }
00816 {
00817 real dummy;
00818 Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
00819 cbet1, cbet2, &s12x, &m12x, &dummy,
00820 (outmask & GEOD_GEODESICSCALE) != 0U, &M12, &M21, C1a, C2a);
00821 }
00822 m12x *= g->b;
00823 s12x *= g->b;
00824 a12 = sig12 / degree;
00825 omg12 = lam12 - omg12;
00826 }
00827 }
00828
00829 if (outmask & GEOD_DISTANCE)
00830 s12 = 0 + s12x;
00831
00832 if (outmask & GEOD_REDUCEDLENGTH)
00833 m12 = 0 + m12x;
00834
00835 if (outmask & GEOD_AREA) {
00836 real
00837
00838 salp0 = salp1 * cbet1,
00839 calp0 = hypotx(calp1, salp1 * sbet1);
00840 real alp12;
00841 if (calp0 != 0 && salp0 != 0) {
00842 real
00843
00844 ssig1 = sbet1, csig1 = calp1 * cbet1,
00845 ssig2 = sbet2, csig2 = calp2 * cbet2,
00846 k2 = sq(calp0) * g->ep2,
00847 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2),
00848
00849 A4 = sq(g->a) * calp0 * salp0 * g->e2;
00850 real C4a[nC4];
00851 real B41, B42;
00852 SinCosNorm(&ssig1, &csig1);
00853 SinCosNorm(&ssig2, &csig2);
00854 C4f(g, eps, C4a);
00855 B41 = SinCosSeries(FALSE, ssig1, csig1, C4a, nC4);
00856 B42 = SinCosSeries(FALSE, ssig2, csig2, C4a, nC4);
00857 S12 = A4 * (B42 - B41);
00858 } else
00859
00860 S12 = 0;
00861
00862 if (!meridian &&
00863 omg12 < (real)(0.75) * pi &&
00864 sbet2 - sbet1 < (real)(1.75)) {
00865
00866
00867
00868 real
00869 somg12 = sin(omg12), domg12 = 1 + cos(omg12),
00870 dbet1 = 1 + cbet1, dbet2 = 1 + cbet2;
00871 alp12 = 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
00872 domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) );
00873 } else {
00874
00875 real
00876 salp12 = salp2 * calp1 - calp2 * salp1,
00877 calp12 = calp2 * calp1 + salp2 * salp1;
00878
00879
00880
00881
00882 if (salp12 == 0 && calp12 < 0) {
00883 salp12 = tiny * calp1;
00884 calp12 = -1;
00885 }
00886 alp12 = atan2(salp12, calp12);
00887 }
00888 S12 += g->c2 * alp12;
00889 S12 *= swapp * lonsign * latsign;
00890
00891 S12 += 0;
00892 }
00893
00894
00895 if (swapp < 0) {
00896 swapx(&salp1, &salp2);
00897 swapx(&calp1, &calp2);
00898 if (outmask & GEOD_GEODESICSCALE)
00899 swapx(&M12, &M21);
00900 }
00901
00902 salp1 *= swapp * lonsign; calp1 *= swapp * latsign;
00903 salp2 *= swapp * lonsign; calp2 *= swapp * latsign;
00904
00905 if (outmask & GEOD_AZIMUTH) {
00906
00907 azi1 = 0 - atan2(-salp1, calp1) / degree;
00908 azi2 = 0 - atan2(-salp2, calp2) / degree;
00909 }
00910
00911 if (outmask & GEOD_DISTANCE)
00912 *ps12 = s12;
00913 if (outmask & GEOD_AZIMUTH) {
00914 if (pazi1) *pazi1 = azi1;
00915 if (pazi2) *pazi2 = azi2;
00916 }
00917 if (outmask & GEOD_REDUCEDLENGTH)
00918 *pm12 = m12;
00919 if (outmask & GEOD_GEODESICSCALE) {
00920 if (pM12) *pM12 = M12;
00921 if (pM21) *pM21 = M21;
00922 }
00923 if (outmask & GEOD_AREA)
00924 *pS12 = S12;
00925
00926
00927 return a12;
00928 }
00929
00930 void geod_inverse(const struct geod_geodesic* g,
00931 real lat1, real lon1, real lat2, real lon2,
00932 real* ps12, real* pazi1, real* pazi2) {
00933 geod_geninverse(g, lat1, lon1, lat2, lon2, ps12, pazi1, pazi2, 0, 0, 0, 0);
00934 }
00935
00936 real SinCosSeries(boolx sinp, real sinx, real cosx, const real c[], int n) {
00937
00938
00939
00940
00941
00942 real ar, y0, y1;
00943 c += (n + sinp);
00944 ar = 2 * (cosx - sinx) * (cosx + sinx);
00945 y0 = n & 1 ? *--c : 0; y1 = 0;
00946
00947 n /= 2;
00948 while (n--) {
00949
00950 y1 = ar * y0 - y1 + *--c;
00951 y0 = ar * y1 - y0 + *--c;
00952 }
00953 return sinp
00954 ? 2 * sinx * cosx * y0
00955 : cosx * (y0 - y1);
00956 }
00957
00958 void Lengths(const struct geod_geodesic* g,
00959 real eps, real sig12,
00960 real ssig1, real csig1, real dn1,
00961 real ssig2, real csig2, real dn2,
00962 real cbet1, real cbet2,
00963 real* ps12b, real* pm12b, real* pm0,
00964 boolx scalep, real* pM12, real* pM21,
00965
00966 real C1a[], real C2a[]) {
00967 real s12b = 0, m12b = 0, m0 = 0, M12 = 0, M21 = 0;
00968 real A1m1, AB1, A2m1, AB2, J12;
00969
00970
00971
00972 C1f(eps, C1a);
00973 C2f(eps, C2a);
00974 A1m1 = A1m1f(eps);
00975 AB1 = (1 + A1m1) * (SinCosSeries(TRUE, ssig2, csig2, C1a, nC1) -
00976 SinCosSeries(TRUE, ssig1, csig1, C1a, nC1));
00977 A2m1 = A2m1f(eps);
00978 AB2 = (1 + A2m1) * (SinCosSeries(TRUE, ssig2, csig2, C2a, nC2) -
00979 SinCosSeries(TRUE, ssig1, csig1, C2a, nC2));
00980 m0 = A1m1 - A2m1;
00981 J12 = m0 * sig12 + (AB1 - AB2);
00982
00983
00984
00985 m12b = dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) - csig1 * csig2 * J12;
00986
00987 s12b = (1 + A1m1) * sig12 + AB1;
00988 if (scalep) {
00989 real csig12 = csig1 * csig2 + ssig1 * ssig2;
00990 real t = g->ep2 * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2);
00991 M12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1;
00992 M21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2;
00993 }
00994 *ps12b = s12b;
00995 *pm12b = m12b;
00996 *pm0 = m0;
00997 if (scalep) {
00998 *pM12 = M12;
00999 *pM21 = M21;
01000 }
01001 }
01002
01003 real Astroid(real x, real y) {
01004
01005
01006 real k;
01007 real
01008 p = sq(x),
01009 q = sq(y),
01010 r = (p + q - 1) / 6;
01011 if ( !(q == 0 && r <= 0) ) {
01012 real
01013
01014
01015 S = p * q / 4,
01016 r2 = sq(r),
01017 r3 = r * r2,
01018
01019
01020 disc = S * (S + 2 * r3);
01021 real u = r;
01022 real v, uv, w;
01023 if (disc >= 0) {
01024 real T3 = S + r3, T;
01025
01026
01027
01028 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc);
01029
01030 T = cbrtx(T3);
01031
01032 u += T + (T != 0 ? r2 / T : 0);
01033 } else {
01034
01035 real ang = atan2(sqrt(-disc), -(S + r3));
01036
01037
01038 u += 2 * r * cos(ang / 3);
01039 }
01040 v = sqrt(sq(u) + q);
01041
01042 uv = u < 0 ? q / (v - u) : u + v;
01043 w = (uv - q) / (2 * v);
01044
01045
01046 k = uv / (sqrt(uv + sq(w)) + w);
01047 } else {
01048
01049
01050 k = 0;
01051 }
01052 return k;
01053 }
01054
01055 real InverseStart(const struct geod_geodesic* g,
01056 real sbet1, real cbet1, real dn1,
01057 real sbet2, real cbet2, real dn2,
01058 real lam12,
01059 real* psalp1, real* pcalp1,
01060
01061 real* psalp2, real* pcalp2,
01062
01063 real* pdnm,
01064
01065 real C1a[], real C2a[]) {
01066 real salp1 = 0, calp1 = 0, salp2 = 0, calp2 = 0, dnm = 0;
01067
01068
01069
01070
01071 real
01072 sig12 = -1,
01073
01074 sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
01075 cbet12 = cbet2 * cbet1 + sbet2 * sbet1;
01076 #if defined(__GNUC__) && __GNUC__ == 4 && \
01077 (__GNUC_MINOR__ < 6 || defined(__MINGW32__))
01078
01079
01080
01081
01082
01083
01084 real sbet12a;
01085 {
01086 volatile real xx1 = sbet2 * cbet1;
01087 volatile real xx2 = cbet2 * sbet1;
01088 sbet12a = xx1 + xx2;
01089 }
01090 #else
01091 real sbet12a = sbet2 * cbet1 + cbet2 * sbet1;
01092 #endif
01093 boolx shortline = cbet12 >= 0 && sbet12 < (real)(0.5) &&
01094 cbet2 * lam12 < (real)(0.5);
01095 real omg12 = lam12, somg12, comg12, ssig12, csig12;
01096 if (shortline) {
01097 real sbetm2 = sq(sbet1 + sbet2);
01098
01099
01100 sbetm2 /= sbetm2 + sq(cbet1 + cbet2);
01101 dnm = sqrt(1 + g->ep2 * sbetm2);
01102 omg12 /= g->f1 * dnm;
01103 }
01104 somg12 = sin(omg12); comg12 = cos(omg12);
01105
01106 salp1 = cbet2 * somg12;
01107 calp1 = comg12 >= 0 ?
01108 sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12) :
01109 sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
01110
01111 ssig12 = hypotx(salp1, calp1);
01112 csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
01113
01114 if (shortline && ssig12 < g->etol2) {
01115
01116 salp2 = cbet1 * somg12;
01117 calp2 = sbet12 - cbet1 * sbet2 *
01118 (comg12 >= 0 ? sq(somg12) / (1 + comg12) : 1 - comg12);
01119 SinCosNorm(&salp2, &calp2);
01120
01121 sig12 = atan2(ssig12, csig12);
01122 } else if (fabs(g->n) > (real)(0.1) ||
01123 csig12 >= 0 ||
01124 ssig12 >= 6 * fabs(g->n) * pi * sq(cbet1)) {
01125
01126 } else {
01127
01128
01129 real y, lamscale, betscale;
01130
01131
01132
01133 volatile real x;
01134 if (g->f >= 0) {
01135
01136 {
01137 real
01138 k2 = sq(sbet1) * g->ep2,
01139 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
01140 lamscale = g->f * cbet1 * A3f(g, eps) * pi;
01141 }
01142 betscale = lamscale * cbet1;
01143
01144 x = (lam12 - pi) / lamscale;
01145 y = sbet12a / betscale;
01146 } else {
01147
01148 real
01149 cbet12a = cbet2 * cbet1 - sbet2 * sbet1,
01150 bet12a = atan2(sbet12a, cbet12a);
01151 real m12b, m0, dummy;
01152
01153
01154 Lengths(g, g->n, pi + bet12a,
01155 sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
01156 cbet1, cbet2, &dummy, &m12b, &m0, FALSE,
01157 &dummy, &dummy, C1a, C2a);
01158 x = -1 + m12b / (cbet1 * cbet2 * m0 * pi);
01159 betscale = x < -(real)(0.01) ? sbet12a / x :
01160 -g->f * sq(cbet1) * pi;
01161 lamscale = betscale / cbet1;
01162 y = (lam12 - pi) / lamscale;
01163 }
01164
01165 if (y > -tol1 && x > -1 - xthresh) {
01166
01167 if (g->f >= 0) {
01168 salp1 = minx((real)(1), -(real)(x)); calp1 = - sqrt(1 - sq(salp1));
01169 } else {
01170 calp1 = maxx((real)(x > -tol1 ? 0 : -1), (real)(x));
01171 salp1 = sqrt(1 - sq(calp1));
01172 }
01173 } else {
01174
01175
01176
01177
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
01201
01202
01203
01204
01205
01206
01207
01208 real k = Astroid(x, y);
01209 real
01210 omg12a = lamscale * ( g->f >= 0 ? -x * k/(1 + k) : -y * (1 + k)/k );
01211 somg12 = sin(omg12a); comg12 = -cos(omg12a);
01212
01213 salp1 = cbet2 * somg12;
01214 calp1 = sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12);
01215 }
01216 }
01217 if (salp1 > 0)
01218 SinCosNorm(&salp1, &calp1);
01219 else {
01220 salp1 = 1; calp1 = 0;
01221 }
01222
01223 *psalp1 = salp1;
01224 *pcalp1 = calp1;
01225 if (shortline)
01226 *pdnm = dnm;
01227 if (sig12 >= 0) {
01228 *psalp2 = salp2;
01229 *pcalp2 = calp2;
01230 }
01231 return sig12;
01232 }
01233
01234 real Lambda12(const struct geod_geodesic* g,
01235 real sbet1, real cbet1, real dn1,
01236 real sbet2, real cbet2, real dn2,
01237 real salp1, real calp1,
01238 real* psalp2, real* pcalp2,
01239 real* psig12,
01240 real* pssig1, real* pcsig1,
01241 real* pssig2, real* pcsig2,
01242 real* peps, real* pdomg12,
01243 boolx diffp, real* pdlam12,
01244
01245 real C1a[], real C2a[], real C3a[]) {
01246 real salp2 = 0, calp2 = 0, sig12 = 0,
01247 ssig1 = 0, csig1 = 0, ssig2 = 0, csig2 = 0, eps = 0, domg12 = 0, dlam12 = 0;
01248 real salp0, calp0;
01249 real somg1, comg1, somg2, comg2, omg12, lam12;
01250 real B312, h0, k2;
01251
01252 if (sbet1 == 0 && calp1 == 0)
01253
01254
01255 calp1 = -tiny;
01256
01257
01258 salp0 = salp1 * cbet1;
01259 calp0 = hypotx(calp1, salp1 * sbet1);
01260
01261
01262
01263 ssig1 = sbet1; somg1 = salp0 * sbet1;
01264 csig1 = comg1 = calp1 * cbet1;
01265 SinCosNorm(&ssig1, &csig1);
01266
01267
01268
01269
01270
01271
01272 salp2 = cbet2 != cbet1 ? salp0 / cbet2 : salp1;
01273
01274
01275
01276
01277 calp2 = cbet2 != cbet1 || fabs(sbet2) != -sbet1 ?
01278 sqrt(sq(calp1 * cbet1) +
01279 (cbet1 < -sbet1 ?
01280 (cbet2 - cbet1) * (cbet1 + cbet2) :
01281 (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 :
01282 fabs(calp1);
01283
01284
01285 ssig2 = sbet2; somg2 = salp0 * sbet2;
01286 csig2 = comg2 = calp2 * cbet2;
01287 SinCosNorm(&ssig2, &csig2);
01288
01289
01290
01291 sig12 = atan2(maxx(csig1 * ssig2 - ssig1 * csig2, (real)(0)),
01292 csig1 * csig2 + ssig1 * ssig2);
01293
01294
01295 omg12 = atan2(maxx(comg1 * somg2 - somg1 * comg2, (real)(0)),
01296 comg1 * comg2 + somg1 * somg2);
01297 k2 = sq(calp0) * g->ep2;
01298 eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2);
01299 C3f(g, eps, C3a);
01300 B312 = (SinCosSeries(TRUE, ssig2, csig2, C3a, nC3-1) -
01301 SinCosSeries(TRUE, ssig1, csig1, C3a, nC3-1));
01302 h0 = -g->f * A3f(g, eps);
01303 domg12 = salp0 * h0 * (sig12 + B312);
01304 lam12 = omg12 + domg12;
01305
01306 if (diffp) {
01307 if (calp2 == 0)
01308 dlam12 = - 2 * g->f1 * dn1 / sbet1;
01309 else {
01310 real dummy;
01311 Lengths(g, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
01312 cbet1, cbet2, &dummy, &dlam12, &dummy,
01313 FALSE, &dummy, &dummy, C1a, C2a);
01314 dlam12 *= g->f1 / (calp2 * cbet2);
01315 }
01316 }
01317
01318 *psalp2 = salp2;
01319 *pcalp2 = calp2;
01320 *psig12 = sig12;
01321 *pssig1 = ssig1;
01322 *pcsig1 = csig1;
01323 *pssig2 = ssig2;
01324 *pcsig2 = csig2;
01325 *peps = eps;
01326 *pdomg12 = domg12;
01327 if (diffp)
01328 *pdlam12 = dlam12;
01329
01330 return lam12;
01331 }
01332
01333 real A3f(const struct geod_geodesic* g, real eps) {
01334
01335 real v = 0;
01336 int i;
01337 for (i = nA3x; i; )
01338 v = eps * v + g->A3x[--i];
01339 return v;
01340 }
01341
01342 void C3f(const struct geod_geodesic* g, real eps, real c[]) {
01343
01344
01345 int i, j, k;
01346 real mult = 1;
01347 for (j = nC3x, k = nC3 - 1; k; ) {
01348 real t = 0;
01349 for (i = nC3 - k; i; --i)
01350 t = eps * t + g->C3x[--j];
01351 c[k--] = t;
01352 }
01353
01354 for (k = 1; k < nC3; ) {
01355 mult *= eps;
01356 c[k++] *= mult;
01357 }
01358 }
01359
01360 void C4f(const struct geod_geodesic* g, real eps, real c[]) {
01361
01362
01363 int i, j, k;
01364 real mult = 1;
01365 for (j = nC4x, k = nC4; k; ) {
01366 real t = 0;
01367 for (i = nC4 - k + 1; i; --i)
01368 t = eps * t + g->C4x[--j];
01369 c[--k] = t;
01370 }
01371
01372 for (k = 1; k < nC4; ) {
01373 mult *= eps;
01374 c[k++] *= mult;
01375 }
01376 }
01377
01378
01379
01380
01381 real A1m1f(real eps) {
01382 real
01383 eps2 = sq(eps),
01384 t = eps2*(eps2*(eps2+4)+64)/256;
01385 return (t + eps) / (1 - eps);
01386 }
01387
01388
01389 void C1f(real eps, real c[]) {
01390 real
01391 eps2 = sq(eps),
01392 d = eps;
01393 c[1] = d*((6-eps2)*eps2-16)/32;
01394 d *= eps;
01395 c[2] = d*((64-9*eps2)*eps2-128)/2048;
01396 d *= eps;
01397 c[3] = d*(9*eps2-16)/768;
01398 d *= eps;
01399 c[4] = d*(3*eps2-5)/512;
01400 d *= eps;
01401 c[5] = -7*d/1280;
01402 d *= eps;
01403 c[6] = -7*d/2048;
01404 }
01405
01406
01407 void C1pf(real eps, real c[]) {
01408 real
01409 eps2 = sq(eps),
01410 d = eps;
01411 c[1] = d*(eps2*(205*eps2-432)+768)/1536;
01412 d *= eps;
01413 c[2] = d*(eps2*(4005*eps2-4736)+3840)/12288;
01414 d *= eps;
01415 c[3] = d*(116-225*eps2)/384;
01416 d *= eps;
01417 c[4] = d*(2695-7173*eps2)/7680;
01418 d *= eps;
01419 c[5] = 3467*d/7680;
01420 d *= eps;
01421 c[6] = 38081*d/61440;
01422 }
01423
01424
01425 real A2m1f(real eps) {
01426 real
01427 eps2 = sq(eps),
01428 t = eps2*(eps2*(25*eps2+36)+64)/256;
01429 return t * (1 - eps) - eps;
01430 }
01431
01432
01433 void C2f(real eps, real c[]) {
01434 real
01435 eps2 = sq(eps),
01436 d = eps;
01437 c[1] = d*(eps2*(eps2+2)+16)/32;
01438 d *= eps;
01439 c[2] = d*(eps2*(35*eps2+64)+384)/2048;
01440 d *= eps;
01441 c[3] = d*(15*eps2+80)/768;
01442 d *= eps;
01443 c[4] = d*(7*eps2+35)/512;
01444 d *= eps;
01445 c[5] = 63*d/1280;
01446 d *= eps;
01447 c[6] = 77*d/2048;
01448 }
01449
01450
01451 void A3coeff(struct geod_geodesic* g) {
01452 g->A3x[0] = 1;
01453 g->A3x[1] = (g->n-1)/2;
01454 g->A3x[2] = (g->n*(3*g->n-1)-2)/8;
01455 g->A3x[3] = ((-g->n-3)*g->n-1)/16;
01456 g->A3x[4] = (-2*g->n-3)/64;
01457 g->A3x[5] = -3/(real)(128);
01458 }
01459
01460
01461 void C3coeff(struct geod_geodesic* g) {
01462 g->C3x[0] = (1-g->n)/4;
01463 g->C3x[1] = (1-g->n*g->n)/8;
01464 g->C3x[2] = ((3-g->n)*g->n+3)/64;
01465 g->C3x[3] = (2*g->n+5)/128;
01466 g->C3x[4] = 3/(real)(128);
01467 g->C3x[5] = ((g->n-3)*g->n+2)/32;
01468 g->C3x[6] = ((-3*g->n-2)*g->n+3)/64;
01469 g->C3x[7] = (g->n+3)/128;
01470 g->C3x[8] = 5/(real)(256);
01471 g->C3x[9] = (g->n*(5*g->n-9)+5)/192;
01472 g->C3x[10] = (9-10*g->n)/384;
01473 g->C3x[11] = 7/(real)(512);
01474 g->C3x[12] = (7-14*g->n)/512;
01475 g->C3x[13] = 7/(real)(512);
01476 g->C3x[14] = 21/(real)(2560);
01477 }
01478
01479
01480
01481
01482 void C4coeff(struct geod_geodesic* g) {
01483 g->C4x[0] = (g->n*(g->n*(g->n*(g->n*(100*g->n+208)+572)+3432)-12012)+30030)/
01484 45045;
01485 g->C4x[1] = (g->n*(g->n*(g->n*(64*g->n+624)-4576)+6864)-3003)/15015;
01486 g->C4x[2] = (g->n*((14144-10656*g->n)*g->n-4576)-858)/45045;
01487 g->C4x[3] = ((-224*g->n-4784)*g->n+1573)/45045;
01488 g->C4x[4] = (1088*g->n+156)/45045;
01489 g->C4x[5] = 97/(real)(15015);
01490 g->C4x[6] = (g->n*(g->n*((-64*g->n-624)*g->n+4576)-6864)+3003)/135135;
01491 g->C4x[7] = (g->n*(g->n*(5952*g->n-11648)+9152)-2574)/135135;
01492 g->C4x[8] = (g->n*(5792*g->n+1040)-1287)/135135;
01493 g->C4x[9] = (468-2944*g->n)/135135;
01494 g->C4x[10] = 1/(real)(9009);
01495 g->C4x[11] = (g->n*((4160-1440*g->n)*g->n-4576)+1716)/225225;
01496 g->C4x[12] = ((4992-8448*g->n)*g->n-1144)/225225;
01497 g->C4x[13] = (1856*g->n-936)/225225;
01498 g->C4x[14] = 8/(real)(10725);
01499 g->C4x[15] = (g->n*(3584*g->n-3328)+1144)/315315;
01500 g->C4x[16] = (1024*g->n-208)/105105;
01501 g->C4x[17] = -136/(real)(63063);
01502 g->C4x[18] = (832-2560*g->n)/405405;
01503 g->C4x[19] = -128/(real)(135135);
01504 g->C4x[20] = 128/(real)(99099);
01505 }
01506
01507 int transit(real lon1, real lon2) {
01508 real lon12;
01509
01510
01511
01512 lon1 = AngNormalize(lon1);
01513 lon2 = AngNormalize(lon2);
01514 lon12 = AngDiff(lon1, lon2);
01515 return lon1 < 0 && lon2 >= 0 && lon12 > 0 ? 1 :
01516 (lon2 < 0 && lon1 >= 0 && lon12 < 0 ? -1 : 0);
01517 }
01518
01519 void accini(real s[]) {
01520
01521 s[0] = s[1] = 0;
01522 }
01523
01524 void acccopy(const real s[], real t[]) {
01525
01526 t[0] = s[0]; t[1] = s[1];
01527 }
01528
01529 void accadd(real s[], real y) {
01530
01531 real u, z = sumx(y, s[1], &u);
01532 s[0] = sumx(z, s[0], &s[1]);
01533 if (s[0] == 0)
01534 s[0] = u;
01535 else
01536 s[1] = s[1] + u;
01537 }
01538
01539 real accsum(const real s[], real y) {
01540
01541 real t[2];
01542 acccopy(s, t);
01543 accadd(t, y);
01544 return t[0];
01545 }
01546
01547 void accneg(real s[]) {
01548
01549 s[0] = -s[0]; s[1] = -s[1];
01550 }
01551
01552 void geod_polygon_init(struct geod_polygon* p, boolx polylinep) {
01553 p->lat0 = p->lon0 = p->lat = p->lon = NaN;
01554 p->polyline = (polylinep != 0);
01555 accini(p->P);
01556 accini(p->A);
01557 p->num = p->crossings = 0;
01558 }
01559
01560 void geod_polygon_addpoint(const struct geod_geodesic* g,
01561 struct geod_polygon* p,
01562 real lat, real lon) {
01563 lon = AngNormalize(lon);
01564 if (p->num == 0) {
01565 p->lat0 = p->lat = lat;
01566 p->lon0 = p->lon = lon;
01567 } else {
01568 real s12, S12;
01569 geod_geninverse(g, p->lat, p->lon, lat, lon,
01570 &s12, 0, 0, 0, 0, 0, p->polyline ? 0 : &S12);
01571 accadd(p->P, s12);
01572 if (!p->polyline) {
01573 accadd(p->A, S12);
01574 p->crossings += transit(p->lon, lon);
01575 }
01576 p->lat = lat; p->lon = lon;
01577 }
01578 ++p->num;
01579 }
01580
01581 void geod_polygon_addedge(const struct geod_geodesic* g,
01582 struct geod_polygon* p,
01583 real azi, real s) {
01584 if (p->num) {
01585 real lat, lon, S12;
01586 geod_gendirect(g, p->lat, p->lon, azi, FALSE, s,
01587 &lat, &lon, 0,
01588 0, 0, 0, 0, p->polyline ? 0 : &S12);
01589 accadd(p->P, s);
01590 if (!p->polyline) {
01591 accadd(p->A, S12);
01592 p->crossings += transit(p->lon, lon);
01593 }
01594 p->lat = lat; p->lon = lon;
01595 ++p->num;
01596 }
01597 }
01598
01599 unsigned geod_polygon_compute(const struct geod_geodesic* g,
01600 const struct geod_polygon* p,
01601 boolx reverse, boolx sign,
01602 real* pA, real* pP) {
01603 real s12, S12, t[2], area0;
01604 int crossings;
01605 if (p->num < 2) {
01606 if (pP) *pP = 0;
01607 if (!p->polyline && pA) *pA = 0;
01608 return p->num;
01609 }
01610 if (p->polyline) {
01611 if (pP) *pP = p->P[0];
01612 return p->num;
01613 }
01614 geod_geninverse(g, p->lat, p->lon, p->lat0, p->lon0,
01615 &s12, 0, 0, 0, 0, 0, &S12);
01616 if (pP) *pP = accsum(p->P, s12);
01617 acccopy(p->A, t);
01618 accadd(t, S12);
01619 crossings = p->crossings + transit(p->lon, p->lon0);
01620 area0 = 4 * pi * g->c2;
01621 if (crossings & 1)
01622 accadd(t, (t[0] < 0 ? 1 : -1) * area0/2);
01623
01624
01625 if (!reverse)
01626 accneg(t);
01627
01628 if (sign) {
01629 if (t[0] > area0/2)
01630 accadd(t, -area0);
01631 else if (t[0] <= -area0/2)
01632 accadd(t, +area0);
01633 } else {
01634 if (t[0] >= area0)
01635 accadd(t, -area0);
01636 else if (t[0] < 0)
01637 accadd(t, +area0);
01638 }
01639 if (pA) *pA = 0 + t[0];
01640 return p->num;
01641 }
01642
01643 unsigned geod_polygon_testpoint(const struct geod_geodesic* g,
01644 const struct geod_polygon* p,
01645 real lat, real lon,
01646 boolx reverse, boolx sign,
01647 real* pA, real* pP) {
01648 real perimeter, tempsum, area0;
01649 int crossings, i;
01650 unsigned num = p->num + 1;
01651 if (num == 1) {
01652 if (pP) *pP = 0;
01653 if (!p->polyline && pA) *pA = 0;
01654 return num;
01655 }
01656 perimeter = p->P[0];
01657 tempsum = p->polyline ? 0 : p->A[0];
01658 crossings = p->crossings;
01659 for (i = 0; i < (p->polyline ? 1 : 2); ++i) {
01660 real s12, S12;
01661 geod_geninverse(g,
01662 i == 0 ? p->lat : lat, i == 0 ? p->lon : lon,
01663 i != 0 ? p->lat0 : lat, i != 0 ? p->lon0 : lon,
01664 &s12, 0, 0, 0, 0, 0, p->polyline ? 0 : &S12);
01665 perimeter += s12;
01666 if (!p->polyline) {
01667 tempsum += S12;
01668 crossings += transit(i == 0 ? p->lon : lon,
01669 i != 0 ? p->lon0 : lon);
01670 }
01671 }
01672
01673 if (pP) *pP = perimeter;
01674 if (p->polyline)
01675 return num;
01676
01677 area0 = 4 * pi * g->c2;
01678 if (crossings & 1)
01679 tempsum += (tempsum < 0 ? 1 : -1) * area0/2;
01680
01681
01682 if (!reverse)
01683 tempsum *= -1;
01684
01685 if (sign) {
01686 if (tempsum > area0/2)
01687 tempsum -= area0;
01688 else if (tempsum <= -area0/2)
01689 tempsum += area0;
01690 } else {
01691 if (tempsum >= area0)
01692 tempsum -= area0;
01693 else if (tempsum < 0)
01694 tempsum += area0;
01695 }
01696 if (pA) *pA = 0 + tempsum;
01697 return num;
01698 }
01699
01700 unsigned geod_polygon_testedge(const struct geod_geodesic* g,
01701 const struct geod_polygon* p,
01702 real azi, real s,
01703 boolx reverse, boolx sign,
01704 real* pA, real* pP) {
01705 real perimeter, tempsum, area0;
01706 int crossings;
01707 unsigned num = p->num + 1;
01708 if (num == 1) {
01709 if (pP) *pP = NaN;
01710 if (!p->polyline && pA) *pA = NaN;
01711 return 0;
01712 }
01713 perimeter = p->P[0] + s;
01714 if (p->polyline) {
01715 if (pP) *pP = perimeter;
01716 return num;
01717 }
01718
01719 tempsum = p->A[0];
01720 crossings = p->crossings;
01721 {
01722 real lat, lon, s12, S12;
01723 geod_gendirect(g, p->lat, p->lon, azi, FALSE, s,
01724 &lat, &lon, 0,
01725 0, 0, 0, 0, &S12);
01726 tempsum += S12;
01727 crossings += transit(p->lon, lon);
01728 geod_geninverse(g, lat, lon, p->lat0, p->lon0,
01729 &s12, 0, 0, 0, 0, 0, &S12);
01730 perimeter += s12;
01731 tempsum += S12;
01732 crossings += transit(lon, p->lon0);
01733 }
01734
01735 area0 = 4 * pi * g->c2;
01736 if (crossings & 1)
01737 tempsum += (tempsum < 0 ? 1 : -1) * area0/2;
01738
01739
01740 if (!reverse)
01741 tempsum *= -1;
01742
01743 if (sign) {
01744 if (tempsum > area0/2)
01745 tempsum -= area0;
01746 else if (tempsum <= -area0/2)
01747 tempsum += area0;
01748 } else {
01749 if (tempsum >= area0)
01750 tempsum -= area0;
01751 else if (tempsum < 0)
01752 tempsum += area0;
01753 }
01754 if (pP) *pP = perimeter;
01755 if (pA) *pA = 0 + tempsum;
01756 return num;
01757 }
01758
01759 void geod_polygonarea(const struct geod_geodesic* g,
01760 real lats[], real lons[], int n,
01761 real* pA, real* pP) {
01762 int i;
01763 struct geod_polygon p;
01764 geod_polygon_init(&p, FALSE);
01765 for (i = 0; i < n; ++i)
01766 geod_polygon_addpoint(g, &p, lats[i], lons[i]);
01767 geod_polygon_compute(g, &p, FALSE, TRUE, pA, pP);
01768 }
01769
01770