Coverage for src/sensai/geoanalytics/geo_coords.py: 0%

1""" 

2Utility functions and classes for geographic coordinates 

3""" 

4 

5import math 

6from typing import Tuple, Iterable 

7 

8import numpy as np 

9import pandas as pd 

10 

11from ..util.string import ToStringMixin 

12 

13EARTH_RADIUS = 6371000 

14EARTH_CIRCUMFERENCE = 2 * math.pi * EARTH_RADIUS 

15LATITUDE_PER_METRE = 360.0 / EARTH_CIRCUMFERENCE 

16 

17 

18def longitude_per_m(latitude): 

19 return LATITUDE_PER_METRE / math.cos(math.radians(latitude)) 

20 

21 

22def latitude_per_m(): 

23 return LATITUDE_PER_METRE 

24 

25 

26def approximate_squared_distance(p1: Tuple[float, float], p2: Tuple[float, float]): 

27 """ 

28 :param p1: a tuple (latitude, longitude) 

29 :param p2: a tuple (latitude, longitude) 

30 :return: the approximate squared distance (in m²) between p1 and p2 

31 """ 

32 lat_per_m = latitude_per_m() 

33 p1lat, p1lon = p1 

34 p2lat, p2lon = p2 

35 lon_per_m = longitude_per_m((p1lat + p2lat) / 2) 

36 dx = (p2lon - p1lon) / lon_per_m 

37 dy = (p2lat - p1lat) / lat_per_m 

38 return dx * dx + dy * dy 

39 

40 

41def closest_point_on_segment(search_pos: Tuple[float, float], segPoint1: Tuple[float, float], segPoint2: Tuple[float, float]): 

42 """ 

43 Gets the point on the line segment connecting segPoint1 and segPoint2 that is closest to searchPos 

44 

45 :param search_pos: the position for which to search for the closest point on the line segment 

46 :param segPoint1: the first point defining the line segment on which to search 

47 :param segPoint2: the second point defining the line segment on which to search 

48 :return: the closest point, which is on the line connecting segPoint1 and segPoint2 (and may be one of the two points) 

49 """ 

50 seg1lat, seg1lon = segPoint1 

51 seg2lat, seg2lon = segPoint2 

52 srchlat, srchlon = search_pos 

53 lat_per_m = latitude_per_m() 

54 lon_per_m = longitude_per_m(srchlat) 

55 sp1x = (seg1lon - srchlon) / lon_per_m 

56 sp1y = (seg1lat - srchlat) / lat_per_m 

57 sp2x = (seg2lon - srchlon) / lon_per_m 

58 sp2y = (seg2lat - srchlat) / lat_per_m 

59 vx = sp2x - sp1x 

60 vy = sp2y - sp1y 

61 c1 = -vx * sp1x - vy * sp1y 

62 if c1 <= 0: 

63 return segPoint1 

64 c2 = vx * vx + vy * vy 

65 if c2 <= c1: 

66 return segPoint2 

67 b = 0 if c2 == 0 else c1 / c2 

68 lon = seg1lon + b * vx * lon_per_m 

69 lat = seg1lat + b * vy * lat_per_m 

70 return [lat, lon] 

71 

72 

73def orientation(p1: Tuple[float, float], p2: Tuple[float, float]) -> float: 

74 """ 

75 Gets the orientation angle for the vector from p1 to p2 

76 

77 :param p1: a (lat, lon) pair 

78 :param p2: a (lat, lon) pair 

79 :return: the orientation angle in rad 

80 """ 

81 p1_lat, p1_lon = p1 

82 p2_lat, p2_lon = p2 

83 center_lat = (p1_lat + p2_lat) / 2 

84 dx = (p2_lon - p1_lon) / longitude_per_m(center_lat) 

85 dy = (p2_lat - p1_lat) / latitude_per_m() 

86 return math.atan2(dy, dx) 

87 

88 

89def abs_angle_difference(a1: float, a2: float) -> float: 

90 """ 

91 Computes the absolute angle difference in ]-pi, pi] between two angles 

92 

93 :param a1: an angle in rad 

94 :param a2: an angle in rad 

95 :return: the difference in rad 

96 """ 

97 d = a1 - a2 

98 while d > math.pi: 

99 d -= 2*math.pi 

100 while d <= -math.pi: 

101 d += 2*math.pi 

102 return abs(d) 

103 

104 

