geo_calculations.c

Go to the documentation of this file.

#include "config.h"
#include "geo_calculations.h"

#include <features.h>
#include <stdio.h>

#include "gpstypes.h"

#ifndef MIN
# define MIN(a,b) (((a) < (b)) ? (a) : (b))
#endif

#ifndef MAX
# define MAX(a,b) (((a) > (b)) ? (a) : (b))
#endif

#define MAX_NUMBER_ITERATIONS (100)

/* if macro USE_MPFR is set, use the mpfr version of the calculation
 * it is not much more accurate, but a can handle small numbers a bit better at the cost of some preformance.
 * Otherwise use the faster calculation which uses long doubles, in almost all cases this should be enough.
 * If you define USE_MPFR, dont forget to link against libmpfr (-lmpfr) instead of libm.
 */

#if defined USE_MPFR && defined HAVE_LIBMPFR

#include <mpfr.h>

#define FORMULA_MPFR
/*
#undef EARTH_RADIUS_IN_METERS
#define EARTH_RADIUS_IN_METERS "6372797.560856"
*/
#ifndef DEFAULT_PREC
#define DEFAULT_PREC 64
#endif

#define _MPFR_DEFAULT_PREC MIN(MAX((DEFAULT_PREC), (MPFR_PREC_MIN)), ((MPFR_PREC_MAX) / 4))

int deg_to_rad_mpfr(mpfr_ptr res, mpfr_srcptr x, mpfr_rnd_t r);
int VincentyDistance_mpfr(mpfr_ptr res, mpfr_srcptr lat1, mpfr_srcptr lng1, mpfr_srcptr lat2, mpfr_srcptr lng2, mpfr_prec_t p, mpfr_rnd_t r);

/* convert degrees to radians, res = x * PI / 180 */
int deg_to_rad_mpfr(mpfr_ptr res, mpfr_srcptr x, mpfr_rnd_t r)
{
  mpfr_set_nan(res);
  mpfr_t pi;
  mpfr_init2(pi, mpfr_get_default_prec() * 2);

  mpfr_const_pi(pi, r);
  mpfr_mul(res, x, pi, r);
  mpfr_div_ui(res, res, 180, r);

  mpfr_free_cache();
  mpfr_clears(pi, (mpfr_ptr)NULL);

  return 0;
}

