geographiclib 0.0.1

data/ext/geographiclib/MagneticCircle.cpp

@@ -0,0 +1,52 @@
+/**
+ * \file MagneticCircle.cpp
+ * \brief Implementation for GeographicLib::MagneticCircle class
+ *
+ * Copyright (c) Charles Karney (2011-2015) <charles@karney.com> and licensed
+ * under the MIT/X11 License.  For more information, see
+ * http://geographiclib.sourceforge.net/
+ **********************************************************************/
+
+#include <GeographicLib/MagneticCircle.hpp>
+#include <fstream>
+#include <sstream>
+#include <GeographicLib/Geocentric.hpp>
+
+namespace GeographicLib {
+
+  using namespace std;
+
+  void MagneticCircle::Field(real lon, bool diffp,
+                             real& Bx, real& By, real& Bz,
+                             real& Bxt, real& Byt, real& Bzt) const {
+    real slam, clam;
+    Math::sincosd(lon, slam, clam);
+    real M[Geocentric::dim2_];
+    Geocentric::Rotation(_sphi, _cphi, slam, clam, M);
+    real BX0, BY0, BZ0, BX1, BY1, BZ1; // Components in geocentric basis
+    real BXc = 0, BYc = 0, BZc = 0;
+    _circ0(slam, clam, BX0, BY0, BZ0);
+    _circ1(slam, clam, BX1, BY1, BZ1);
+    if (_constterm)
+      _circ2(slam, clam, BXc, BYc, BZc);
+    if (_interpolate) {
+      BX1 = (BX1 - BX0) / _dt0;
+      BY1 = (BY1 - BY0) / _dt0;
+      BZ1 = (BZ1 - BZ0) / _dt0;
+    }
+    BX0 += _t1 * BX1 + BXc;
+    BY0 += _t1 * BY1 + BYc;
+    BZ0 += _t1 * BZ1 + BZc;
+    if (diffp) {
+      Geocentric::Unrotate(M, BX1, BY1, BZ1, Bxt, Byt, Bzt);
+      Bxt *= - _a;
+      Byt *= - _a;
+      Bzt *= - _a;
+    }
+    Geocentric::Unrotate(M, BX0, BY0, BZ0, Bx, By, Bz);
+    Bx *= - _a;
+    By *= - _a;
+    Bz *= - _a;
+  }
+
+} // namespace GeographicLib

data/ext/geographiclib/MagneticModel.cpp

