src/TransverseMercatorExact.cpp
Go to the documentation of this file.
1 
43 
44 #if defined(_MSC_VER)
45 // Squelch warnings about constant conditional expressions
46 # pragma warning (disable: 4127)
47 #endif
48 
49 namespace GeographicLib {
50 
51  using namespace std;
52 
54  bool extendp)
55  : tol_(numeric_limits<real>::epsilon())
56  , tol2_(real(0.1) * tol_)
57  , taytol_(pow(tol_, real(0.6)))
58  , _a(a)
59  , _f(f)
60  , _k0(k0)
61  , _mu(_f * (2 - _f)) // e^2
62  , _mv(1 - _mu) // 1 - e^2
63  , _e(sqrt(_mu))
64  , _extendp(extendp)
65  , _Eu(_mu)
66  , _Ev(_mv)
67  {
68  if (!(Math::isfinite(_a) && _a > 0))
69  throw GeographicErr("Equatorial radius is not positive");
70  if (!(_f > 0))
71  throw GeographicErr("Flattening is not positive");
72  if (!(_f < 1))
73  throw GeographicErr("Polar semi-axis is not positive");
74  if (!(Math::isfinite(_k0) && _k0 > 0))
75  throw GeographicErr("Scale is not positive");
76  }
77 
82  return utm;
83  }
84 
85  void TransverseMercatorExact::zeta(real /*u*/, real snu, real cnu, real dnu,
86  real /*v*/, real snv, real cnv, real dnv,
87  real& taup, real& lam) const {
88  // Lee 54.17 but write
89  // atanh(snu * dnv) = asinh(snu * dnv / sqrt(cnu^2 + _mv * snu^2 * snv^2))
90  // atanh(_e * snu / dnv) =
91  // asinh(_e * snu / sqrt(_mu * cnu^2 + _mv * cnv^2))
92  // Overflow value s.t. atan(overflow) = pi/2
93  static const real
95  real
96  d1 = sqrt(Math::sq(cnu) + _mv * Math::sq(snu * snv)),
97  d2 = sqrt(_mu * Math::sq(cnu) + _mv * Math::sq(cnv)),
98  t1 = (d1 != 0 ? snu * dnv / d1 : (snu < 0 ? -overflow : overflow)),
99  t2 = (d2 != 0 ? sinh( _e * Math::asinh(_e * snu / d2) ) :
100  (snu < 0 ? -overflow : overflow));
101  // psi = asinh(t1) - asinh(t2)
102  // taup = sinh(psi)
103  taup = t1 * Math::hypot(real(1), t2) - t2 * Math::hypot(real(1), t1);
104  lam = (d1 != 0 && d2 != 0) ?
105  atan2(dnu * snv, cnu * cnv) - _e * atan2(_e * cnu * snv, dnu * cnv) :
106  0;
107  }
108 
110  real snu, real cnu, real dnu,
111  real /*v*/,
112  real snv, real cnv, real dnv,
113  real& du, real& dv) const {
114  // Lee 54.21 but write (1 - dnu^2 * snv^2) = (cnv^2 + _mu * snu^2 * snv^2)
115  // (see A+S 16.21.4)
116  real d = _mv * Math::sq(Math::sq(cnv) + _mu * Math::sq(snu * snv));
117  du = cnu * dnu * dnv * (Math::sq(cnv) - _mu * Math::sq(snu * snv)) / d;
118  dv = -snu * snv * cnv * (Math::sq(dnu * dnv) + _mu * Math::sq(cnu)) / d;
119  }
120 
121  // Starting point for zetainv
123  real& u, real& v) const {
124  bool retval = false;
125  if (psi < -_e * Math::pi()/4 &&
126  lam > (1 - 2 * _e) * Math::pi()/2 &&
127  psi < lam - (1 - _e) * Math::pi()/2) {
128  // N.B. this branch is normally not taken because psi < 0 is converted
129  // psi > 0 by Forward.
130  //
131  // There's a log singularity at w = w0 = Eu.K() + i * Ev.K(),
132  // corresponding to the south pole, where we have, approximately
133  //
134  // psi = _e + i * pi/2 - _e * atanh(cos(i * (w - w0)/(1 + _mu/2)))
135  //
136  // Inverting this gives:
137  real
138  psix = 1 - psi / _e,
139  lamx = (Math::pi()/2 - lam) / _e;
140  u = Math::asinh(sin(lamx) / Math::hypot(cos(lamx), sinh(psix))) *
141  (1 + _mu/2);
142  v = atan2(cos(lamx), sinh(psix)) * (1 + _mu/2);
143  u = _Eu.K() - u;
144  v = _Ev.K() - v;
145  } else if (psi < _e * Math::pi()/2 &&
146  lam > (1 - 2 * _e) * Math::pi()/2) {
147  // At w = w0 = i * Ev.K(), we have
148  //
149  // zeta = zeta0 = i * (1 - _e) * pi/2
150  // zeta' = zeta'' = 0
151  //
152  // including the next term in the Taylor series gives:
153  //
154  // zeta = zeta0 - (_mv * _e) / 3 * (w - w0)^3
155  //
156  // When inverting this, we map arg(w - w0) = [-90, 0] to
157  // arg(zeta - zeta0) = [-90, 180]
158  real
159  dlam = lam - (1 - _e) * Math::pi()/2,
160  rad = Math::hypot(psi, dlam),
161  // atan2(dlam-psi, psi+dlam) + 45d gives arg(zeta - zeta0) in range
162  // [-135, 225). Subtracting 180 (since multiplier is negative) makes
163  // range [-315, 45). Multiplying by 1/3 (for cube root) gives range
164  // [-105, 15). In particular the range [-90, 180] in zeta space maps
165  // to [-90, 0] in w space as required.
166  ang = atan2(dlam-psi, psi+dlam) - real(0.75) * Math::pi();
167  // Error using this guess is about 0.21 * (rad/e)^(5/3)
168  retval = rad < _e * taytol_;
169  rad = Math::cbrt(3 / (_mv * _e) * rad);
170  ang /= 3;
171  u = rad * cos(ang);
172  v = rad * sin(ang) + _Ev.K();
173  } else {
174  // Use spherical TM, Lee 12.6 -- writing atanh(sin(lam) / cosh(psi)) =
175  // asinh(sin(lam) / hypot(cos(lam), sinh(psi))). This takes care of the
176  // log singularity at zeta = Eu.K() (corresponding to the north pole)
177  v = Math::asinh(sin(lam) / Math::hypot(cos(lam), sinh(psi)));
178  u = atan2(sinh(psi), cos(lam));
179  // But scale to put 90,0 on the right place
180  u *= _Eu.K() / (Math::pi()/2);
181  v *= _Eu.K() / (Math::pi()/2);
182  }
183  return retval;
184  }
185 
186  // Invert zeta using Newton's method
188  real& u, real& v) const {
189  real
190  psi = Math::asinh(taup),
191  scal = 1/Math::hypot(real(1), taup);
192  if (zetainv0(psi, lam, u, v))
193  return;
194  real stol2 = tol2_ / Math::sq(max(psi, real(1)));
195  // min iterations = 2, max iterations = 6; mean = 4.0
196  for (int i = 0, trip = 0; i < numit_ || GEOGRAPHICLIB_PANIC; ++i) {
197  real snu, cnu, dnu, snv, cnv, dnv;
198  _Eu.sncndn(u, snu, cnu, dnu);
199  _Ev.sncndn(v, snv, cnv, dnv);
200  real tau1, lam1, du1, dv1;
201  zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau1, lam1);
202  dwdzeta(u, snu, cnu, dnu, v, snv, cnv, dnv, du1, dv1);
203  tau1 -= taup;
204  lam1 -= lam;
205  tau1 *= scal;
206  real
207  delu = tau1 * du1 - lam1 * dv1,
208  delv = tau1 * dv1 + lam1 * du1;
209  u -= delu;
210  v -= delv;
211  if (trip)
212  break;
213  real delw2 = Math::sq(delu) + Math::sq(delv);
214  if (!(delw2 >= stol2))
215  ++trip;
216  }
217  }
218 
219  void TransverseMercatorExact::sigma(real /*u*/, real snu, real cnu, real dnu,
220  real v, real snv, real cnv, real dnv,
221  real& xi, real& eta) const {
222  // Lee 55.4 writing
223  // dnu^2 + dnv^2 - 1 = _mu * cnu^2 + _mv * cnv^2
224  real d = _mu * Math::sq(cnu) + _mv * Math::sq(cnv);
225  xi = _Eu.E(snu, cnu, dnu) - _mu * snu * cnu * dnu / d;
226  eta = v - _Ev.E(snv, cnv, dnv) + _mv * snv * cnv * dnv / d;
227  }
228 
230  real snu, real cnu, real dnu,
231  real /*v*/,
232  real snv, real cnv, real dnv,
233  real& du, real& dv) const {
234  // Reciprocal of 55.9: dw/ds = dn(w)^2/_mv, expanding complex dn(w) using
235  // A+S 16.21.4
236  real d = _mv * Math::sq(Math::sq(cnv) + _mu * Math::sq(snu * snv));
237  real
238  dnr = dnu * cnv * dnv,
239  dni = - _mu * snu * cnu * snv;
240  du = (Math::sq(dnr) - Math::sq(dni)) / d;
241  dv = 2 * dnr * dni / d;
242  }
243 
244  // Starting point for sigmainv
246  real& u, real& v) const {
247  bool retval = false;
248  if (eta > real(1.25) * _Ev.KE() ||
249  (xi < -real(0.25) * _Eu.E() && xi < eta - _Ev.KE())) {
250  // sigma as a simple pole at w = w0 = Eu.K() + i * Ev.K() and sigma is
251  // approximated by
252  //
253  // sigma = (Eu.E() + i * Ev.KE()) + 1/(w - w0)
254  real
255  x = xi - _Eu.E(),
256  y = eta - _Ev.KE(),
257  r2 = Math::sq(x) + Math::sq(y);
258  u = _Eu.K() + x/r2;
259  v = _Ev.K() - y/r2;
260  } else if ((eta > real(0.75) * _Ev.KE() && xi < real(0.25) * _Eu.E())
261  || eta > _Ev.KE()) {
262  // At w = w0 = i * Ev.K(), we have
263  //
264  // sigma = sigma0 = i * Ev.KE()
265  // sigma' = sigma'' = 0
266  //
267  // including the next term in the Taylor series gives:
268  //
269  // sigma = sigma0 - _mv / 3 * (w - w0)^3
270  //
271  // When inverting this, we map arg(w - w0) = [-pi/2, -pi/6] to
272  // arg(sigma - sigma0) = [-pi/2, pi/2]
273  // mapping arg = [-pi/2, -pi/6] to [-pi/2, pi/2]
274  real
275  deta = eta - _Ev.KE(),
276  rad = Math::hypot(xi, deta),
277  // Map the range [-90, 180] in sigma space to [-90, 0] in w space. See
278  // discussion in zetainv0 on the cut for ang.
279  ang = atan2(deta-xi, xi+deta) - real(0.75) * Math::pi();
280  // Error using this guess is about 0.068 * rad^(5/3)
281  retval = rad < 2 * taytol_;
282  rad = Math::cbrt(3 / _mv * rad);
283  ang /= 3;
284  u = rad * cos(ang);
285  v = rad * sin(ang) + _Ev.K();
286  } else {
287  // Else use w = sigma * Eu.K/Eu.E (which is correct in the limit _e -> 0)
288  u = xi * _Eu.K()/_Eu.E();
289  v = eta * _Eu.K()/_Eu.E();
290  }
291  return retval;
292  }
293 
294  // Invert sigma using Newton's method
296  real& u, real& v) const {
297  if (sigmainv0(xi, eta, u, v))
298  return;
299  // min iterations = 2, max iterations = 7; mean = 3.9
300  for (int i = 0, trip = 0; i < numit_ || GEOGRAPHICLIB_PANIC; ++i) {
301  real snu, cnu, dnu, snv, cnv, dnv;
302  _Eu.sncndn(u, snu, cnu, dnu);
303  _Ev.sncndn(v, snv, cnv, dnv);
304  real xi1, eta1, du1, dv1;
305  sigma(u, snu, cnu, dnu, v, snv, cnv, dnv, xi1, eta1);
306  dwdsigma(u, snu, cnu, dnu, v, snv, cnv, dnv, du1, dv1);
307  xi1 -= xi;
308  eta1 -= eta;
309  real
310  delu = xi1 * du1 - eta1 * dv1,
311  delv = xi1 * dv1 + eta1 * du1;
312  u -= delu;
313  v -= delv;
314  if (trip)
315  break;
316  real delw2 = Math::sq(delu) + Math::sq(delv);
317  if (!(delw2 >= tol2_))
318  ++trip;
319  }
320  }
321 
323  real snu, real cnu, real dnu,
324  real snv, real cnv, real dnv,
325  real& gamma, real& k) const {
326  real sec2 = 1 + Math::sq(tau); // sec(phi)^2
327  // Lee 55.12 -- negated for our sign convention. gamma gives the bearing
328  // (clockwise from true north) of grid north
329  gamma = atan2(_mv * snu * snv * cnv, cnu * dnu * dnv);
330  // Lee 55.13 with nu given by Lee 9.1 -- in sqrt change the numerator
331  // from
332  //
333  // (1 - snu^2 * dnv^2) to (_mv * snv^2 + cnu^2 * dnv^2)
334  //
335  // to maintain accuracy near phi = 90 and change the denomintor from
336  //
337  // (dnu^2 + dnv^2 - 1) to (_mu * cnu^2 + _mv * cnv^2)
338  //
339  // to maintain accuracy near phi = 0, lam = 90 * (1 - e). Similarly
340  // rewrite sqrt term in 9.1 as
341  //
342  // _mv + _mu * c^2 instead of 1 - _mu * sin(phi)^2
343  k = sqrt(_mv + _mu / sec2) * sqrt(sec2) *
344  sqrt( (_mv * Math::sq(snv) + Math::sq(cnu * dnv)) /
345  (_mu * Math::sq(cnu) + _mv * Math::sq(cnv)) );
346  }
347 
349  real& x, real& y,
350  real& gamma, real& k) const {
351  lat = Math::LatFix(lat);
352  lon = Math::AngDiff(lon0, lon);
353  // Explicitly enforce the parity
354  int
355  latsign = (!_extendp && lat < 0) ? -1 : 1,
356  lonsign = (!_extendp && lon < 0) ? -1 : 1;
357  lon *= lonsign;
358  lat *= latsign;
359  bool backside = !_extendp && lon > 90;
360  if (backside) {
361  if (lat == 0)
362  latsign = -1;
363  lon = 180 - lon;
364  }
365  real
366  lam = lon * Math::degree(),
367  tau = Math::tand(lat);
368 
369  // u,v = coordinates for the Thompson TM, Lee 54
370  real u, v;
371  if (lat == 90) {
372  u = _Eu.K();
373  v = 0;
374  } else if (lat == 0 && lon == 90 * (1 - _e)) {
375  u = 0;
376  v = _Ev.K();
377  } else
378  // tau = tan(phi), taup = sinh(psi)
379  zetainv(Math::taupf(tau, _e), lam, u, v);
380 
381  real snu, cnu, dnu, snv, cnv, dnv;
382  _Eu.sncndn(u, snu, cnu, dnu);
383  _Ev.sncndn(v, snv, cnv, dnv);
384 
385  real xi, eta;
386  sigma(u, snu, cnu, dnu, v, snv, cnv, dnv, xi, eta);
387  if (backside)
388  xi = 2 * _Eu.E() - xi;
389  y = xi * _a * _k0 * latsign;
390  x = eta * _a * _k0 * lonsign;
391 
392  if (lat == 90) {
393  gamma = lon;
394  k = 1;
395  } else {
396  // Recompute (tau, lam) from (u, v) to improve accuracy of Scale
397  zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau, lam);
398  tau = Math::tauf(tau, _e);
399  Scale(tau, lam, snu, cnu, dnu, snv, cnv, dnv, gamma, k);
400  gamma /= Math::degree();
401  }
402  if (backside)
403  gamma = 180 - gamma;
404  gamma *= latsign * lonsign;
405  k *= _k0;
406  }
407 
409  real& lat, real& lon,
410  real& gamma, real& k) const {
411  // This undoes the steps in Forward.
412  real
413  xi = y / (_a * _k0),
414  eta = x / (_a * _k0);
415  // Explicitly enforce the parity
416  int
417  latsign = !_extendp && y < 0 ? -1 : 1,
418  lonsign = !_extendp && x < 0 ? -1 : 1;
419  xi *= latsign;
420  eta *= lonsign;
421  bool backside = !_extendp && xi > _Eu.E();
422  if (backside)
423  xi = 2 * _Eu.E()- xi;
424 
425  // u,v = coordinates for the Thompson TM, Lee 54
426  real u, v;
427  if (xi == 0 && eta == _Ev.KE()) {
428  u = 0;
429  v = _Ev.K();
430  } else
431  sigmainv(xi, eta, u, v);
432 
433  real snu, cnu, dnu, snv, cnv, dnv;
434  _Eu.sncndn(u, snu, cnu, dnu);
435  _Ev.sncndn(v, snv, cnv, dnv);
436  real phi, lam, tau;
437  if (v != 0 || u != _Eu.K()) {
438  zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau, lam);
439  tau = Math::tauf(tau, _e);
440  phi = atan(tau);
441  lat = phi / Math::degree();
442  lon = lam / Math::degree();
443  Scale(tau, lam, snu, cnu, dnu, snv, cnv, dnv, gamma, k);
444  gamma /= Math::degree();
445  } else {
446  lat = 90;
447  lon = lam = gamma = 0;
448  k = 1;
449  }
450 
451  if (backside)
452  lon = 180 - lon;
453  lon *= lonsign;
454  lon = Math::AngNormalize(lon + Math::AngNormalize(lon0));
455  lat *= latsign;
456  if (backside)
457  gamma = 180 - gamma;
458  gamma *= latsign * lonsign;
459  k *= _k0;
460  }
461 
462 } // namespace GeographicLib
static T AngNormalize(T x)
Definition: Math.hpp:440
#define max(a, b)
Definition: datatypes.h:20
static T pi()
Definition: Math.hpp:202
void sncndn(real x, real &sn, real &cn, real &dn) const
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
static const TransverseMercatorExact & UTM()
Scalar * y
An exact implementation of the transverse Mercator projection.
static const double lat
void sigmainv(real xi, real eta, real &u, real &v) const
static T cbrt(T x)
Definition: Math.hpp:345
static bool isfinite(T x)
Definition: Math.hpp:806
static T LatFix(T x)
Definition: Math.hpp:467
void dwdzeta(real u, real snu, real cnu, real dnu, real v, real snv, real cnv, real dnv, real &du, real &dv) const
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Definition: BFloat16.h:88
Header for GeographicLib::TransverseMercatorExact class.
static T AngDiff(T x, T y, T &e)
Definition: Math.hpp:486
void Reverse(real lon0, real x, real y, real &lat, real &lon, real &gamma, real &k) const
EIGEN_DEVICE_FUNC const SinhReturnType sinh() const
bool sigmainv0(real xi, real eta, real &u, real &v) const
static double epsilon
Definition: testRot3.cpp:37
static T asinh(T x)
Definition: Math.hpp:311
EIGEN_DEVICE_FUNC const AtanReturnType atan() const
static T hypot(T x, T y)
Definition: Math.hpp:243
TransverseMercatorExact(real a, real f, real k0, bool extendp=false)
void Scale(real tau, real lam, real snu, real cnu, real dnu, real snv, real cnv, real dnv, real &gamma, real &k) const
static T sq(T x)
Definition: Math.hpp:232
bool zetainv0(real psi, real lam, real &u, real &v) const
void sigma(real u, real snu, real cnu, real dnu, real v, real snv, real cnv, real dnv, real &xi, real &eta) const
Array< int, Dynamic, 1 > v
void zeta(real u, real snu, real cnu, real dnu, real v, real snv, real cnv, real dnv, real &taup, real &lam) const
Definition: main.h:100
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Namespace for GeographicLib.
static const double r2
const double lon0
static T degree()
Definition: Math.hpp:216
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Vector xi
Definition: testPose2.cpp:148
static T tand(T x)
Definition: Math.hpp:671
void Forward(real lon0, real lat, real lon, real &x, real &y, real &gamma, real &k) const
Exception handling for GeographicLib.
Definition: Constants.hpp:389
static const double lon
void dwdsigma(real u, real snu, real cnu, real dnu, real v, real snv, real cnv, real dnv, real &du, real &dv) const
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
static T tauf(T taup, T es)
Definition: Math.cpp:31
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
static T taupf(T tau, T es)
Definition: Math.cpp:25
#define GEOGRAPHICLIB_PANIC
Definition: Math.hpp:87
void zetainv(real taup, real lam, real &u, real &v) const
int EIGEN_BLAS_FUNC() scal(int *n, RealScalar *palpha, RealScalar *px, int *incx)
Definition: level1_impl.h:117


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:26