int VincentyDistance_mpfr(mpfr_ptr res, mpfr_srcptr lat1, mpfr_srcptr lng1, mpfr_srcptr lat2, mpfr_srcptr lng2, mpfr_prec_t p, mpfr_rnd_t r)
{
  mpfr_t a, b, f, fInv, L, U1, U2, sinU1, sinU2, cosU1, cosU2, lambda, lambdaP, sinLambda, cosLambda, sigma, sinSigma, cosSigma, sinAlpha, cosSqAlpha, cos2SigmaM, C, dLambda, lambdaC, uSq, k1, A, B, deltaSigma;
  mpfr_inits2(p, a, b, f, fInv, L, U1, U2, sinU1, sinU2, cosU1, cosU2, lambda, lambdaP, sinLambda, cosLambda, sigma, sinSigma, cosSigma, sinAlpha, cosSqAlpha, cos2SigmaM, C, dLambda, lambdaC, uSq, k1, A, B, deltaSigma, (mpfr_ptr)NULL);
  mpfr_t part1, tmp1, tmp2, tmp3, tmp4;
  mpfr_inits2(p, part1, tmp1, tmp2, tmp3, tmp4, (mpfr_ptr)NULL);

  /* set result to NaN (not a number, until we reach the final calculation */
  mpfr_set_nan(res);

  /* some constants */
  /* precision target, setting this smaller than 1 * 10 ^ -12 does not have much effect */
  mpfr_set_ld(lambdaC, (long double)1e-24, MPFR_RNDA); /* round away from zero (0). what ever precision is choosen, this value is alwas hihger then zero (0) */
  /* radius at equator */
  mpfr_set_ld(a, (long double)6378137.0, r);
  /* flattening of the ellipsoid (fInv is the inverse of this) */
  mpfr_set_ld(fInv, (long double)298.257223563, r);
  mpfr_ui_div(f, 1, fInv, r);

  /* b = (1 - f) * a */
  mpfr_ui_sub(tmp1, 1, f, r);
  mpfr_mul(b, tmp1, a, r);

  /* L = lng2 - lng1 */
  mpfr_sub(L, lng2, lng1, r);

  /*
  U1 = atan((1 - f) * tan(lat1))
  U2 = atan((1 - f) * tan(lat2))
  */
  mpfr_ui_sub(part1, 1, f, r);
  mpfr_tan(tmp1, lat1, r);
  mpfr_mul(U1, part1, tmp1, r);
  mpfr_atan(U1, U1, r);
  mpfr_tan(tmp1, lat2, r);
  mpfr_mul(U2, part1, tmp1, r);
  mpfr_atan(U2, U2, r);
  //mpfr_printf("sin(U1) = %Rf\ncos(U1) = %Rf\n", sinU1, cosU1);
  //mpfr_printf("sin(U2) = %Rf\ncos(U2) = %Rf\n", sinU2, cosU2);

  mpfr_sin(sinU1, U1, r);
  mpfr_sin(sinU2, U2, r);
  mpfr_cos(cosU1, U1, r);
  mpfr_cos(cosU2, U2, r);
  //mpfr_printf("sin(U1) = %Rf\ncos(U1) = %Rf\n", sinU1, cosU1);
  //mpfr_printf("sin(U2) = %Rf\ncos(U2) = %Rf\n", sinU2, cosU2);

    //lambda = L;
  mpfr_set(lambda, L, r);
  //lambdaP = 0;
  mpfr_set_ui(lambdaP, 0, r);

  /* maximum number of iterations, after this amount we return without a result */
  int iterLimit = MAX_NUMBER_ITERATIONS;

  do
  {
    //printf("2\n");
    //sinLambda = sin(lambda);
    //cosLambda = cos(lambda);
    mpfr_sin(sinLambda, lambda, r);
    mpfr_cos(cosLambda, lambda, r);
    //mpfr_printf("sin(lambda) = %Rf\ncos(lambda) = %Rf\n", sinLambda, cosLambda);
    
    // sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
    mpfr_mul(tmp1, cosU2, sinLambda, r);
    //mpfr_printf("cosU2 * sinLambda = %Rf\n", tmp1);
    mpfr_sqr(tmp1, tmp1, r);
    //mpfr_printf("sqr(cosU2 * sinLambda) = %Rf\n", tmp1);
    mpfr_mul(tmp2, cosU1, sinU2, r);
    //mpfr_printf("cosU1 * sinU2 = %Rf\n", tmp2);
    mpfr_mul(tmp3, sinU1, cosU2, r);
    mpfr_mul(tmp3, tmp3, cosLambda, r);
    //mpfr_printf("sinU1 * cosU2 * cosLambda = %Rf\n", tmp3);
    mpfr_sub(tmp2, tmp2, tmp3, r);
    //mpfr_printf("cosU1 * sinU2 - sinU1 * cosU2 * cosLambda = %Rf\n", tmp2);
    mpfr_sqr(tmp2, tmp2, r);
    //mpfr_printf("sqr(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) = %Rf\n", tmp2);
    mpfr_add(tmp3, tmp1, tmp2, r);
    //mpfr_printf("sqr(cosU2 * sinLambda) + sqr(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) = %Rf\n", tmp3);
    mpfr_sqrt(sinSigma, tmp3, r);

    if(mpfr_nan_p(sinSigma) != 0 || mpfr_zero_p(sinSigma) != 0 || mpfr_cmp_ui(sinSigma, 0) == 0) return -1;  // co-incident points

    // cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
    mpfr_mul(tmp1, sinU1, sinU2, r);
    mpfr_mul(tmp2, cosU1, cosU2, r);
    mpfr_mul(tmp2, tmp2, cosLambda, r);
    mpfr_add(cosSigma, tmp1, tmp2, r);

    // sigma = atan2(sinSigma, cosSigma)
    mpfr_atan2(sigma, sinSigma, cosSigma, r);

    // sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma
    mpfr_mul(sinAlpha, cosU1, cosU2, r);
    mpfr_mul(sinAlpha, sinAlpha, sinLambda, r);
    mpfr_mul(sinAlpha, sinAlpha, sinSigma, r);

    // cosSqAlpha = 1 - sinAlpha * sinAlpha
    mpfr_mul(tmp1, sinAlpha, sinAlpha, r);
    mpfr_ui_sub(cosSqAlpha, 1, tmp1, r);

    // cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
    mpfr_mul(tmp1, sinU1, sinU2, r);
    mpfr_div(tmp1, tmp1, cosSqAlpha, r);
    mpfr_mul_ui(tmp1, tmp1, 2, r);
    mpfr_sub(cos2SigmaM, cosSigma, tmp1, r);

    if(mpfr_nan_p(cos2SigmaM) != 0) mpfr_set_ui(cos2SigmaM, 0, r); // equatorial line: cosSqAlpha=0

    // C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
    mpfr_mul_ui(tmp1, cosSqAlpha, 3, r);
    mpfr_ui_sub(tmp2, 4, tmp1, r);
    mpfr_mul(tmp1, f, tmp2, r);
    mpfr_add_ui(tmp2, tmp1, 4, r);
    mpfr_mul(tmp1, cosSqAlpha, tmp2, r);
    mpfr_div_ui(tmp2, f, 16, r);
    mpfr_mul(C, tmp2, tmp1, r);

    mpfr_set(lambdaP, lambda, r);

    // lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)))
    mpfr_sqr(tmp3, cos2SigmaM, r);
    mpfr_mul_ui(tmp3, tmp3, 2, r);
    mpfr_sub_ui(tmp2, tmp3, 1, r);
    mpfr_mul(tmp1, cosSigma, tmp2, r);
    mpfr_mul(tmp1, C, tmp1, r);
    mpfr_add(tmp3, cos2SigmaM, tmp1, r);
    mpfr_mul(tmp2, sinSigma, tmp3, r);
    mpfr_mul(tmp2, C, tmp2, r);
    mpfr_add(tmp1, sigma, tmp2, r);
    mpfr_mul(tmp3, sinAlpha, tmp1, r);
    mpfr_mul(tmp3, f, tmp3, r);
    mpfr_ui_sub(tmp2, 1, C, r);
    mpfr_mul(tmp1, tmp2, tmp3, r);
    mpfr_add(lambda, L, tmp1, r);
 
    mpfr_sub(dLambda, lambda, lambdaP, r);
    mpfr_abs(dLambda, dLambda, r);
  }
  while((mpfr_less_p(dLambda, lambdaC) == 0) && (--iterLimit > 0));

  // uSq = cosSqAlpha * (a * a - b * b) / (b * b)
  mpfr_sqr(tmp1, a, r);
  mpfr_sqr(tmp2, b, r);
  mpfr_sub(tmp3, tmp1, tmp2, r);
  mpfr_div(tmp1, tmp3, tmp2, r);
  mpfr_mul(uSq, cosSqAlpha, tmp1, r);

  // k1 = (sqrt(1 + uSq) - 1) / (sqrt(1 + uSq) + 1)
  mpfr_add_ui(tmp2, uSq, 1, r);
  mpfr_sqrt(tmp1, tmp2, r);
  mpfr_sub_ui(tmp2, tmp1, 1, r);
  mpfr_add_ui(tmp3, tmp1, 1, r);
  mpfr_div(k1, tmp2, tmp3, r);

  // A = (1 + 0.25 * k1 * k1) / (1 - k1)
  mpfr_sqr(tmp1, k1, r);
  mpfr_div_ui(tmp3, tmp1, 4, r);
  mpfr_add_ui(tmp2, tmp3, 1, r);
  mpfr_ui_sub(tmp3, 1, k1, r);
  mpfr_div(A, tmp2, tmp3, r);

  // B = k1 * (1 - 3/8 * k1 * k1)
  mpfr_mul_ui(tmp3, tmp1, 3, r);
  mpfr_div_ui(tmp3, tmp3, 8, r);
  mpfr_ui_sub(tmp2, 1, tmp3, r);
  mpfr_mul(B, k1, tmp2, r);

  // deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)))
  mpfr_sqr(tmp2, sinSigma, r);
  mpfr_mul_ui(tmp2, tmp2, 4, r);
  mpfr_sub_ui(tmp2, tmp2, 3, r);
  mpfr_sqr(tmp4, cos2SigmaM, r);
  mpfr_mul_ui(tmp3, tmp4, 4, r);
  mpfr_sub_ui(tmp3, tmp3, 3, r);
  mpfr_mul(tmp1, tmp2, tmp3, r);

  mpfr_mul(tmp3, cos2SigmaM, tmp1, r);
  mpfr_mul(tmp3, tmp3, B, r);
  mpfr_div_ui(tmp3, tmp3, 6, r);

  mpfr_mul_ui(tmp2, tmp4, 2, r);
  mpfr_sub_ui(tmp2, tmp2, 1, r);

  mpfr_mul(tmp1, cosSigma, tmp2, r);
  mpfr_sub(tmp1, tmp1, tmp3, r);

  mpfr_mul(tmp2, B, tmp1, r);
  mpfr_div_ui(tmp2, tmp2, 4, r);
  mpfr_add(tmp2, cos2SigmaM, tmp2, r);

  mpfr_mul(tmp1, sinSigma, tmp2, r);
  mpfr_mul(deltaSigma, B, tmp1, r);

  // res = b * A * (sigma - deltaSigma)
  mpfr_sub(tmp1, sigma, deltaSigma, r);
  mpfr_mul(tmp2, A, tmp1, r);
  mpfr_mul(res, b, tmp2, r);
  
  /* calculate heading 
  //heading_start = atan2((cosU2 * sinLambda) / (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda))
  mpfr_mul(part1, cosU2, sinLambda, r);
  mpfr_mul(tmp1, cosU1, sinU2, r);
  mpfr_mul(tmp2, sinU1, cosU2, r);
  mpfr_mul(tmp2, tmp2, cosLambda, r);
  mpfr_sub(tmp1, tmp1, tmp2, r);
  mpfr_atan2(heading_start, part1, tmp1, r);
  
  //heading_end = atan2((cosU1 * sinLambda) / (-sinU1 * cosU2 + cosU1 * sinU2 * cosLambda))
  mpfr_mul(part1, cosU1, sinLambda, r);
  mpfr_mul_si(tmp1, sinU1, -1, r);
  mpfr_mul(tmp1, tmp1, cosU2, r);
  mpfr_mul(tmp2, cosU1, sinU2, r);
  mpfr_mul(tmp2, tmp2, cosLambda, r);
  mpfr_sub(tmp1, tmp1, tmp2, r);
  mpfr_atan2(heading_end, part1, tmp1, r);
*/
  //mpfr_clears(a, b, f, L, U1, U2, sinU1, sinU2, cosU1, cosU2, lambda, lambdaP, sinLambda, cosLambda, sigma, sinSigma, cosSigma, sinAlpha, cosSqAlpha, cos2SigmaM, C, dLambda, lambdaC, (mpfr_ptr)NULL);
  //mpfr_clears(tmp1, tmp2, tmp3, tmp4, (mpfr_ptr)NULL);
  mpfr_clears(a, b, f, fInv, L, U1, U2, sinU1, sinU2, cosU1, cosU2, lambda, lambdaP, sinLambda, cosLambda, sigma, sinSigma, cosSigma, sinAlpha, cosSqAlpha, cos2SigmaM, C, dLambda, lambdaC, uSq, k1, A, B, deltaSigma, (mpfr_ptr)NULL);
  mpfr_clears(part1, tmp1, tmp2, tmp3, tmp4, (mpfr_ptr)NULL);

  return 0;
}

