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