1# misc.py - miscellaneous geodesy and time functions 2"miscellaneous geodesy and time functions" 3# 4# This file is Copyright (c) 2010 by the GPSD project 5# BSD terms apply: see the file COPYING in the distribution root for details. 6 7# This code runs compatibly under Python 2 and 3.x for x >= 2. 8# Preserve this property! 9from __future__ import absolute_import, print_function, division 10 11import calendar 12import io 13import math 14import time 15 16def monotonic(): 17 """return monotonic seconds, of unknown epoch. 18 Python 2 to 3.7 has time.clock(), deprecates in 3.3+, removed in 3.8 19 Python 3.5+ has time.monotonic() 20 This always works 21 """ 22 23 if hasattr(time, 'monotonic'): 24 return time.monotonic() 25 # else 26 return time.clock() 27 28 29# Determine a single class for testing "stringness" 30try: 31 STR_CLASS = basestring # Base class for 'str' and 'unicode' in Python 2 32except NameError: 33 STR_CLASS = str # In Python 3, 'str' is the base class 34 35# We need to be able to handle data which may be a mixture of text and binary 36# data. The text in this context is known to be limited to US-ASCII, so 37# there aren't any issues regarding character sets, but we need to ensure 38# that binary data is preserved. In Python 2, this happens naturally with 39# "strings" and the 'str' and 'bytes' types are synonyms. But in Python 3, 40# these are distinct types (with 'str' being based on Unicode), and conversions 41# are encoding-sensitive. The most straightforward encoding to use in this 42# context is 'latin-1' (a.k.a.'iso-8859-1'), which directly maps all 256 43# 8-bit character values to Unicode page 0. Thus, if we can enforce the use 44# of 'latin-1' encoding, we can preserve arbitrary binary data while correctly 45# mapping any actual text to the proper characters. 46 47BINARY_ENCODING = 'latin-1' 48 49if bytes is str: # In Python 2 these functions can be null transformations 50 51 polystr = str 52 polybytes = bytes 53 54 def make_std_wrapper(stream): 55 "Dummy stdio wrapper function." 56 return stream 57 58 def get_bytes_stream(stream): 59 "Dummy stdio bytes buffer function." 60 return stream 61 62else: # Otherwise we do something real 63 64 def polystr(o): 65 "Convert bytes or str to str with proper encoding." 66 if isinstance(o, str): 67 return o 68 if isinstance(o, bytes): 69 return str(o, encoding=BINARY_ENCODING) 70 if isinstance(o, bytearray): 71 return str(o, encoding=BINARY_ENCODING) 72 raise ValueError 73 74 def polybytes(o): 75 "Convert bytes or str to bytes with proper encoding." 76 if isinstance(o, bytes): 77 return o 78 if isinstance(o, str): 79 return bytes(o, encoding=BINARY_ENCODING) 80 raise ValueError 81 82 def make_std_wrapper(stream): 83 "Standard input/output wrapper factory function" 84 # This ensures that the encoding of standard output and standard 85 # error on Python 3 matches the binary encoding we use to turn 86 # bytes to Unicode in polystr above. 87 # 88 # newline="\n" ensures that Python 3 won't mangle line breaks 89 # line_buffering=True ensures that interactive command sessions 90 # work as expected 91 return io.TextIOWrapper(stream.buffer, encoding=BINARY_ENCODING, 92 newline="\n", line_buffering=True) 93 94 def get_bytes_stream(stream): 95 "Standard input/output bytes buffer function" 96 return stream.buffer 97 98 99# some multipliers for interpreting GPS output 100# Note: A Texas Foot is ( meters * 3937/1200) 101# (Texas Natural Resources Code, Subchapter D, Sec 21.071 - 79) 102# not the same as an international fooot. 103METERS_TO_FEET = 3.28083989501312 # Meters to U.S./British feet 104METERS_TO_MILES = 0.000621371192237334 # Meters to miles 105METERS_TO_FATHOMS = 0.546806649168854 # Meters to fathoms 106KNOTS_TO_MPH = 1.15077944802354 # Knots to miles per hour 107KNOTS_TO_KPH = 1.852 # Knots to kilometers per hour 108KNOTS_TO_MPS = 0.514444444444445 # Knots to meters per second 109MPS_TO_KPH = 3.6 # Meters per second to klicks/hr 110MPS_TO_MPH = 2.2369362920544 # Meters/second to miles per hour 111MPS_TO_KNOTS = 1.9438444924406 # Meters per second to knots 112 113 114def Deg2Rad(x): 115 "Degrees to radians." 116 return x * (math.pi / 180) 117 118 119def Rad2Deg(x): 120 "Radians to degrees." 121 return x * (180 / math.pi) 122 123 124def CalcRad(lat): 125 "Radius of curvature in meters at specified latitude WGS-84." 126 # the radius of curvature of an ellipsoidal Earth in the plane of a 127 # meridian of latitude is given by 128 # 129 # R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2) 130 # 131 # where 132 # a is the equatorial radius (surface to center distance), 133 # b is the polar radius (surface to center distance), 134 # e is the first eccentricity of the ellipsoid 135 # e2 is e^2 = (a^2 - b^2) / a^2 136 # es is the second eccentricity of the ellipsoid (UNUSED) 137 # es2 is es^2 = (a^2 - b^2) / b^2 138 # 139 # for WGS-84: 140 # a = 6378.137 km (3963 mi) 141 # b = 6356.752314245 km (3950 mi) 142 # e2 = 0.00669437999014132 143 # es2 = 0.00673949674227643 144 a = 6378.137 145 e2 = 0.00669437999014132 146 sc = math.sin(math.radians(lat)) 147 x = a * (1.0 - e2) 148 z = 1.0 - e2 * pow(sc, 2) 149 y = pow(z, 1.5) 150 r = x / y 151 152 r = r * 1000.0 # Convert to meters 153 return r 154 155 156def EarthDistance(c1, c2): 157 """ 158 Vincenty's formula (inverse method) to calculate the distance (in 159 kilometers or miles) between two points on the surface of a spheroid 160 WGS 84 accurate to 1mm! 161 """ 162 163 (lat1, lon1) = c1 164 (lat2, lon2) = c2 165 166 # WGS 84 167 a = 6378137 # meters 168 f = 1 / 298.257223563 169 b = 6356752.314245 # meters; b = (1 - f)a 170 171 # MILES_PER_KILOMETER = 1000.0 / (.3048 * 5280.0) 172 173 MAX_ITERATIONS = 200 174 CONVERGENCE_THRESHOLD = 1e-12 # .000,000,000,001 175 176 # short-circuit coincident points 177 if lat1 == lat2 and lon1 == lon2: 178 return 0.0 179 180 U1 = math.atan((1 - f) * math.tan(math.radians(lat1))) 181 U2 = math.atan((1 - f) * math.tan(math.radians(lat2))) 182 L = math.radians(lon1 - lon2) 183 Lambda = L 184 185 sinU1 = math.sin(U1) 186 cosU1 = math.cos(U1) 187 sinU2 = math.sin(U2) 188 cosU2 = math.cos(U2) 189 190 for _ in range(MAX_ITERATIONS): 191 sinLambda = math.sin(Lambda) 192 cosLambda = math.cos(Lambda) 193 sinSigma = math.sqrt((cosU2 * sinLambda) ** 2 + 194 (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) ** 2) 195 if sinSigma == 0: 196 return 0.0 # coincident points 197 cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda 198 sigma = math.atan2(sinSigma, cosSigma) 199 sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma 200 cosSqAlpha = 1 - sinAlpha ** 2 201 try: 202 cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha 203 except ZeroDivisionError: 204 cos2SigmaM = 0 205 C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha)) 206 LambdaPrev = Lambda 207 Lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * 208 (cos2SigmaM + C * cosSigma * 209 (-1 + 2 * cos2SigmaM ** 2))) 210 if abs(Lambda - LambdaPrev) < CONVERGENCE_THRESHOLD: 211 break # successful convergence 212 else: 213 # failure to converge 214 # fall back top EarthDistanceSmall 215 return EarthDistanceSmall(c1, c2) 216 217 uSq = cosSqAlpha * (a ** 2 - b ** 2) / (b ** 2) 218 A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))) 219 B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))) 220 deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * ( 221 cosSigma * (-1 + 2 * cos2SigmaM ** 2) - B / 6 * cos2SigmaM * 222 (-3 + 4 * sinSigma ** 2) * (-3 + 4 * cos2SigmaM ** 2))) 223 s = b * A * (sigma - deltaSigma) 224 225 # return meters to 6 decimal places 226 return round(s, 6) 227 228 229def EarthDistanceSmall(c1, c2): 230 "Distance in meters between two close points specified in degrees." 231 # This calculation is known as an Equirectangular Projection 232 # fewer numeric issues for small angles that other methods 233 # the main use here is for when Vincenty's fails to converge. 234 (lat1, lon1) = c1 235 (lat2, lon2) = c2 236 avglat = (lat1 + lat2) / 2 237 phi = math.radians(avglat) # radians of avg latitude 238 # meters per degree at this latitude, corrected for WGS84 ellipsoid 239 # Note the wikipedia numbers are NOT ellipsoid corrected: 240 # https://en.wikipedia.org/wiki/Decimal_degrees#Precision 241 m_per_d = (111132.954 - 559.822 * math.cos(2 * phi) + 242 1.175 * math.cos(4 * phi)) 243 dlat = (lat1 - lat2) * m_per_d 244 dlon = (lon1 - lon2) * m_per_d * math.cos(phi) 245 246 dist = math.sqrt(math.pow(dlat, 2) + math.pow(dlon, 2)) 247 return dist 248 249 250def MeterOffset(c1, c2): 251 "Return offset in meters of second arg from first." 252 (lat1, lon1) = c1 253 (lat2, lon2) = c2 254 dx = EarthDistance((lat1, lon1), (lat1, lon2)) 255 dy = EarthDistance((lat1, lon1), (lat2, lon1)) 256 if lat1 < lat2: 257 dy = -dy 258 if lon1 < lon2: 259 dx = -dx 260 return (dx, dy) 261 262 263def isotime(s): 264 "Convert timestamps in ISO8661 format to and from Unix time." 265 if isinstance(s, int): 266 return time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s)) 267 268 if isinstance(s, float): 269 date = int(s) 270 msec = s - date 271 date = time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s)) 272 return date + "." + repr(msec)[3:] 273 274 if isinstance(s, STR_CLASS): 275 if s[-1] == "Z": 276 s = s[:-1] 277 if "." in s: 278 (date, msec) = s.split(".") 279 else: 280 date = s 281 msec = "0" 282 # Note: no leap-second correction! 283 return calendar.timegm( 284 time.strptime(date, "%Y-%m-%dT%H:%M:%S")) + float("0." + msec) 285 286 # else: 287 raise TypeError 288 289# End 290