GeographicLib  1.43
Geocentric.cpp
Go to the documentation of this file.
1 /**
2  * \file Geocentric.cpp
3  * \brief Implementation for GeographicLib::Geocentric class
4  *
5  * Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
6  * under the MIT/X11 License. For more information, see
7  * http://geographiclib.sourceforge.net/
8  **********************************************************************/
9 
11 
12 namespace GeographicLib {
13 
14  using namespace std;
15 
16  Geocentric::Geocentric(real a, real f)
17  : _a(a)
18  , _f(f <= 1 ? f : 1/f)
19  , _e2(_f * (2 - _f))
20  , _e2m(Math::sq(1 - _f)) // 1 - _e2
21  , _e2a(abs(_e2))
22  , _e4a(Math::sq(_e2))
23  , _maxrad(2 * _a / numeric_limits<real>::epsilon())
24  {
25  if (!(Math::isfinite(_a) && _a > 0))
26  throw GeographicErr("Major radius is not positive");
27  if (!(Math::isfinite(_f) && _f < 1))
28  throw GeographicErr("Minor radius is not positive");
29  }
30 
32  static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
33  return wgs84;
34  }
35 
36  void Geocentric::IntForward(real lat, real lon, real h,
37  real& X, real& Y, real& Z,
38  real M[dim2_]) const {
39  lon = Math::AngNormalize(lon);
40  real
41  phi = lat * Math::degree(),
42  lam = lon * Math::degree(),
43  sphi = sin(phi),
44  cphi = abs(lat) == 90 ? 0 : cos(phi),
45  n = _a/sqrt(1 - _e2 * Math::sq(sphi)),
46  slam = lon == -180 ? 0 : sin(lam),
47  clam = abs(lon) == 90 ? 0 : cos(lam);
48  Z = (_e2m * n + h) * sphi;
49  X = (n + h) * cphi;
50  Y = X * slam;
51  X *= clam;
52  if (M)
53  Rotation(sphi, cphi, slam, clam, M);
54  }
55 
56  void Geocentric::IntReverse(real X, real Y, real Z,
57  real& lat, real& lon, real& h,
58  real M[dim2_]) const {
59  real
60  R = Math::hypot(X, Y),
61  slam = R ? Y / R : 0,
62  clam = R ? X / R : 1;
63  h = Math::hypot(R, Z); // Distance to center of earth
64  real sphi, cphi;
65  if (h > _maxrad) {
66  // We really far away (> 12 million light years); treat the earth as a
67  // point and h, above, is an acceptable approximation to the height.
68  // This avoids overflow, e.g., in the computation of disc below. It's
69  // possible that h has overflowed to inf; but that's OK.
70  //
71  // Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
72  R = Math::hypot(X/2, Y/2);
73  slam = R ? (Y/2) / R : 0;
74  clam = R ? (X/2) / R : 1;
75  real H = Math::hypot(Z/2, R);
76  sphi = (Z/2) / H;
77  cphi = R / H;
78  } else if (_e4a == 0) {
79  // Treat the spherical case. Dealing with underflow in the general case
80  // with _e2 = 0 is difficult. Origin maps to N pole same as with
81  // ellipsoid.
82  real H = Math::hypot(h == 0 ? 1 : Z, R);
83  sphi = (h == 0 ? 1 : Z) / H;
84  cphi = R / H;
85  h -= _a;
86  } else {
87  // Treat prolate spheroids by swapping R and Z here and by switching
88  // the arguments to phi = atan2(...) at the end.
89  real
90  p = Math::sq(R / _a),
91  q = _e2m * Math::sq(Z / _a),
92  r = (p + q - _e4a) / 6;
93  if (_f < 0) swap(p, q);
94  if ( !(_e4a * q == 0 && r <= 0) ) {
95  real
96  // Avoid possible division by zero when r = 0 by multiplying
97  // equations for s and t by r^3 and r, resp.
98  S = _e4a * p * q / 4, // S = r^3 * s
99  r2 = Math::sq(r),
100  r3 = r * r2,
101  disc = S * (2 * r3 + S);
102  real u = r;
103  if (disc >= 0) {
104  real T3 = S + r3;
105  // Pick the sign on the sqrt to maximize abs(T3). This minimizes
106  // loss of precision due to cancellation. The result is unchanged
107  // because of the way the T is used in definition of u.
108  T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
109  // N.B. cbrt always returns the real root. cbrt(-8) = -2.
110  real T = Math::cbrt(T3); // T = r * t
111  // T can be zero; but then r2 / T -> 0.
112  u += T + (T ? r2 / T : 0);
113  } else {
114  // T is complex, but the way u is defined the result is real.
115  real ang = atan2(sqrt(-disc), -(S + r3));
116  // There are three possible cube roots. We choose the root which
117  // avoids cancellation. Note that disc < 0 implies that r < 0.
118  u += 2 * r * cos(ang / 3);
119  }
120  real
121  v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
122  // Avoid loss of accuracy when u < 0. Underflow doesn't occur in
123  // e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
124  uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
125  // Need to guard against w going negative due to roundoff in uv - q.
126  w = max(real(0), _e2a * (uv - q) / (2 * v)),
127  // Rearrange expression for k to avoid loss of accuracy due to
128  // subtraction. Division by 0 not possible because uv > 0, w >= 0.
129  k = uv / (sqrt(uv + Math::sq(w)) + w),
130  k1 = _f >= 0 ? k : k - _e2,
131  k2 = _f >= 0 ? k + _e2 : k,
132  d = k1 * R / k2,
133  H = Math::hypot(Z/k1, R/k2);
134  sphi = (Z/k1) / H;
135  cphi = (R/k2) / H;
136  h = (1 - _e2m/k1) * Math::hypot(d, Z);
137  } else { // e4 * q == 0 && r <= 0
138  // This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
139  // (prolate, rotation axis) and the generation of 0/0 in the general
140  // formulas for phi and h. using the general formula and division by 0
141  // in formula for h. So handle this case by taking the limits:
142  // f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
143  // f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
144  real
145  zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
146  xx = sqrt( _f < 0 ? _e4a - p : p ),
147  H = Math::hypot(zz, xx);
148  sphi = zz / H;
149  cphi = xx / H;
150  if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
151  h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
152  }
153  }
154  lat = atan2(sphi, cphi) / Math::degree();
155  lon = Math::atan2d(slam, clam);
156  if (M)
157  Rotation(sphi, cphi, slam, clam, M);
158  }
159 
160  void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
161  real M[dim2_]) {
162  // This rotation matrix is given by the following quaternion operations
163  // qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
164  // or
165  // qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
166  // where
167  // qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
168 
169  // Local X axis (east) in geocentric coords
170  M[0] = -slam; M[3] = clam; M[6] = 0;
171  // Local Y axis (north) in geocentric coords
172  M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
173  // Local Z axis (up) in geocentric coords
174  M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
175  }
176 
177 } // namespace GeographicLib
static T AngNormalize(T x)
Definition: Math.hpp:445
GeographicLib::Math::real real
Definition: GeodSolve.cpp:32
static T cbrt(T x)
Definition: Math.hpp:357
static bool isfinite(T x)
Definition: Math.hpp:614
Mathematical functions needed by GeographicLib.
Definition: Math.hpp:102
Geocentric coordinates
Definition: Geocentric.hpp:67
static T hypot(T x, T y)
Definition: Math.hpp:255
static T sq(T x)
Definition: Math.hpp:244
static const Geocentric & WGS84()
Definition: Geocentric.cpp:31
static T atan2d(T y, T x)
Definition: Math.hpp:551
Namespace for GeographicLib.
Definition: Accumulator.cpp:12
static T degree()
Definition: Math.hpp:228
Header for GeographicLib::Geocentric class.
Exception handling for GeographicLib.
Definition: Constants.hpp:382