geo_lest.cpp

#include <cmath>
#include <utility>

// Input: WGS84 longitude and latitude
// Output: x - easting, y - northing in L-EST97 (EPSG:3301)
// Source: http://www.maaamet.ee/rr/geo-lest/files/geo-lest_function_php.txt
std::pair<double, double> geo_lest(const double lon, const double lat) {
  using namespace std;
  constexpr double deg2rad = M_PI / 180.;
  constexpr double a = 6378137.;
  constexpr double L0 = 24. * deg2rad;
  constexpr double FN = 6375000.;
  constexpr double FE = 500000.;
  
  // These are all constants that can be replaced with precomputed values
  // constexpr double RF = 298.257222100883;
  // constexpr double B0 = (57. + 31. / 60. + 3.194148 / 3600.) * deg2rad;
  // constexpr double B1 = (59. + 20. / 60.) * deg2rad;
  // constexpr double B2 = 58. * deg2rad;
  // constexpr double f1 = 1. / RF;
  // constexpr double er = (2. * f1) - (f1 * f1);
  // double e = sqrt(er);
  // double sB0 = sin(B0);
  // double sB1 = sin(B1);
  // double sB2 = sin(B2);
  // double t1 = sqrt(((1. - sB1) / (1. + sB1)) * pow(((1. + e * sB1) / (1. - e * sB1)), e));
  // double t2 = sqrt(((1. - sB2) / (1. + sB2)) * pow(((1. + e * sB2) / (1. - e * sB2)), e));
  // double t0 = sqrt(((1. - sB0) / (1. + sB0)) * pow(((1. + e * sB0) / (1. - e * sB0)), e));
  // double m1 = cos(B1) / sqrt(1. - er * sin(B1) * sin(B1));
  // double m2 = cos(B2) / sqrt(1. - er * sB2 * sB2);
  // double n = (log(m1) - log(m2)) / (log(t1) - log(t2));
  // double FF = m1 / (n * pow(t1, n));
  // double p0 = a * FF * pow(t0, n);

  constexpr double e = 0.081819191042831806992552401425200514495372772216796875;
  constexpr double n = 0.8541758580870759676173520347219891846179962158203125;
  constexpr double FF = 1.798847851401471853449720583739690482616424560546875;
  constexpr double p0 = 4020205.4786977176554501056671142578125;

  double slat = sin(lat * deg2rad);
  double t = sqrt(((1. - slat) / (1. + slat)) * pow(((1. + e * slat) / (1. - e * slat)), e));
  double FII = n * (lon * deg2rad - L0);
  double p = a * FF * pow(t, n);
  double x = p * sin(FII) + FE;
  double y = p0 - p * cos(FII) + FN;
  return {x, y};
}