Coverage for pyguymer3/geo/calc_loc_from_loc_and_bearing_and_dist.py: 98%

1#!/usr/bin/env python3 

2 

3# Define function ... 

4def calc_loc_from_loc_and_bearing_and_dist( 

5 lon1_deg, 

6 lat1_deg, 

7 alpha1_deg, 

8 s_m, 

9 /, 

10 *, 

11 eps = 1.0e-12, 

12 nIter = 100, 

13): 

14 """Calculate the location and reverse bearing from another location and a 

15 forward bearing and a distance. 

16 

17 This function reads in coordinates (in degrees) on the surface of the Earth, 

18 a bearing (in degrees) and a distance (in metres). It then calculates the 

19 coordinates (in degrees) that are at the end of the vector and the bearing 

20 (in degrees) back to the first coordinate. 

21 

22 Parameters 

23 ---------- 

24 lon1_deg : float 

25 the longitude of the first coordinate (in degrees) 

26 lat1_deg : float 

27 the latitude of the first coordinate (in degrees) 

28 alpha1_deg : float 

29 the bearing to the second coordinate from the first coordinate (in 

30 degrees) 

31 s_m : float 

32 the distance between the two coordinates (in metres) 

33 eps : float, optional 

34 the tolerance of the Vincenty formula iterations 

35 nIter : int, optional 

36 the maximum number of iterations (particularly the Vincenty formula) 

37 

38 Returns 

39 ------- 

40 lon2_deg : float 

41 the longitude of the second coordinate (in degrees) 

42 lat2_deg : float 

43 the latitude of the second coordinate (in degrees) 

44 alpha2_deg : float 

45 the bearing to the first coordinate from the second coordinate (in 

46 degrees) 

47 

48 Notes 

49 ----- 

50 This function uses `Vincenty's formulae 

51 <https://en.wikipedia.org/wiki/Vincenty%27s_formulae>`_ ; there is a 

52 `JavaScript implementation 

53 <https://www.movable-type.co.uk/scripts/latlong-vincenty.html>`_ online too. 

54 

55 ``lambda`` is a reserved word in Python so I use ``lam`` as my variable name 

56 instead. 

57 

58 Copyright 2017 Thomas Guymer [1]_ 

59 

60 References 

61 ---------- 

62 .. [1] PyGuymer3, https://github.com/Guymer/PyGuymer3 

63 """ 

64 

65 # Import standard modules ... 

66 import math 

67 

68 # ************************************************************************** 

69 

70 # Convert to radians ... 

71 lon1 = math.radians(lon1_deg) # [rad] 

72 lat1 = math.radians(lat1_deg) # [rad] 

73 alpha1 = math.radians(alpha1_deg) # [rad] 

74 

75 # Set constants ... 

76 a = 6378137.0 # [m] 

77 f = 1.0 / 298.257223563 

78 b = (1.0 - f) * a # [m] 

79 u1 = math.atan((1.0 - f) * math.tan(lat1)) # [rad] 

80 sigma1 = math.atan2( 

81 math.tan(u1), 

82 math.cos(alpha1) 

83 ) 

84 sin_alpha = math.cos(u1) * math.sin(alpha1) 

85 cosSq_alpha = 1.0 - sin_alpha ** 2 

86 c = f * cosSq_alpha * (4.0 + f * (4.0 - 3.0 * cosSq_alpha)) / 16.0 

87 uSq = cosSq_alpha * (a ** 2 - b ** 2) / b ** 2 

88 bigA = 1.0 + uSq * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq))) / 16384.0 

89 bigB = uSq * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq))) / 1024.0 

90 

91 # Set initial value of sigma and initialize counter ... 

92 sigma = s_m / (b * bigA) 

93 iIter = 0 # [#] 

94 

95 # Start infinite loop ... 

96 while True: 

97 # Stop looping if the function has been called too many times ... 

98 if iIter >= nIter: 

99 raise Exception(f"failed to converge; loc = ({lon1_deg:+.9f}°,{lat1_deg:+.9f}°); alpha1 = {alpha1_deg:.15e}°, s = {s_m:.15e}m; eps = {eps:.15e}; nIter = {nIter:,d}") from None 

100 

101 # Find new value of sigma and increment counter ... 

102 two_sigma_m = 2.0 * sigma1 + sigma 

103 delta_sigma = bigB * math.sin(sigma) * (math.cos(two_sigma_m) + 0.25 * bigB * (math.cos(sigma) * (2.0 * math.cos(two_sigma_m) ** 2 - 1.0) - bigB * math.cos(two_sigma_m) * (4.0 * math.sin(sigma) ** 2 - 3.0) * (4.0 * math.cos(two_sigma_m) ** 2 - 3.0) / 6.0)) 

104 sigmaNew = s_m / (b * bigA) + delta_sigma 

105 iIter += 1 # [#] 

106 

107 # Only check the solution after at least 3 function calls ... 

108 if iIter >= 3: 

109 if sigmaNew == sigma: 

110 # Replace old sigma with new sigma ... 

111 sigma = sigmaNew 

112 

113 break 

114 if abs(sigmaNew - sigma) / abs(sigmaNew) <= eps: 

115 # Replace old sigma with new sigma ... 

116 sigma = sigmaNew 

117 

118 break 

119 

120 # Replace old sigma with new sigma ... 

121 sigma = sigmaNew 

122 

123 # Calculate end point and forward azimuth ... 

124 lat2 = math.atan2( 

125 math.sin(u1) * math.cos(sigma) + math.cos(u1) * math.sin(sigma) * math.cos(alpha1), 

126 (1.0 - f) * math.hypot( 

127 sin_alpha, 

128 math.sin(u1) * math.sin(sigma) - math.cos(u1) * math.cos(sigma) * math.cos(alpha1) 

129 ) 

130 ) # [rad] 

131 lam = math.atan2( 

132 math.sin(sigma) * math.sin(alpha1), 

133 math.cos(u1) * math.cos(sigma) - math.sin(u1) * math.sin(sigma) * math.cos(alpha1) 

134 ) 

135 l = lam - (1.0 - c) * f * sin_alpha * (sigma + c * math.sin(sigma) * (math.cos(two_sigma_m) + c * math.cos(sigma) * (2.0 * math.cos(two_sigma_m) ** 2 - 1.0))) 

136 lon2 = l + lon1 # [rad] 

137 lon2 = ((lon2 + 3.0 * math.pi) % (2.0 * math.pi)) - math.pi # NOTE: Normalize to -180° <--> +180° 

138 alpha2 = math.atan2( 

139 sin_alpha, 

140 math.cos(u1) * math.cos(sigma) * math.cos(alpha1) - math.sin(u1) * math.sin(sigma) 

141 ) # [rad] 

142 alpha2 = (alpha2 + 2.0 * math.pi) % (2.0 * math.pi) # NOTE: Normalize to +0° <--> +360° 

143 

144 # Return end point and forward azimuth ... 

145 return math.degrees(lon2), math.degrees(lat2), math.degrees(alpha2)