long double calculate_distance(lat_t lat1_deg, lng_t lng1_deg, alt_t ele1_in, lat_t lat2_deg, lng_t lng2_deg, alt_t ele2_in)
{
  mpfr_rnd_t rnd = MPFR_RNDN; /* round to nearest */
  mpfr_prec_t prec = MAX(_MPFR_DEFAULT_PREC, 32); /* minimal a precision from 32 bits */

  mpfr_t ans, lat1, lng1, ele1, lat2, lng2, ele2, lat_start, lat_end, lng_start, lng_end;
  mpfr_inits2(prec, ans, lat1, lng1, ele1, lat2, lng2, ele2, lat_start, lat_end, lng_start, lng_end, (mpfr_ptr)NULL);

  mpfr_set_default_prec(prec);

  /* save the parameters as mpfr variables */
  mpfr_set_d(lat_start, lat1_deg, rnd);
  mpfr_set_d(lng_start, lng1_deg, rnd);
  mpfr_set_d(ele1, ele1_in, rnd);
  mpfr_set_d(lat_end, lat2_deg, rnd);
  mpfr_set_d(lng_end, lng2_deg, rnd);
  mpfr_set_d(ele2, ele2_in, rnd);

  deg_to_rad_mpfr(lat1, lat_start, rnd);
  deg_to_rad_mpfr(lat2, lat_end, rnd);
  deg_to_rad_mpfr(lng1, lng_start, rnd);
  deg_to_rad_mpfr(lng2, lng_end, rnd);
  //mpfr_printf("point 1\nlat: %.10Rf\nlng: %.10Rf\npoint 2\nlat: %.10Rf\nlng: %.10Rf\n", lat1, lng1, lat2, lng2);

  /* if NaN (not a number is returned, double the precision and calculate again. */
  for(prec = _MPFR_DEFAULT_PREC; (mpfr_nan_p(ans) != 0) && (prec < (MPFR_PREC_MAX / 4)); prec *= 2)
  {
    //printf("1\n");
    mpfr_set_nan(ans); /* set ans2 back to NaN for sanity */
    if(VincentyDistance_mpfr(ans, lat1, lng1, lat2, lng2, prec, rnd) == -1)
      return 0;
    //mpfr_printf("ans: %Rf\n", ans);
  }
  
  /* use pythagoras to calculate the length on a slope */
  mpfr_t ans_real, dAlt, tmp1, tmp2, tmp3;
  mpfr_inits2(prec, ans_real, dAlt, tmp1, tmp2, tmp3, (mpfr_ptr)NULL);
  mpfr_sub(dAlt, ele1, ele2, rnd);
  mpfr_sqr(tmp1, dAlt, rnd);
  mpfr_sqr(tmp2, ans, rnd);
  mpfr_add(tmp3, tmp1, tmp2, rnd);
  mpfr_sqrt(ans_real, tmp3, rnd);
/*
  mpfr_printf("c^2 = a^2 + b^2\n");
  mpfr_printf("c^2 = %Rf^2 + %Rf^2\n", ans, dAlt);
  mpfr_printf("c^2 = %Rf + %Rf\n", tmp2, tmp1);
  mpfr_printf("c^2 = %Rf\n", tmp3);
  mpfr_printf("c = %Rf\n", ans_real);
*/
  mpfr_clears(dAlt, tmp1, tmp2, tmp3, (mpfr_ptr)NULL);

  mpfr_clears(lat1, lng1, ele1, lat2, lng2, ele2, (mpfr_ptr)NULL);
  /* save the answer in a normal variable */
  long double res = mpfr_get_ld(ans_real, rnd);

  mpfr_clears(ans, ans_real, (mpfr_ptr)NULL);
  mpfr_free_cache();

  /* return the answer */
  return res;
}