@@ -0,0 +1,269 @@
+/**
+ * \file MagneticModel.cpp
+ * \brief Implementation for GeographicLib::MagneticModel class
+ *
+ * Copyright (c) Charles Karney (2011-2015) <charles@karney.com> and licensed
+ * under the MIT/X11 License.  For more information, see
+ * http://geographiclib.sourceforge.net/
+ **********************************************************************/
+
+#include <GeographicLib/MagneticModel.hpp>
+#include <fstream>
+#include <GeographicLib/SphericalEngine.hpp>
+#include <GeographicLib/MagneticCircle.hpp>
+#include <GeographicLib/Utility.hpp>
+
+#if !defined(GEOGRAPHICLIB_DATA)
+#  if defined(_WIN32)
+#    define GEOGRAPHICLIB_DATA "C:/ProgramData/GeographicLib"
+#  else
+#    define GEOGRAPHICLIB_DATA "/usr/local/share/GeographicLib"
+#  endif
+#endif
+
+#if !defined(GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME)
+#  define GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME "wmm2015"
+#endif
+
+#if defined(_MSC_VER)
+// Squelch warnings about unsafe use of getenv
+#  pragma warning (disable: 4996)
+#endif
+
+namespace GeographicLib {
+
+  using namespace std;
+
+  MagneticModel::MagneticModel(const std::string& name,const std::string& path,
+                               const Geocentric& earth)
+    : _name(name)
+    , _dir(path)
+    , _description("NONE")
+    , _date("UNKNOWN")
+    , _t0(Math::NaN())
+    , _dt0(1)
+    , _tmin(Math::NaN())
+    , _tmax(Math::NaN())
+    , _a(Math::NaN())
+    , _hmin(Math::NaN())
+    , _hmax(Math::NaN())
+    , _Nmodels(1)
+    , _Nconstants(0)
+    , _norm(SphericalHarmonic::SCHMIDT)
+    , _earth(earth)
+  {
+    if (_dir.empty())
+      _dir = DefaultMagneticPath();
+    ReadMetadata(_name);
+    _G.resize(_Nmodels + 1 + _Nconstants);
+    _H.resize(_Nmodels + 1 + _Nconstants);
+    {
+      string coeff = _filename + ".cof";
+      ifstream coeffstr(coeff.c_str(), ios::binary);
+      if (!coeffstr.good())
+        throw GeographicErr("Error opening " + coeff);
+      char id[idlength_ + 1];
+      coeffstr.read(id, idlength_);
+      if (!coeffstr.good())
+        throw GeographicErr("No header in " + coeff);
+      id[idlength_] = '\0';
+      if (_id != string(id))
+        throw GeographicErr("ID mismatch: " + _id + " vs " + id);
+      for (int i = 0; i < _Nmodels + 1 + _Nconstants; ++i) {
+        int N, M;
+        SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _G[i], _H[i]);
+        if (!(M < 0 || _G[i][0] == 0))
+          throw GeographicErr("A degree 0 term is not permitted");
+        _harm.push_back(SphericalHarmonic(_G[i], _H[i], N, N, M, _a, _norm));
+      }
+      int pos = int(coeffstr.tellg());
+      coeffstr.seekg(0, ios::end);
+      if (pos != coeffstr.tellg())
+        throw GeographicErr("Extra data in " + coeff);
+    }
+  }
+
+  void MagneticModel::ReadMetadata(const std::string& name) {
+    const char* spaces = " \t\n\v\f\r";
+    _filename = _dir + "/" + name + ".wmm";
+    ifstream metastr(_filename.c_str());
+    if (!metastr.good())
+      throw GeographicErr("Cannot open " + _filename);
+    string line;
+    getline(metastr, line);
+    if (!(line.size() >= 6 && line.substr(0,5) == "WMMF-"))
+      throw GeographicErr(_filename + " does not contain WMMF-n signature");
+    string::size_type n = line.find_first_of(spaces, 5);
+    if (n != string::npos)
+      n -= 5;
+    string version = line.substr(5, n);
+    if (!(version == "1" || version == "2"))
+      throw GeographicErr("Unknown version in " + _filename + ": " + version);
+    string key, val;
+    while (getline(metastr, line)) {
+      if (!Utility::ParseLine(line, key, val))
+        continue;
+      // Process key words
+      if (key == "Name")
+        _name = val;
+      else if (key == "Description")
+        _description = val;
+      else if (key == "ReleaseDate")
+        _date = val;
+      else if (key == "Radius")
+        _a = Utility::num<real>(val);
+      else if (key == "Type") {
+        if (!(val == "Linear" || val == "linear"))
+          throw GeographicErr("Only linear models are supported");
+      } else if (key == "Epoch")
+        _t0 = Utility::num<real>(val);
+      else if (key == "DeltaEpoch")
+        _dt0 = Utility::num<real>(val);
+      else if (key == "NumModels")
+        _Nmodels = Utility::num<int>(val);
+      else if (key == "NumConstants")
+        _Nconstants = Utility::num<int>(val);
+      else if (key == "MinTime")
+        _tmin = Utility::num<real>(val);
+      else if (key == "MaxTime")
+        _tmax = Utility::num<real>(val);
+      else if (key == "MinHeight")
+        _hmin = Utility::num<real>(val);
+      else if (key == "MaxHeight")
+        _hmax = Utility::num<real>(val);
+      else if (key == "Normalization") {
+        if (val == "FULL" || val == "Full" || val == "full")
+          _norm = SphericalHarmonic::FULL;
+        else if (val == "SCHMIDT" || val == "Schmidt" || val == "schmidt")
+          _norm = SphericalHarmonic::SCHMIDT;
+        else
+          throw GeographicErr("Unknown normalization " + val);
+      } else if (key == "ByteOrder") {
+        if (val == "Big" || val == "big")
+          throw GeographicErr("Only little-endian ordering is supported");
+        else if (!(val == "Little" || val == "little"))
+          throw GeographicErr("Unknown byte ordering " + val);
+      } else if (key == "ID")
+        _id = val;
+      // else unrecognized keywords are skipped
+    }
+    // Check values
+    if (!(Math::isfinite(_a) && _a > 0))
+      throw GeographicErr("Reference radius must be positive");
+    if (!(_t0 > 0))
+      throw GeographicErr("Epoch time not defined");
+    if (_tmin >= _tmax)
+      throw GeographicErr("Min time exceeds max time");
+    if (_hmin >= _hmax)
+      throw GeographicErr("Min height exceeds max height");
+    if (int(_id.size()) != idlength_)
+      throw GeographicErr("Invalid ID");
+    if (_Nmodels < 1)
+      throw GeographicErr("NumModels must be positive");
+    if (!(_Nconstants == 0 || _Nconstants == 1))
+      throw GeographicErr("NumConstants must be 0 or 1");
+    if (!(_dt0 > 0)) {
+      if (_Nmodels > 1)
+        throw GeographicErr("DeltaEpoch must be positive");
+      else
+        _dt0 = 1;
+    }
+  }
+
+  void MagneticModel::Field(real t, real lat, real lon, real h, bool diffp,
+                            real& Bx, real& By, real& Bz,
+                            real& Bxt, real& Byt, real& Bzt) const {
+    t -= _t0;
+    int n = max(min(int(floor(t / _dt0)), _Nmodels - 1), 0);
+    bool interpolate = n + 1 < _Nmodels;
+    t -= n * _dt0;
+    real X, Y, Z;
+    real M[Geocentric::dim2_];
+    _earth.IntForward(lat, lon, h, X, Y, Z, M);
+    // Components in geocentric basis
+    // initial values to suppress warning
+    real BX0 = 0, BY0 = 0, BZ0 = 0, BX1 = 0, BY1 = 0, BZ1 = 0;
+    real BXc = 0, BYc = 0, BZc = 0;
+    _harm[n](X, Y, Z, BX0, BY0, BZ0);
+    _harm[n + 1](X, Y, Z, BX1, BY1, BZ1);
+    if (_Nconstants)
+      _harm[_Nmodels + 1](X, Y, Z, BXc, BYc, BZc);
+    if (interpolate) {
+      // Convert to a time derivative
+      BX1 = (BX1 - BX0) / _dt0;
+      BY1 = (BY1 - BY0) / _dt0;
+      BZ1 = (BZ1 - BZ0) / _dt0;
+    }
+    BX0 += t * BX1 + BXc;
+    BY0 += t * BY1 + BYc;
+    BZ0 += t * BZ1 + BZc;
+    if (diffp) {
+      Geocentric::Unrotate(M, BX1, BY1, BZ1, Bxt, Byt, Bzt);
+      Bxt *= - _a;
+      Byt *= - _a;
+      Bzt *= - _a;
+    }
+    Geocentric::Unrotate(M, BX0, BY0, BZ0, Bx, By, Bz);
+    Bx *= - _a;
+    By *= - _a;
+    Bz *= - _a;
+  }
+
+  MagneticCircle MagneticModel::Circle(real t, real lat, real h) const {
+    real t1 = t - _t0;
+    int n = max(min(int(floor(t1 / _dt0)), _Nmodels - 1), 0);
+    bool interpolate = n + 1 < _Nmodels;
+    t1 -= n * _dt0;
+    real X, Y, Z, M[Geocentric::dim2_];
+    _earth.IntForward(lat, 0, h, X, Y, Z, M);
+    // Y = 0, cphi = M[7], sphi = M[8];
+
+    return (_Nconstants == 0 ?
+            MagneticCircle(_a, _earth._f, lat, h, t,
+                           M[7], M[8], t1, _dt0, interpolate,
+                           _harm[n].Circle(X, Z, true),
+                           _harm[n + 1].Circle(X, Z, true)) :
+            MagneticCircle(_a, _earth._f, lat, h, t,
+                           M[7], M[8], t1, _dt0, interpolate,
+                           _harm[n].Circle(X, Z, true),
+                           _harm[n + 1].Circle(X, Z, true),
+                           _harm[_Nmodels + 1].Circle(X, Z, true)));
+  }
+
+  void MagneticModel::FieldComponents(real Bx, real By, real Bz,
+                                      real Bxt, real Byt, real Bzt,
+                                      real& H, real& F, real& D, real& I,
+                                      real& Ht, real& Ft,
+                                      real& Dt, real& It) {
+    H = Math::hypot(Bx, By);
+    Ht = H ? (Bx * Bxt + By * Byt) / H : Math::hypot(Bxt, Byt);
+    D = H ? Math::atan2d(Bx, By) : Math::atan2d(Bxt, Byt);
+    Dt = (H ? (By * Bxt - Bx * Byt) / Math::sq(H) : 0) / Math::degree();
+    F = Math::hypot(H, Bz);
+    Ft = F ? (H * Ht + Bz * Bzt) / F : Math::hypot(Ht, Bzt);
+    I = F ? Math::atan2d(-Bz, H) : Math::atan2d(-Bzt, Ht);
+    It = (F ? (Bz * Ht - H * Bzt) / Math::sq(F) : 0) / Math::degree();
+  }
+
+  std::string MagneticModel::DefaultMagneticPath() {
+    string path;
+    char* magneticpath = getenv("GEOGRAPHICLIB_MAGNETIC_PATH");
+    if (magneticpath)
+      path = string(magneticpath);
+    if (!path.empty())
+      return path;
+    char* datapath = getenv("GEOGRAPHICLIB_DATA");
+    if (datapath)
+      path = string(datapath);
+    return (!path.empty() ? path : string(GEOGRAPHICLIB_DATA)) + "/magnetic";
+  }
+
+  std::string MagneticModel::DefaultMagneticName() {
+    string name;
+    char* magneticname = getenv("GEOGRAPHICLIB_MAGNETIC_NAME");
+    if (magneticname)
+      name = string(magneticname);
+    return !name.empty() ? name : string(GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME);
+  }
+
+} // namespace GeographicLib

