path: root/gps/misc.py

# misc.py - miscellaneous geodesy and time functions
"miscellaneous geodesy and time functions"
#
# This file is Copyright (c) 2010 by the GPSD project
# BSD terms apply: see the file COPYING in the distribution root for details.

# This code runs compatibly under Python 2 and 3.x for x >= 2.
# Preserve this property!
from __future__ import absolute_import, print_function, division

import calendar
import io
import math
import time

# Determine a single class for testing "stringness"
try:
    STR_CLASS = basestring  # Base class for 'str' and 'unicode' in Python 2
except NameError:
    STR_CLASS = str         # In Python 3, 'str' is the base class

# We need to be able to handle data which may be a mixture of text and binary
# data.  The text in this context is known to be limited to US-ASCII, so
# there aren't any issues regarding character sets, but we need to ensure
# that binary data is preserved.  In Python 2, this happens naturally with
# "strings" and the 'str' and 'bytes' types are synonyms.  But in Python 3,
# these are distinct types (with 'str' being based on Unicode), and conversions
# are encoding-sensitive.  The most straightforward encoding to use in this
# context is 'latin-1' (a.k.a.'iso-8859-1'), which directly maps all 256
# 8-bit character values to Unicode page 0.  Thus, if we can enforce the use
# of 'latin-1' encoding, we can preserve arbitrary binary data while correctly
# mapping any actual text to the proper characters.

BINARY_ENCODING = 'latin-1'

if bytes is str:  # In Python 2 these functions can be null transformations

    polystr = str
    polybytes = bytes

    def make_std_wrapper(stream):
        "Dummy stdio wrapper function."
        return stream

    def get_bytes_stream(stream):
        "Dummy stdio bytes buffer function."
        return stream

else:  # Otherwise we do something real

    def polystr(o):
        "Convert bytes or str to str with proper encoding."
        if isinstance(o, str):
            return o
        if isinstance(o, bytes):
            return str(o, encoding=BINARY_ENCODING)
        if isinstance(o, bytearray):
            return str(o, encoding=BINARY_ENCODING)
        raise ValueError

    def polybytes(o):
        "Convert bytes or str to bytes with proper encoding."
        if isinstance(o, bytes):
            return o
        if isinstance(o, str):
            return bytes(o, encoding=BINARY_ENCODING)
        raise ValueError

    def make_std_wrapper(stream):
        "Standard input/output wrapper factory function"
        # This ensures that the encoding of standard output and standard
        # error on Python 3 matches the binary encoding we use to turn
        # bytes to Unicode in polystr above.
        #
        # newline="\n" ensures that Python 3 won't mangle line breaks
        # line_buffering=True ensures that interactive command sessions
        # work as expected
        return io.TextIOWrapper(stream.buffer, encoding=BINARY_ENCODING,
                                newline="\n", line_buffering=True)

    def get_bytes_stream(stream):
        "Standard input/output bytes buffer function"
        return stream.buffer


# some multipliers for interpreting GPS output
# Note: A Texas Foot is ( meters * 3937/1200)
#       (Texas Natural Resources Code, Subchapter D, Sec 21.071 - 79)
#       not the same as an international fooot.
METERS_TO_FEET = 3.28083989501312         # Meters to U.S./British feet
METERS_TO_MILES = 0.000621371192237334    # Meters to miles
METERS_TO_FATHOMS = 0.546806649168854     # Meters to fathoms
KNOTS_TO_MPH = 1.15077944802354           # Knots to miles per hour
KNOTS_TO_KPH = 1.852                      # Knots to kilometers per hour
KNOTS_TO_MPS = 0.514444444444445          # Knots to meters per second
MPS_TO_KPH = 3.6                          # Meters per second to klicks/hr
MPS_TO_MPH = 2.2369362920544              # Meters/second to miles per hour
MPS_TO_KNOTS = 1.9438444924406            # Meters per second to knots


def Deg2Rad(x):
    "Degrees to radians."
    return x * (math.pi / 180)


def Rad2Deg(x):
    "Radians to degrees."
    return x * (180 / math.pi)


def CalcRad(lat):
    "Radius of curvature in meters at specified latitude WGS-84."
    # the radius of curvature of an ellipsoidal Earth in the plane of a
    # meridian of latitude is given by
    #
    # R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2)
    #
    # where
    #   a   is the equatorial radius (surface to center distance),
    #   b   is the polar radius (surface to center distance),
    #   e   is the first  eccentricity of the ellipsoid
    #   e2  is e^2  = (a^2 - b^2) / a^2
    #   es  is the second eccentricity of the ellipsoid (UNUSED)
    #   es2 is es^2 = (a^2 - b^2) / b^2
    #
    # for WGS-84:
    # a   = 6378.137 km (3963 mi)
    # b   = 6356.752314245 km (3950 mi)
    # e2  = 0.00669437999014132
    # es2 = 0.00673949674227643
    a = 6378.137
    e2 = 0.00669437999014132
    sc = math.sin(math.radians(lat))
    x = a * (1.0 - e2)
    z = 1.0 - e2 * pow(sc, 2)
    y = pow(z, 1.5)
    r = x / y

    r = r * 1000.0      # Convert to meters
    return r


