1 #include <math.h>

2 #include <stdbool.h>

3 #include <stdio.h>

4 

5 #ifndef VINCENTY_TOLERANCE

6 

7 #define VINCENTY_TOLERANCE 1e-12

8 #endif

9 

10 #ifndef VINCENTY_MAX_LOOP

11 

12 #define VINCENTY_MAX_LOOP 1e3

13 #endif

14 

15 #ifndef USE_HELMERTS

16 

20 #define USE_HELMERTS 1

21 #endif

22 

23 #define pi 3.14159265358979323846264338327950288419716939937510582097494459230781640628620899L

24 #define flattening (1 / 298.257223563) // flattening of the ellipsoid

25 #define semimajor 6378137.0 // abbreviated a, radius at the equator

26 #define semiminor ((1 - flattening) * semimajor) // abbreviated b, radius at the poles

27 

28 inline static double deg2rad(const double deg) {

29  return (pi * deg / 180.0);

30 }

31 //inline static double rad2deg(const double rad) {

32 // return (rad * 180.0 / pi);

33 //}

34 

35 inline static void vincenty_AB_equations(const double u2, double *A, double *B) {

36 #ifdef USE_HELMERTS

37  const double k1 = (sqrt(1 + u2) - 1) / (sqrt(1 + u2) + 1);

38  *A = (1 + 1 / 4 * pow(k1, 2));

39  *B = k1 * (1 - 3 / 8 * pow(k1, 2));

40 # else

41  *A = 1 + (u2 / 16384) * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)));

42  *B = (u2 / 1024) * (256 + u2 * (-128 + u2 * (74 - 47 * u2)));

43 #endif

44 }

45 

46 // https://en.wikipedia.org/wiki/Vincenty's_formulae#Inverse_problem

47 double vincenty_distance(double lat1, double lon1, double lat2, double lon2) {

48  if (lat1 == lat2 && lon1 == lon2) { // returns -nan in this case

49  return 0;

50  }

51  lat1 = deg2rad(lat1);

52  lon1 = deg2rad(lon1);

53  lat2 = deg2rad(lat2);

54  lon2 = deg2rad(lon2);

55 

56  // reduced latitudes (latitudes on the auxiliary sphere)

57  const double U1 = atan((1 - flattening) * tan(lat1));

58  const double U2 = atan((1 - flattening) * tan(lat2));

59  const double sin_U1 = sin(U1), cos_U1 = cos(U1);

60  const double sin_U2 = sin(U2), cos_U2 = cos(U2);

61  const double L = lon2 - lon1;

62 

63  double lambda = L;

64 

65  double old_lambda, sin_sigma, cos_sigma, sigma, sin_alpha, cos2_alpha, cos_2sigmam = 0, C;

66  int loop_count = 0;

67  do {

68  old_lambda = lambda;

69  const double cos_lambda = cos(lambda), sin_lambda = sin(lambda);

70  sin_sigma = sqrt(pow(cos_U2 * sin_lambda, 2) + pow(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda, 2));

71  cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;

72  sigma = atan2(sin_sigma, cos_sigma);

73  sin_alpha = (cos_U1 * cos_U2 * sin_lambda) / sin_sigma;

74  cos2_alpha = 1 - pow(sin_alpha, 2);

75  if (cos2_alpha == 0) {

76  lambda = L + flattening * sin_alpha * sigma;

77  } else {

78  cos_2sigmam = cos_sigma - (2 * sin_U1 * sin_U2) / cos2_alpha;

79  C = (flattening / 16) * cos2_alpha * (4 + flattening * (4 - 3 * cos2_alpha));

80  lambda = L + (1 - C) * flattening * sin_alpha * (sigma + C * sin_sigma * (cos_2sigmam + C * cos_sigma * (-1 + 2 * cos_2sigmam)));

81  }

82  } while (fabs(lambda - old_lambda) > VINCENTY_TOLERANCE && ++loop_count < VINCENTY_MAX_LOOP);

83 

84  const double u2 = cos2_alpha * (pow(semimajor, 2) - pow(semiminor, 2)) / pow(semiminor, 2);

85  double A, B;

86  vincenty_AB_equations(u2, &A, &B);

87  const double cos2_2sigmam = pow(cos_2sigmam, 2);

88  const double delta_sigma = B * sin_sigma * (cos_2sigmam + 1 / 4 * B * (-1 + 2 * cos2_2sigmam) - 1 / 6 * B * cos_2sigmam * (-3 + 4 * pow(sin_sigma, 2)) * (-3 + 4 * cos2_2sigmam));

89  const double s = semiminor * A * (sigma - delta_sigma);

90 

91  // I think this stuff calculates the bearings, not required for this function but useful for a geolib.

92 // const double cos_lambda = cos(lambda);

93 // const double sin_lambda = sin(lambda);

94 // const double cosU1_sin_U2 = cos_U1 * sin_U2;

95 // const double alpha1 = atan((cos_U2 * sin_lambda) / (cosU1_sin_U2 - (sin_U1 * cos_U2 * cos_lambda)));

96 // const double alpha2 = atan((cos_U1 * sin_lambda) / ((-sin_U1 * cos_U2) - (cosU1_sin_U2 * cos_lambda)));

97 

98 // printf("%f, %f, %f, %f = %.64f\n", rad2deg(lat1), rad2deg(lon1), rad2deg(lat2), rad2deg(lon2), s);

99  return s;

100 }