data/ext/geographiclib/Math.cpp

@@ -0,0 +1,63 @@
+/**
+ * \file Math.cpp
+ * \brief Implementation for GeographicLib::Math class
+ *
+ * Copyright (c) Charles Karney (2015) <charles@karney.com> and licensed
+ * under the MIT/X11 License.  For more information, see
+ * http://geographiclib.sourceforge.net/
+ **********************************************************************/
+
+#include <GeographicLib/Math.hpp>
+
+#if defined(_MSC_VER)
+// Squelch warnings about constant conditional expressions
+#  pragma warning (disable: 4127)
+#endif
+
+namespace GeographicLib {
+
+  using namespace std;
+
+  template<typename T> T Math::eatanhe(T x, T es)  {
+    return es > T(0) ? es * atanh(es * x) : -es * atan(es * x);
+  }
+
+  template<typename T> T Math::taupf(T tau, T es) {
+    T tau1 = hypot(T(1), tau),
+      sig = sinh( eatanhe(tau / tau1, es ) );
+    return hypot(T(1), sig) * tau - sig * tau1;
+  }
+
+  template<typename T> T Math::tauf(T taup, T es) {
+    static const int numit = 5;
+    static const T tol = sqrt(numeric_limits<T>::epsilon()) / T(10);
+    T e2m = T(1) - sq(es),
+      // To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
+      // tau = taup/_e2m as a starting guess.  (This starting guess is the
+      // geocentric latitude which, to first order in the flattening, is equal
+      // to the conformal latitude.)  Only 1 iteration is needed for |lat| <
+      // 3.35 deg, otherwise 2 iterations are needed.  If, instead, tau = taup
+      // is used the mean number of iterations increases to 1.99 (2 iterations
+      // are needed except near tau = 0).
+      tau = taup/e2m,
+      stol = tol * max(T(1), abs(taup));
+    // min iterations = 1, max iterations = 2; mean = 1.94
+    for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
+      T taupa = taupf(tau, es),
+        dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
+        ( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
+      tau += dtau;
+      if (!(abs(dtau) >= stol))
+        break;
+    }
+    return tau;
+  }
+
+  /// \cond SKIP
+  // Instantiate
+  template Math::real Math::eatanhe<Math::real>(Math::real, Math::real);
+  template Math::real Math::taupf<Math::real>(Math::real, Math::real);
+  template Math::real Math::tauf<Math::real>(Math::real, Math::real);
+  /// \endcond
+
+} // namespace GeographicLib

