// geodetic_task.hpp
pragma once
class GeodeticTask
{
public:
GeodeticTask();
GeodeticTask(double a, double flattening);
bool SolveDirect(double lat1, double lon1, double alpha1, double s, double& lat2, double& lon2, double& alpha2);
bool SolveInverse(double lat1, double lon1, double lat2, double lon2, double& s, double& alpha1, double& alpha2);
private:
static void getAB1(double u2, double& A, double& B);
static void getAB2(double u2, double& A, double& B);
static void getAB(double u2, double& A, double& B);
static double getC(double f, double cos2Alpha);
static double getDeltaSigma(double B, double sin_sigma, double cos_sigma, double cos_2sigma_m, double cos2_2sigma_m);
static double get_u2(double cos2_alpha, double a, double b);
static double get_lambda_L_term(double C, double f, double sin_alpha, double sigma, double sin_sigma, double cos_sigma, double cos_2sigma_m, double cos2_2sigma_m);
private:
const double a;
const double f;
const double b;
const double TOL;
};
// geodetic_task.cpp
#define _USE_MATH_DEFINES
#include <math.h>
#include <float.h>
#include "geodetic_task.hpp"
const double DEG_TO_RAD = M_PI / 180;
const double RAD_TO_DEG = 180 / M_PI;
static double norm_base(double base, double val)
{
if (val < 0)
{
return fmod(val, base) + base;
}
else if (val < base)
{
return val;
}
else // >= base
{
return fmod(val, base);
}
}
static double wrap360(double deg)
{
return norm_base(360, deg);
}
GeodeticTask::GeodeticTask() // WGS84 datum
: a(6378137.0)
, f(1 / 298.257223563)
, b((1 - f) * a)
, TOL(1e-12)
{}
GeodeticTask::GeodeticTask(double _a, double _f)
: a(_a)
, f(_f)
, b((1 - f) * a)
, TOL(1e-12)
{}
void GeodeticTask::getAB1(double u2, double& A, double& B)
{
A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)));
B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)));
}
void GeodeticTask::getAB2(double u2, double& A, double& B)
{
const double sqrt_u2 = sqrt(1 + u2 * u2);
const double k1 = (sqrt_u2 - 1) / (sqrt_u2 + 1);
A = (1 + k1 * k1 / 4) / (1 - k1);
B = k1 * (1 - 3 * k1 * k1 / 8);
}
void GeodeticTask::getAB(double u2, double& A, double& B)
{
GeodeticTask::getAB1(u2, A, B);
}
double GeodeticTask::getC(double f, double cos2Alpha)
{
return f / 16 * cos2Alpha * (4 + f * (4 - 3 * cos2Alpha));
}
double GeodeticTask::getDeltaSigma(double B, double sin_sigma, double cos_sigma, double cos_2sigma_m, double cos2_2sigma_m)
{
return B * sin_sigma * (cos_2sigma_m + B / 4 * (cos_sigma * (-1 + 2 * cos2_2sigma_m) - B / 6 * cos_2sigma_m * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * cos2_2sigma_m)));
};
double GeodeticTask::get_u2(double cos2_alpha, double a, double b)
{
return cos2_alpha * ((a * a) / (b * b) - 1);
}
double GeodeticTask::get_lambda_L_term(double C, double f, double sin_alpha, double sigma, double sin_sigma, double cos_sigma, double cos_2sigma_m, double cos2_2sigma_m)
{
return (1 - C) * f * sin_alpha * (sigma + C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos2_2sigma_m)));
}
bool GeodeticTask::SolveInverse(double lat1, double lon1, double lat2, double lon2, double& s, double& alpha1, double& alpha2)
{
const double phi1 = lat1 * DEG_TO_RAD;
const double phi2 = lat2 * DEG_TO_RAD;
const double L1 = lon1 * DEG_TO_RAD;
const double L2 = lon2 * DEG_TO_RAD;
const double L = L2 - L1;
const bool isAntipodal = (fabs(L) > M_PI_2) || (fabs(phi2 - phi1) > M_PI_2);
const double tanU1 = (1 - f) * tan(phi1);
const double tanU2 = (1 - f) * tan(phi2);
const double cosU1 = 1 / sqrt(1 + tanU1 * tanU1);
const double cosU2 = 1 / sqrt(1 + tanU2 * tanU2);
const double sinU1 = tanU1 * cosU1;
const double sinU2 = tanU2 * cosU2;
const double cosU1sinU2 = cosU1 * sinU2;
const double sinU1cosU2 = sinU1 * cosU2;
double sin_lambda = 0;
double cos_lambda = 0;
double sigma = isAntipodal ? M_PI : 0;
double sin_sigma = 0;
double cos_sigma = isAntipodal ? -1 : 1;
double sin2_sigma = 0;
double cos_2sigma_m = 1;
double cos2_2sigma_m = 1;
double sin_alpha = 0;
double cos2_alpha = 1;
double C = 0;
double p = 0, q = 0;
double lambda = L;
bool foundSolution = false;
const int MAX_ITER = 100;
for (int i = 0; i < MAX_ITER; ++i)
{
sin_lambda = sin(lambda);
cos_lambda = cos(lambda);
p = cosU2 * sin_lambda;
q = cosU1sinU2 - sinU1cosU2 * cos_lambda;
sin2_sigma = p * p + q * q;
if (sin2_sigma < DBL_EPSILON)
break;
sin_sigma = sqrt(sin2_sigma);
cos_sigma = sinU1 * sinU2 + cosU1 * cosU2 * cos_lambda;
sigma = atan2(sin_sigma, cos_sigma);
sin_alpha = cosU1 * cosU2 * sin_lambda / sin_sigma;
cos2_alpha = 1 - sin_alpha * sin_alpha;
cos_2sigma_m = (0 == cos2_alpha) ? 0 : cos_sigma - 2 * sinU1 * sinU2 / cos2_alpha;
cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m;
C = getC(f, cos2_alpha);
double lambda_ = lambda;
lambda = L + get_lambda_L_term(C, f, sin_alpha, sigma, sin_sigma, cos_sigma, cos_2sigma_m, cos2_2sigma_m);
const double antipodalCheck = fabs(lambda) - (isAntipodal ? M_PI : 0);
if (antipodalCheck > M_PI) // lambda > pi
break;
if (fabs(lambda - lambda_) <= TOL)
{
foundSolution = true;
break;
}
}
if (!foundSolution)
return false;
const double u2 = get_u2(cos2_alpha, a, b);
double A = 0, B = 0;
getAB(u2, A, B);
const double delta_sigma = getDeltaSigma(B, sin_sigma, cos_sigma, cos_2sigma_m, cos2_2sigma_m);
s = b * A * (sigma - delta_sigma);
if (fabs(s) < DBL_EPSILON)
{
alpha1 = NAN;
alpha2 = NAN;
}
else if (fabs(sin2_sigma) < DBL_EPSILON)
{
alpha1 = 0;
alpha2 = 180;
}
else
{
alpha1 = RAD_TO_DEG * atan2(p, q);
alpha2 = RAD_TO_DEG * atan2(cosU1 * sin_lambda, -sinU1cosU2 + cosU1sinU2 * cos_lambda);
alpha1 = wrap360(alpha1);
alpha2 = wrap360(alpha2);
}
return true;
}
bool GeodeticTask::SolveDirect(double lat1, double lon1, double alpha1, double s, double& lat2, double& lon2, double& alpha2)
{
const double phi1 = lat1 * DEG_TO_RAD;
const double tanU1 = (1 - f) * tan(phi1);
const double cosU1 = 1 / sqrt(1 + tanU1 * tanU1);
const double sinU1 = tanU1 * cosU1;
const double sin_alpha1 = sin(alpha1 * DEG_TO_RAD);
const double cos_alpha1 = cos(alpha1 * DEG_TO_RAD);
const double sigma1 = atan2(tanU1, cos_alpha1);
const double sin_alpha = cosU1 * sin_alpha1;
const double sin2_alpha = sin_alpha * sin_alpha;
const double cos2_alpha = 1 - sin2_alpha;
const double u2 = get_u2(cos2_alpha, a, b);
double A = 0, B = 0;
getAB(u2, A, B);
const double sigma0 = s / (b * A);
double sigma = sigma0;
double sin_sigma = 0;
double cos_sigma = 0;
double delta_sigma = 0;
double cos_2sigma_m = 0;
double cos2_2sigma_m = 0;
bool foundSolution = false;
const int MAX_ITER = 100;
for (int i = 0; i < MAX_ITER; ++i)
{
cos_2sigma_m = cos(2 * sigma1 + sigma);
cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m;
cos_sigma = cos(sigma);
sin_sigma = sin(sigma);
delta_sigma = getDeltaSigma(B, sin_sigma, cos_sigma, cos_2sigma_m, cos2_2sigma_m);
double sigma_ = sigma;
sigma = sigma0 + delta_sigma;
if (fabs(sigma - sigma_) <= TOL)
{
foundSolution = true;
break;
}
}
if (!foundSolution)
return false;
const double x = sinU1 * sin_sigma - cosU1 * cos_sigma * cos_alpha1;
const double phi2 = atan2(sinU1 * cos_sigma + cosU1 * sin_sigma * cos_alpha1, (1 - f) * sqrt(sin2_alpha + x * x));
const double lambda = atan2(sin_sigma * sin_alpha1, cosU1 * cos_sigma - sinU1 * sin_sigma * cos_alpha1);
const double C = getC(f, cos2_alpha);
const double L = lambda - get_lambda_L_term(C, f, sin_alpha, sigma, sin_sigma, cos_sigma, cos_2sigma_m, cos2_2sigma_m);
lat2 = phi2 * RAD_TO_DEG;
lon2 = lon1 + RAD_TO_DEG * L;
alpha2 = atan2(sin_alpha, -x) * RAD_TO_DEG;
alpha2 = wrap360(alpha2);
return true;
}
// main.cpp
#include <iostream>
#include <iomanip>
#include <vector>
#include "geodetic_task.hpp"
std::vector<std::pair<double, double> > getPoints(double lat, double lon, double s, std::size_t N)
{
std::vector<std::pair<double, double>> points;
points.reserve(N);
GeodeticTask geodeticTask;
for (std::size_t i = 0; i < N; ++i)
{
double alpha = i * 360 / N;
double lat2 = 0, lon2 = 0, alpha2 = 0;
if (geodeticTask.SolveDirect(lat, lon, alpha, s, lat2, lon2, alpha2))
{
points.push_back(std::make_pair(lat2, lon2));
}
}
return points;
}
int main()
{
double lat = 55.754755;
double lon = 37.620584;
double s = 1000;
std::size_t N = 8;
std::vector<std::pair<double, double>> points = getPoints(lat, lon, s, N);
for (std::size_t i = 0; i < points.size(); ++i)
{
std::cout
<< std::setprecision(8)
<< std::fixed
<< points[i].first << " " << points[i].second
<< std::endl;
}
return 0;
}
55.76373670 37.62058400
55.76110551 37.63184777
55.75475397 37.63651079
55.74840345 37.63184411
55.74577328 37.62058400
55.74840345 37.60932389
55.75475397 37.60465721
55.76110551 37.60932023