def EarthDistance(c1, c2):
    """
    Vincenty's formula (inverse method) to calculate the distance (in
    kilometers or miles) between two points on the surface of a spheroid
    WGS 84 accurate to 1mm!
    """

    (lat1, lon1) = c1
    (lat2, lon2) = c2

    # WGS 84
    a = 6378137  # meters
    f = 1 / 298.257223563
    b = 6356752.314245  # meters; b = (1 - f)a

    # MILES_PER_KILOMETER = 1000.0 / (.3048 * 5280.0)

    MAX_ITERATIONS = 200
    CONVERGENCE_THRESHOLD = 1e-12  # .000,000,000,001

    # short-circuit coincident points
    if lat1 == lat2 and lon1 == lon2:
        return 0.0

    U1 = math.atan((1 - f) * math.tan(math.radians(lat1)))
    U2 = math.atan((1 - f) * math.tan(math.radians(lat2)))
    L = math.radians(lon1 - lon2)
    Lambda = L

    sinU1 = math.sin(U1)
    cosU1 = math.cos(U1)
    sinU2 = math.sin(U2)
    cosU2 = math.cos(U2)

    for _ in range(MAX_ITERATIONS):
        sinLambda = math.sin(Lambda)
        cosLambda = math.cos(Lambda)
        sinSigma = math.sqrt((cosU2 * sinLambda) ** 2 +
                             (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) ** 2)
        if sinSigma == 0:
            return 0.0  # coincident points
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
        sigma = math.atan2(sinSigma, cosSigma)
        sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma
        cosSqAlpha = 1 - sinAlpha ** 2
        try:
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
        except ZeroDivisionError:
            cos2SigmaM = 0
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
        LambdaPrev = Lambda
        Lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma *
                                               (cos2SigmaM + C * cosSigma *
                                                (-1 + 2 * cos2SigmaM ** 2)))
        if abs(Lambda - LambdaPrev) < CONVERGENCE_THRESHOLD:
            break  # successful convergence
    else:
        # failure to converge
        # fall back top EarthDistanceSmall
        return EarthDistanceSmall(c1, c2)

    uSq = cosSqAlpha * (a ** 2 - b ** 2) / (b ** 2)
    A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
    deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (
        cosSigma * (-1 + 2 * cos2SigmaM ** 2) - B / 6 * cos2SigmaM *
        (-3 + 4 * sinSigma ** 2) * (-3 + 4 * cos2SigmaM ** 2)))
    s = b * A * (sigma - deltaSigma)

    # return meters to 6 decimal places
    return round(s, 6)


def EarthDistanceSmall(c1, c2):
    "Distance in meters between two close points specified in degrees."
    # This calculation is known as an Equirectangular Projection
    # fewer numeric issues for small angles that other methods
    # the main use here is for when Vincenty's fails to converge.
    (lat1, lon1) = c1
    (lat2, lon2) = c2
    avglat = (lat1 + lat2) / 2
    phi = math.radians(avglat)    # radians of avg latitude
    # meters per degree at this latitude, corrected for WGS84 ellipsoid
    # Note the wikipedia numbers are NOT ellipsoid corrected:
    # https://en.wikipedia.org/wiki/Decimal_degrees#Precision
    m_per_d = (111132.954 - 559.822 * math.cos(2 * phi) +
               1.175 * math.cos(4 * phi))
    dlat = (lat1 - lat2) * m_per_d
    dlon = (lon1 - lon2) * m_per_d * math.cos(phi)

    dist = math.sqrt(math.pow(dlat, 2) + math.pow(dlon, 2))
    return dist


def MeterOffset(c1, c2):
    "Return offset in meters of second arg from first."
    (lat1, lon1) = c1
    (lat2, lon2) = c2
    dx = EarthDistance((lat1, lon1), (lat1, lon2))
    dy = EarthDistance((lat1, lon1), (lat2, lon1))
    if lat1 < lat2:
        dy = -dy
    if lon1 < lon2:
        dx = -dx
    return (dx, dy)


def isotime(s):
    "Convert timestamps in ISO8661 format to and from Unix time."
    if isinstance(s, int):
        return time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
    elif isinstance(s, float):
        date = int(s)
        msec = s - date
        date = time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
        return date + "." + repr(msec)[3:]
    elif isinstance(s, STR_CLASS):
        if s[-1] == "Z":
            s = s[:-1]
        if "." in s:
            (date, msec) = s.split(".")
        else:
            date = s
            msec = "0"
        # Note: no leap-second correction!
        return calendar.timegm(
            time.strptime(date, "%Y-%m-%dT%H:%M:%S")) + float("0." + msec)
    else:
        raise TypeError

# End