data/ext/geographiclib/NormalGravity.cpp

@@ -0,0 +1,262 @@
+/**
+ * \file NormalGravity.cpp
+ * \brief Implementation for GeographicLib::NormalGravity class
+ *
+ * Copyright (c) Charles Karney (2011-2015) <charles@karney.com> and licensed
+ * under the MIT/X11 License.  For more information, see
+ * http://geographiclib.sourceforge.net/
+ **********************************************************************/
+
+#include <GeographicLib/NormalGravity.hpp>
+
+#if defined(_MSC_VER)
+// Squelch warnings about constant conditional expressions
+#  pragma warning (disable: 4127)
+#endif
+
+namespace GeographicLib {
+
+  using namespace std;
+
+  NormalGravity::NormalGravity(real a, real GM, real omega, real f, real J2)
+    : _a(a)
+    , _GM(GM)
+    , _omega(omega)
+    , _f(f)
+    , _J2(J2)
+    , _omega2(Math::sq(_omega))
+    , _aomega2(Math::sq(_omega * _a))
+    {
+      if (!(Math::isfinite(_a) && _a > 0))
+        throw GeographicErr("Major radius is not positive");
+      if (!(Math::isfinite(_GM) && _GM > 0))
+        throw GeographicErr("Gravitational constant is not positive");
+      if (!(_omega == 0 && _f == 0 && _J2 == 0)) {
+        bool flatp = _f > 0 && Math::isfinite(_f);
+        if (_J2 > 0 && Math::isfinite(_J2) && flatp)
+          throw GeographicErr("Cannot specify both f and J2");
+        if (!(_J2 > 0 && Math::isfinite(_J2)) && !flatp)
+          throw GeographicErr("Must specify one of f and J2");
+        if (!(Math::isfinite(_omega) && _omega != 0))
+          throw GeographicErr("Angular velocity is not non-zero");
+        if (flatp)
+          _J2 = FlatteningToJ2(_a, _GM, _omega, _f);
+        else
+          _f = J2ToFlattening(_a, _GM, _omega, _J2);
+      } // else we have a sphere: omega = f = J2 = 0
+      _e2 = _f * (2 - _f);
+      _ep2 = _e2 / (1 - _e2);
+      _q0 = qf(_ep2);
+      _earth = Geocentric(_a, _f);
+      _b = _a * (1 - _f);
+      _E = _a * sqrt(_e2);      // H+M, Eq 2-54
+      // H+M, Eq 2-61
+      _U0 = _GM / (_E ? _E / atan(sqrt(_ep2)) : _b) + _aomega2 / 3;
+      // The approximate ratio of the centrifugal acceleration (at the equator)
+      // to gravity.
+      _m = _aomega2 * _b / _GM; // H+M, Eq 2-70
+      real
+        Q = _m * (_q0 ? sqrt(_ep2) * qpf(_ep2) / (3 * _q0) : 1),
+        G = (1 - _m - Q / 2);
+      _gammae = _GM / (_a * _b) * G;       // H+M, Eq 2-73
+      _gammap = _GM / (_a * _a) * (1 + Q); // H+M, Eq 2-74
+      // k = b * gammap / (a * gammae) - 1
+      _k = (_m + 3 * Q / 2 - _e2 * (1 + Q)) / G;
+      // f* = (gammap - gammae) / gammae
+      _fstar = (_m + 3 * Q / 2 - _f * (1 + Q)) / G;
+    }
+
+  const NormalGravity& NormalGravity::WGS84() {
+    static const NormalGravity wgs84(Constants::WGS84_a(),
+                                     Constants::WGS84_GM(),
+                                     Constants::WGS84_omega(),
+                                     Constants::WGS84_f(), 0);
+    return wgs84;
+  }
+
+  const NormalGravity& NormalGravity::GRS80() {
+    static const NormalGravity grs80(Constants::GRS80_a(),
+                                     Constants::GRS80_GM(),
+                                     Constants::GRS80_omega(),
+                                     0, Constants::GRS80_J2());
+    return grs80;
+  }
+
+  // (atan(y)-(y-y^3/3+y^5/5))/y^7 (y = sqrt(x)) = -1/7+x/9-x^2/11+x^3/13...
+  Math::real NormalGravity::atan7(real x) {
+    if (abs(x) >= real(0.5)) {
+      real y = sqrt(abs(x)), x2 = Math::sq(x);
+      return ((x > 0 ? atan(y) : Math::atanh(y))- y * (1 - x / 3 + x2 / 5)) /
+        (x * x2 * y);
+    } else {
+      real xn = -1, q = 0;
+      for (int n = 7; ; n += 2) {
+        real qn = q + xn / n;
+        if (qn == q)
+          break;
+        q = qn;
+        xn *= -x;
+      }
+      return q;
+    }
+  }
+
+  // (atan(y)-(y-y^3/3))/y^5 (y = sqrt(x)) = 1/5-x/7+x^2/9-x^3/11...
+  Math::real NormalGravity::atan5(real x)
+  { return 1/real(5) + x * atan7(x); }
+
+  Math::real NormalGravity::qf(real ep2) {
+    // Compute
+    //
+    //   ((1 + 3/e'^2) * atan(e') - 3/e')/2
+    //
+    // See H+M, Eq 2-57, with E/u = e'.  This suffers from two levels of
+    // cancelation.  The e'^-1 and e'^1 terms drop out, so that the leading
+    // term is O(e'^3).  Substitute atan(e') = e' - e'^3/3 + e'^5*atan5(e'^2)
+    return sqrt(ep2) * ep2 * (3 * (3 + ep2) * atan5(ep2) - 1) / 6;
+  }
+
+  Math::real NormalGravity::dq(real ep2) {
+    // Compute d qf(ep2) / d ep2 and substitute
+    // atan(e') = e' - e'^3/3 + e'^5/5 + e'^7*atan7(e'^2)
+    return sqrt(ep2) * (5 - 3 * (1 + ep2) * (1 + 5 * ep2 * atan7(ep2))) /
+      (10 * (1 + ep2));
+  }
+
+  Math::real NormalGravity::qpf(real ep2) {
+    // Compute
+    //
+    //   3*(1 + 1/e'^2) * (1 - atan(e')/e') - 1
+    //
+    // See H+M, Eq 2-67, with E/u = e'.  This suffers from two levels of
+    // cancelation.  The e'^-2 and e'^0 terms drop out, so that the leading
+    // term is O(e'^2).
+    return ep2 * (1 - 3 * (1 + ep2) * atan5(ep2));
+  }
+
+  Math::real NormalGravity::Jn(int n) const {
+    // Note Jn(0) = -1; Jn(2) = _J2; Jn(odd) = 0
+    if (n & 1 || n < 0)
+      return 0;
+    n /= 2;
+    real e2n = 1;            // Perhaps this should just be e2n = pow(-_e2, n);
+    for (int j = n; j--;)
+      e2n *= -_e2;
+    return                      // H+M, Eq 2-92
+      -3 * e2n * ((1 - n) + 5 * n * _J2 / _e2) / ((2 * n + 1) * (2 * n + 3));
+  }
+
+  Math::real NormalGravity::SurfaceGravity(real lat) const {
+    real sphi = Math::sind(Math::LatFix(lat));
+    // H+M, Eq 2-78
+    return _gammae * (1 + _k * Math::sq(sphi)) / sqrt(1 - _e2 * Math::sq(sphi));
+  }
+
+  Math::real NormalGravity::V0(real X, real Y, real Z,
+                               real& GammaX, real& GammaY, real& GammaZ)
+    const {
+    // See H+M, Sec 6-2
+    real
+      p = Math::hypot(X, Y),
+      clam = p ? X/p : 1,
+      slam = p ? Y/p : 0,
+      r = Math::hypot(p, Z),
+      Q = Math::sq(r) - Math::sq(_E),
+      t2 = Math::sq(2 * _E * Z),
+      disc = sqrt(Math::sq(Q) + t2),
+      // This is H+M, Eq 6-8a, but generalized to deal with Q negative
+      // accurately.
+      u = sqrt((Q >= 0 ? (Q + disc) : t2 / (disc - Q)) / 2),
+      uE = Math::hypot(u, _E),
+      // H+M, Eq 6-8b
+      sbet = Z * uE,
+      cbet = p * u,
+      s = Math::hypot(cbet, sbet);
+    cbet = s ? cbet/s : 0;
+    sbet = s ? sbet/s : 1;
+    real
+      invw = uE / Math::hypot(u, _E * sbet), // H+M, Eq 2-63
+      ep = _E/u,
+      ep2 = Math::sq(ep),
+      q = _q0 ? qf(ep2) / _q0 : pow(_a / u, 3),
+      qp = qpf(ep2) / _q0,
+      // H+M, Eqs 2-62 + 6-9, but omitting last (rotational) term .
+      Vres = (_GM / (_E ? _E / atan(_E / u) : u)
+              + _aomega2 * q * (Math::sq(sbet) - 1/real(3)) / 2),
+      // H+M, Eq 6-10
+      gamu = - invw * (_GM
+                       + (_aomega2 * (_q0 ? _E * qp : 3 * q * u)
+                          * (Math::sq(sbet) - 1/real(3)) / 2)) / Math::sq(uE),
+      gamb = _aomega2 * q * sbet * cbet * invw / uE,
+      t = u * invw / uE;
+    // H+M, Eq 6-12
+    GammaX = t * cbet * gamu - invw * sbet * gamb;
+    GammaY = GammaX * slam;
+    GammaX *= clam;
+    GammaZ = invw * sbet * gamu + t * cbet * gamb;
+    return Vres;
+  }
+
+  Math::real NormalGravity::Phi(real X, real Y, real& fX, real& fY)
+    const {
+    fX = _omega2 * X;
+    fY = _omega2 * Y;
+    // N.B. fZ = 0;
+    return _omega2 * (Math::sq(X) + Math::sq(Y)) / 2;
+  }
+
+  Math::real NormalGravity::U(real X, real Y, real Z,
+                              real& gammaX, real& gammaY, real& gammaZ)
+    const {
+    real fX, fY;
+    real Ures = V0(X, Y, Z, gammaX, gammaY, gammaZ) + Phi(X, Y, fX, fY);
+    gammaX += fX;
+    gammaY += fY;
+    return Ures;
+  }
+
+  Math::real NormalGravity::Gravity(real lat, real h,
+                                    real& gammay, real& gammaz)
+    const {
+    real X, Y, Z;
+    real M[Geocentric::dim2_];
+    _earth.IntForward(lat, 0, h, X, Y, Z, M);
+    real gammaX, gammaY, gammaZ,
+      Ures = U(X, Y, Z, gammaX, gammaY, gammaZ);
+    // gammax = M[0] * gammaX + M[3] * gammaY + M[6] * gammaZ;
+    gammay = M[1] * gammaX + M[4] * gammaY + M[7] * gammaZ;
+    gammaz = M[2] * gammaX + M[5] * gammaY + M[8] * gammaZ;
+    return Ures;
+  }
+
+  Math::real NormalGravity::J2ToFlattening(real a, real GM,
+                                           real omega, real J2) {
+    real
+      K = 2 * Math::sq(a * omega) * a / (15 * GM),
+      e2 = 3 * J2;              // See Moritz (1980), p 398.
+    // Solve using Newton's method
+    for (int j = 0; j < maxit_ || GEOGRAPHICLIB_PANIC; ++j) {
+      real e2a = e2,
+        ep2 = e2 / (1 - e2),
+        q0 = qf(ep2),
+        dq0 = dq(ep2) / Math::sq(1 - e2),
+        h = e2 * (1 - sqrt(e2) * K / q0) - 3 * J2,
+        dh = 1 - sqrt(e2) * K * (3 * q0 - 2 * e2 * dq0) / (2 * Math::sq(q0)),
+        de2 = - h / dh;
+      e2 = e2a + de2;
+      if (e2 == e2a)
+        break;
+    }
+    return e2 / (1 + sqrt(1 - e2));
+  }
+
+  Math::real NormalGravity::FlatteningToJ2(real a, real GM,
+                                           real omega, real f) {
+    real
+      K = 2 * Math::sq(a * omega) * a / (15 * GM),
+      e2 = f * (2 - f),
+      q0 = qf(e2 / (1 - e2));
+    return e2 * (1 - K * sqrt(e2) / q0) / 3; // H+M, Eq 2-90
+  }
+
+} // namespace GeographicLib