float calculate_incline(long double dst, alt_t d_ele)
{
  mpfr_rnd_t rnd = MPFR_RNDN; /* round to nearest */
  mpfr_prec_t prec = MAX(_MPFR_DEFAULT_PREC, 32); /* minimal a precision from 32 bits */

  mpfr_t ans, e, d, tmp1;
  mpfr_inits2(prec, ans, e, d, tmp1, (mpfr_ptr)NULL);

  mpfr_set_default_prec(prec);

  mpfr_set_ld(d, dst, rnd);
  mpfr_set_d(e, d_ele, rnd);

  // tan(asin(ele / dst))
  mpfr_div(tmp1, e, d, rnd);
  mpfr_asin(tmp1, tmp1, rnd);
  mpfr_tan(ans, tmp1, rnd);
  float res = mpfr_get_d(ans, rnd);

  mpfr_clears(ans, e, d, tmp1, (mpfr_ptr)NULL);
  mpfr_free_cache();

  return res;
}

#else /* USE_MPFR */

#define FORMULA_DOUBLE

#include <stdlib.h>
#include <math.h>

#define f ((long double)(1/(long double)298.257223563))
#define a ((long double)6378137.0)
#define PI ((long double)(M_PI))

long double calculate_distance(lat_t lat1_deg, lng_t lng1_deg, alt_t ele1, lat_t lat2_deg, lng_t lng2_deg, alt_t ele2)
{
  long double lat1, lat2, lng1, lng2;
  lat1 = (long double)lat1_deg / 180 * PI;
  lng1 = (long double)lng1_deg / 180 * PI;
  lat2 = (long double)lat2_deg / 180 * PI;
  lng2 = (long double)lng2_deg / 180 * PI;
  long double b, lambda, L, U1, U2, cosU1, cosU2, sinU1, sinU2, sinLambda, cosLambda, sinSigma, cosSigma, sigma, sinAlpha, cosSqAlpha, cos2SigmaM, C, uSq, k1, A, B, deltaSigma, res;
  b = a * (1 - f);
  lambda = L = lng2 - lng1;
  U1 = atan((1 - f) * tan(lat1));
  U2 = atan((1 - f) * tan(lat2));
  cosU1 = cos(U1);
  cosU2 = cos(U2);
  sinU1 = sin(U1);
  sinU2 = sin(U2);
  long double lambda_prev;
  int iterLimit = MAX_NUMBER_ITERATIONS;
  do
  {
    lambda_prev = lambda;
    sinLambda = sin(lambda);
    cosLambda = cos(lambda);
    sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
    //sinSigma = sqrt(tmp1 + tmp2);
    //if(mpfr_nan_p(sinSigma) != 0 || mpfr_zero_p(sinSigma) != 0 || mpfr_cmp_ui(sinSigma, 0) == 0) return 0;  // co-incident points
    cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
    sigma = atan2(sinSigma, cosSigma);
    sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
    cosSqAlpha = 1 - sinAlpha * sinAlpha;
    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
    //if(if(mpfr_nan_p(cos2SigmaM) != 0) mpfr_set_ui(cos2SigmaM, 0, r); // equatorial line: cosSqAlpha=0
    C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
    lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
  }
  while(abs(lambda - lambda_prev) > (long double)1e-24 && (--iterLimit > 0));

  uSq = cosSqAlpha * (a * a - b * b) / (b * b);
  k1 = (sqrt(1 + uSq) - 1) / (sqrt(1 + uSq) + 1);
  A = (1 + 0.25 * k1 * k1) / (1 - k1);
  B = k1 * (1 - 3/8 * k1 * k1);
  deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
  res = b * A * (sigma - deltaSigma);

  /* calculate heading
  heading_start = atan2((cosU2 * sinLambda) / (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
  heading_end = atan2((cosU1 * sinLambda) / (-sinU1 * cosU2 + cosU1 * sinU2 * cosLambda));
  */

  double res_real = (double)sqrt(pow((long double)ele1 - (long double)ele2, 2) + pow(res, 2));
  return res_real;
}

float calculate_incline(long double dst, alt_t d_ele)
{
  return (float)tan(asin((long double)d_ele / dst));
}

#undef f
#undef a
#undef b

#endif /* USE_MPFR */