105def closest_point_on_polyline(search_pos, polyline, search_orientation_angle=None, max_angle_difference=0) \ 

106 -> Tuple[Tuple[float, float], float, int]: 

107 """ 

108 Gets the point on the given polyline that is closest to the given search position along with the 

109 distance (in metres) to the polyline 

110 

111 :param search_pos: a (lat, lon) pair indicating the position for which to find the closest math on the polyline 

112 :param polyline: list of (lat, lon) pairs that make up the polyline on which to search 

113 :param search_orientation_angle: if not None, defines the orientation with which to compute angle differences 

114 (if maxAngleDifference > 0) 

115 :param max_angle_difference: the maximum absolute angle difference (in rad) that is admissible (between the orientation of the 

116 respective line segment and the orientation given in searchOrientationAngle) 

117 :return: a tuple (opt_point, opt_dist, opt_segment_start_idx) where 

118 opt_point is the closest point (with admissible orientation - or None if there is none), 

119 opt_dist is the distance from the polyline to the closest point, 

120 opt_segment_start_idx is the index of the first point of the segment on the polyline for which the closest point was found 

121 """ 

122 if len(polyline) < 2: 

123 raise Exception("Polyline must consist of at least two points") 

124 opt_segment_start_idx = None 

125 opt_point = None 

126 opt_sq_dist = None 

127 for i in range(len(polyline)-1): 

128 if max_angle_difference > 0: 

129 orientation_angle = orientation(polyline[i], polyline[i+1]) 

130 ang_diff = abs_angle_difference(orientation_angle, search_orientation_angle) 

131 if ang_diff > max_angle_difference: 

132 continue 

133 opt_seg_point = closest_point_on_segment(search_pos, polyline[i], polyline[i + 1]) 

134 sq_dist = approximate_squared_distance(search_pos, opt_seg_point) 

135 if opt_sq_dist is None or sq_dist < opt_sq_dist: 

136 opt_point = opt_seg_point 

137 opt_sq_dist = sq_dist 

138 opt_segment_start_idx = i 

139 return opt_point, math.sqrt(opt_sq_dist), opt_segment_start_idx 

140 

141 

142class GeoCoord(ToStringMixin): 

143 """ 

144 Represents geographic coordinates (WGS84) 

145 """ 

146 def __init__(self, lat: float, lon: float): 

147 self.lat = lat 

148 self.lon = lon 

149 

150 def latlon(self): 

151 return self.lat, self.lon 

152 

153 def distance_to(self, gps_position: 'GeoCoord'): 

154 return math.sqrt(self.squared_distance_to(gps_position)) 

155 

156 def squared_distance_to(self, gps_position: 'GeoCoord'): 

157 return approximate_squared_distance(self.latlon(), gps_position.latlon()) 

158 

159 def local_coords(self, lcs): 

160 return lcs.get_local_coords(self.lat, self.lon) 

161 

162 @classmethod 

163 def mean_coord(cls, geo_coords: Iterable["GeoCoord"]): 

164 mean_lat = np.mean([c.lat for c in geo_coords]) 

165 mean_lon = np.mean([c.lon for c in geo_coords]) 

166 # noinspection PyTypeChecker 

167 return GeoCoord(mean_lat, mean_lon) 

168 

169 

170class GpsTracePoint(GeoCoord): 

171 def __init__(self, lat, lon, time: pd.Timestamp): 

172 super().__init__(lat, lon) 

173 self.time = time 

174 

175 

176class GeoRect: 

177 def __init__(self, min_lat: float, min_lon: float, max_lat: float, max_lon: float): 

178 if max_lat < min_lat or max_lon < min_lon: 

179 raise ValueError() 

180 self.minLat = min_lat 

181 self.minLon = min_lon 

182 self.maxLat = max_lat 

183 self.maxLon = max_lon 

184 

185 @staticmethod 

186 def from_circle(centre_lat, centre_lon, radius_m): 

187 """Creates the bounding rectangle for the given circular area""" 

188 from .local_coords import LocalCoordinateSystem 

189 lcs = LocalCoordinateSystem(centre_lat, centre_lon) 

190 min_lat, min_lon = lcs.get_lat_lon(-radius_m, -radius_m) 

191 max_lat, max_lon = lcs.get_lat_lon(radius_m, radius_m) 

192 return GeoRect(min_lat, min_lon, max_lat, max_lon)