00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <GeographicLib/EllipticFunction.hpp>
00011
00012 #if defined(_MSC_VER)
00013
00014 # pragma warning (disable: 4127)
00015 #endif
00016
00017 namespace GeographicLib {
00018
00019 using namespace std;
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 Math::real EllipticFunction::RF(real x, real y, real z) {
00030
00031 real tolRF =
00032 pow(3 * numeric_limits<real>::epsilon() * real(0.01), 1/real(8));
00033 real
00034 A0 = (x + y + z)/3,
00035 An = A0,
00036 Q = max(max(abs(A0-x), abs(A0-y)), abs(A0-z)) / tolRF,
00037 x0 = x,
00038 y0 = y,
00039 z0 = z,
00040 mul = 1;
00041 while (Q >= mul * abs(An)) {
00042
00043 real lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0);
00044 An = (An + lam)/4;
00045 x0 = (x0 + lam)/4;
00046 y0 = (y0 + lam)/4;
00047 z0 = (z0 + lam)/4;
00048 mul *= 4;
00049 }
00050 real
00051 X = (A0 - x) / (mul * An),
00052 Y = (A0 - y) / (mul * An),
00053 Z = - (X + Y),
00054 E2 = X*Y - Z*Z,
00055 E3 = X*Y*Z;
00056
00057
00058
00059
00060
00061 return (E3 * (6930 * E3 + E2 * (15015 * E2 - 16380) + 17160) +
00062 E2 * ((10010 - 5775 * E2) * E2 - 24024) + 240240) /
00063 (240240 * sqrt(An));
00064 }
00065
00066 Math::real EllipticFunction::RF(real x, real y) {
00067
00068 real tolRG0 =
00069 real(2.7) * sqrt((numeric_limits<real>::epsilon() * real(0.01)));
00070 real xn = sqrt(x), yn = sqrt(y);
00071 if (xn < yn) swap(xn, yn);
00072 while (abs(xn-yn) > tolRG0 * xn) {
00073
00074 real t = (xn + yn) /2;
00075 yn = sqrt(xn * yn);
00076 xn = t;
00077 }
00078 return Math::pi() / (xn + yn);
00079 }
00080
00081 Math::real EllipticFunction::RC(real x, real y) {
00082 return ( !(x >= y) ?
00083
00084 atan(sqrt((y - x) / x)) / sqrt(y - x) :
00085 ( x == y && y > 0 ? 1 / sqrt(y) :
00086 Math::atanh( y > 0 ?
00087
00088 sqrt((x - y) / x) :
00089
00090 sqrt(x / (x - y)) ) / sqrt(x - y) ) );
00091 }
00092
00093 Math::real EllipticFunction::RG(real x, real y, real z) {
00094 if (z == 0)
00095 swap(y, z);
00096
00097 return (z * RF(x, y, z) - (x-z) * (y-z) * RD(x, y, z) / 3
00098 + sqrt(x * y / z)) / 2;
00099 }
00100
00101 Math::real EllipticFunction::RG(real x, real y) {
00102
00103 real tolRG0 =
00104 real(2.7) * sqrt((numeric_limits<real>::epsilon() * real(0.01)));
00105 real
00106 x0 = sqrt(max(x, y)),
00107 y0 = sqrt(min(x, y)),
00108 xn = x0,
00109 yn = y0,
00110 s = 0,
00111 mul = real(0.25);
00112 while (abs(xn-yn) > tolRG0 * xn) {
00113
00114 real t = (xn + yn) /2;
00115 yn = sqrt(xn * yn);
00116 xn = t;
00117 mul *= 2;
00118 t = xn - yn;
00119 s += mul * t * t;
00120 }
00121 return (Math::sq( (x0 + y0)/2 ) - s) * Math::pi() / (2 * (xn + yn));
00122 }
00123
00124 Math::real EllipticFunction::RJ(real x, real y, real z, real p) {
00125
00126 real tolRD = pow(real(0.2) * (numeric_limits<real>::epsilon() * real(0.01)),
00127 1/real(8));
00128 real
00129 A0 = (x + y + z + 2*p)/5,
00130 An = A0,
00131 delta = (p-x) * (p-y) * (p-z),
00132 Q = max(max(abs(A0-x), abs(A0-y)), max(abs(A0-z), abs(A0-p))) / tolRD,
00133 x0 = x,
00134 y0 = y,
00135 z0 = z,
00136 p0 = p,
00137 mul = 1,
00138 mul3 = 1,
00139 s = 0;
00140 while (Q >= mul * abs(An)) {
00141
00142 real
00143 lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0),
00144 d0 = (sqrt(p0)+sqrt(x0)) * (sqrt(p0)+sqrt(y0)) * (sqrt(p0)+sqrt(z0)),
00145 e0 = delta/(mul3 * Math::sq(d0));
00146 s += RC(1, 1 + e0)/(mul * d0);
00147 An = (An + lam)/4;
00148 x0 = (x0 + lam)/4;
00149 y0 = (y0 + lam)/4;
00150 z0 = (z0 + lam)/4;
00151 p0 = (p0 + lam)/4;
00152 mul *= 4;
00153 mul3 *= 64;
00154 }
00155 real
00156 X = (A0 - x) / (mul * An),
00157 Y = (A0 - y) / (mul * An),
00158 Z = (A0 - z) / (mul * An),
00159 P = -(X + Y + Z) / 2,
00160 E2 = X*Y + X*Z + Y*Z - 3*P*P,
00161 E3 = X*Y*Z + 2*P * (E2 + 2*P*P),
00162 E4 = (2*X*Y*Z + P * (E2 + 3*P*P)) * P,
00163 E5 = X*Y*Z*P*P;
00164
00165
00166
00167
00168
00169 return ((471240 - 540540 * E2) * E5 +
00170 (612612 * E2 - 540540 * E3 - 556920) * E4 +
00171 E3 * (306306 * E3 + E2 * (675675 * E2 - 706860) + 680680) +
00172 E2 * ((417690 - 255255 * E2) * E2 - 875160) + 4084080) /
00173 (4084080 * mul * An * sqrt(An)) + 6 * s;
00174 }
00175
00176 Math::real EllipticFunction::RD(real x, real y, real z) {
00177
00178 real tolRD = pow(real(0.2) * (numeric_limits<real>::epsilon() * real(0.01)),
00179 1/real(8));
00180 real
00181 A0 = (x + y + 3*z)/5,
00182 An = A0,
00183 Q = max(max(abs(A0-x), abs(A0-y)), abs(A0-z)) / tolRD,
00184 x0 = x,
00185 y0 = y,
00186 z0 = z,
00187 mul = 1,
00188 s = 0;
00189 while (Q >= mul * abs(An)) {
00190
00191 real lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0);
00192 s += 1/(mul * sqrt(z0) * (z0 + lam));
00193 An = (An + lam)/4;
00194 x0 = (x0 + lam)/4;
00195 y0 = (y0 + lam)/4;
00196 z0 = (z0 + lam)/4;
00197 mul *= 4;
00198 }
00199 real
00200 X = (A0 - x) / (mul * An),
00201 Y = (A0 - y) / (mul * An),
00202 Z = -(X + Y) / 3,
00203 E2 = X*Y - 6*Z*Z,
00204 E3 = (3*X*Y - 8*Z*Z)*Z,
00205 E4 = 3 * (X*Y - Z*Z) * Z*Z,
00206 E5 = X*Y*Z*Z*Z;
00207
00208
00209
00210
00211
00212 return ((471240 - 540540 * E2) * E5 +
00213 (612612 * E2 - 540540 * E3 - 556920) * E4 +
00214 E3 * (306306 * E3 + E2 * (675675 * E2 - 706860) + 680680) +
00215 E2 * ((417690 - 255255 * E2) * E2 - 875160) + 4084080) /
00216 (4084080 * mul * An * sqrt(An)) + 3 * s;
00217 }
00218
00219 void EllipticFunction::Reset(real k2, real alpha2,
00220 real kp2, real alphap2) {
00221 _k2 = k2;
00222 _kp2 = kp2;
00223 _alpha2 = alpha2;
00224 _alphap2 = alphap2;
00225 _eps = _k2/Math::sq(sqrt(_kp2) + 1);
00226 if (_k2) {
00227
00228
00229 _Kc = _kp2 ? RF(_kp2, 1) : Math::infinity();
00230
00231
00232 _Ec = _kp2 ? 2 * RG(_kp2, 1) : 1;
00233
00234
00235 _Dc = _kp2 ? RD(real(0), _kp2, 1) / 3 : Math::infinity();
00236 } else {
00237 _Kc = _Ec = Math::pi()/2; _Dc = _Kc/2;
00238 }
00239 if (_alpha2) {
00240
00241 real rj = _kp2 ? RJ(0, _kp2, 1, _alphap2) : Math::infinity();
00242
00243 _Pic = _Kc + _alpha2 * rj / 3;
00244
00245 _Gc = _kp2 ? _Kc + (_alpha2 - _k2) * rj / 3 : RC(1, _alphap2);
00246
00247 _Hc = _kp2 ? _Kc - _alphap2 * rj / 3 : RC(1, _alphap2);
00248 } else {
00249 _Pic = _Kc; _Gc = _Ec; _Hc = _Kc - _Dc;
00250 }
00251 }
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261 void EllipticFunction::sncndn(real x, real& sn, real& cn, real& dn)
00262 const {
00263
00264 real tolJAC = sqrt(numeric_limits<real>::epsilon() * real(0.01));
00265 if (_kp2 != 0) {
00266 real mc = _kp2, d = 0;
00267 if (_kp2 < 0) {
00268 d = 1 - mc;
00269 mc /= -d;
00270 d = sqrt(d);
00271 x *= d;
00272 }
00273 real c = 0;
00274 real m[num_], n[num_];
00275 unsigned l = 0;
00276 for (real a = 1; l < num_ || GEOGRAPHICLIB_PANIC; ++l) {
00277
00278 m[l] = a;
00279 n[l] = mc = sqrt(mc);
00280 c = (a + mc) / 2;
00281 if (!(abs(a - mc) > tolJAC * a)) {
00282 ++l;
00283 break;
00284 }
00285 mc *= a;
00286 a = c;
00287 }
00288 x *= c;
00289 sn = sin(x);
00290 cn = cos(x);
00291 dn = 1;
00292 if (sn != 0) {
00293 real a = cn / sn;
00294 c *= a;
00295 while (l--) {
00296 real b = m[l];
00297 a *= c;
00298 c *= dn;
00299 dn = (n[l] + a) / (b + a);
00300 a = c / b;
00301 }
00302 a = 1 / sqrt(c*c + 1);
00303 sn = sn < 0 ? -a : a;
00304 cn = c * sn;
00305 if (_kp2 < 0) {
00306 swap(cn, dn);
00307 sn /= d;
00308 }
00309 }
00310 } else {
00311 sn = tanh(x);
00312 dn = cn = 1 / cosh(x);
00313 }
00314 }
00315
00316 Math::real EllipticFunction::F(real sn, real cn, real dn) const {
00317
00318
00319 real fi = abs(sn) * RF(cn*cn, dn*dn, 1);
00320
00321 if (cn < 0)
00322 fi = 2 * K() - fi;
00323 if (sn < 0)
00324 fi = -fi;
00325 return fi;
00326 }
00327
00328 Math::real EllipticFunction::E(real sn, real cn, real dn) const {
00329 real
00330 cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
00331 ei = ( _k2 <= 0 ?
00332
00333
00334 RF(cn2, dn2, 1) - _k2 * sn2 * RD(cn2, dn2, 1) / 3 :
00335 ( _kp2 >= 0 ?
00336
00337 _kp2 * RF(cn2, dn2, 1) +
00338 _k2 * _kp2 * sn2 * RD(cn2, 1, dn2) / 3 +
00339 _k2 * abs(cn) / dn :
00340
00341 - _kp2 * sn2 * RD(dn2, 1, cn2) / 3 + dn / abs(cn) ) );
00342 ei *= abs(sn);
00343
00344 if (cn < 0)
00345 ei = 2 * E() - ei;
00346 if (sn < 0)
00347 ei = -ei;
00348 return ei;
00349 }
00350
00351 Math::real EllipticFunction::D(real sn, real cn, real dn) const {
00352
00353
00354 real di = abs(sn) * sn*sn * RD(cn*cn, dn*dn, 1) / 3;
00355
00356 if (cn < 0)
00357 di = 2 * D() - di;
00358 if (sn < 0)
00359 di = -di;
00360 return di;
00361 }
00362
00363 Math::real EllipticFunction::Pi(real sn, real cn, real dn) const {
00364
00365
00366 real
00367 cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
00368 pii = abs(sn) * (RF(cn2, dn2, 1) +
00369 _alpha2 * sn2 * RJ(cn2, dn2, 1, 1 - _alpha2 * sn2) / 3);
00370
00371 if (cn < 0)
00372 pii = 2 * Pi() - pii;
00373 if (sn < 0)
00374 pii = -pii;
00375 return pii;
00376 }
00377
00378 Math::real EllipticFunction::G(real sn, real cn, real dn) const {
00379 real
00380 cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
00381 gi = abs(sn) * (RF(cn2, dn2, 1) +
00382 (_alpha2 - _k2) * sn2 *
00383 RJ(cn2, dn2, 1, cn2 + _alphap2 * sn2) / 3);
00384
00385 if (cn < 0)
00386 gi = 2 * G() - gi;
00387 if (sn < 0)
00388 gi = -gi;
00389 return gi;
00390 }
00391
00392 Math::real EllipticFunction::H(real sn, real cn, real dn) const {
00393 real
00394 cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
00395 hi = abs(sn) * (RF(cn2, dn2, 1) -
00396 _alphap2 * sn2 *
00397 RJ(cn2, dn2, 1, cn2 + _alphap2 * sn2) / 3);
00398
00399 if (cn < 0)
00400 hi = 2 * H() - hi;
00401 if (sn < 0)
00402 hi = -hi;
00403 return hi;
00404 }
00405
00406 Math::real EllipticFunction::deltaF(real sn, real cn, real dn) const {
00407
00408 if (cn < 0) { cn = -cn; sn = -sn; }
00409 return F(sn, cn, dn) * (Math::pi()/2) / K() - atan2(sn, cn);
00410 }
00411
00412 Math::real EllipticFunction::deltaE(real sn, real cn, real dn) const {
00413
00414 if (cn < 0) { cn = -cn; sn = -sn; }
00415 return E(sn, cn, dn) * (Math::pi()/2) / E() - atan2(sn, cn);
00416 }
00417
00418 Math::real EllipticFunction::deltaPi(real sn, real cn, real dn)
00419 const {
00420
00421 if (cn < 0) { cn = -cn; sn = -sn; }
00422 return Pi(sn, cn, dn) * (Math::pi()/2) / Pi() - atan2(sn, cn);
00423 }
00424
00425 Math::real EllipticFunction::deltaD(real sn, real cn, real dn) const {
00426
00427 if (cn < 0) { cn = -cn; sn = -sn; }
00428 return D(sn, cn, dn) * (Math::pi()/2) / D() - atan2(sn, cn);
00429 }
00430
00431 Math::real EllipticFunction::deltaG(real sn, real cn, real dn) const {
00432
00433 if (cn < 0) { cn = -cn; sn = -sn; }
00434 return G(sn, cn, dn) * (Math::pi()/2) / G() - atan2(sn, cn);
00435 }
00436
00437 Math::real EllipticFunction::deltaH(real sn, real cn, real dn) const {
00438
00439 if (cn < 0) { cn = -cn; sn = -sn; }
00440 return H(sn, cn, dn) * (Math::pi()/2) / H() - atan2(sn, cn);
00441 }
00442
00443 Math::real EllipticFunction::F(real phi) const {
00444 real sn = sin(phi), cn = cos(phi);
00445 return (deltaF(sn, cn, Delta(sn, cn)) + phi) * K() / (Math::pi()/2);
00446 }
00447
00448 Math::real EllipticFunction::E(real phi) const {
00449 real sn = sin(phi), cn = cos(phi);
00450 return (deltaE(sn, cn, Delta(sn, cn)) + phi) * E() / (Math::pi()/2);
00451 }
00452
00453 Math::real EllipticFunction::Ed(real ang) const {
00454 real n = ceil(ang/360 - real(0.5));
00455 ang -= 360 * n;
00456 real
00457 phi = ang * Math::degree(),
00458 sn = abs(ang) == 180 ? 0 : sin(phi),
00459 cn = abs(ang) == 90 ? 0 : cos(phi);
00460 return E(sn, cn, Delta(sn, cn)) + 4 * E() * n;
00461 }
00462
00463 Math::real EllipticFunction::Pi(real phi) const {
00464 real sn = sin(phi), cn = cos(phi);
00465 return (deltaPi(sn, cn, Delta(sn, cn)) + phi) * Pi() / (Math::pi()/2);
00466 }
00467
00468 Math::real EllipticFunction::D(real phi) const {
00469 real sn = sin(phi), cn = cos(phi);
00470 return (deltaD(sn, cn, Delta(sn, cn)) + phi) * D() / (Math::pi()/2);
00471 }
00472
00473 Math::real EllipticFunction::G(real phi) const {
00474 real sn = sin(phi), cn = cos(phi);
00475 return (deltaG(sn, cn, Delta(sn, cn)) + phi) * G() / (Math::pi()/2);
00476 }
00477
00478 Math::real EllipticFunction::H(real phi) const {
00479 real sn = sin(phi), cn = cos(phi);
00480 return (deltaH(sn, cn, Delta(sn, cn)) + phi) * H() / (Math::pi()/2);
00481 }
00482
00483 Math::real EllipticFunction::Einv(real x) const {
00484 real tolJAC = sqrt(numeric_limits<real>::epsilon() * real(0.01));
00485 real n = floor(x / (2 * _Ec) + 0.5);
00486 x -= 2 * _Ec * n;
00487
00488 real phi = Math::pi() * x / (2 * _Ec);
00489
00490 phi -= _eps * sin(2 * phi) / 2;
00491 for (int i = 0; i < num_ || GEOGRAPHICLIB_PANIC; ++i) {
00492 real
00493 sn = sin(phi),
00494 cn = cos(phi),
00495 dn = Delta(sn, cn),
00496 err = (E(sn, cn, dn) - x)/dn;
00497 phi = phi - err;
00498 if (abs(err) < tolJAC)
00499 break;
00500 }
00501 return n * Math::pi() + phi;
00502 }
00503
00504 Math::real EllipticFunction::deltaEinv(real stau, real ctau) const {
00505
00506 if (ctau < 0) { ctau = -ctau; stau = -stau; }
00507 real tau = atan2(stau, ctau);
00508 return Einv( tau * E() / (Math::pi()/2) ) - tau;
00509 }
00510
00511 }