Geodesic Distance Triangulation in Python

#! /usr/bin/env python

''' Geodesic distance triangulation

    Find two locations that are at given distances
    from two fixed points, using the WGS84 reference ellipsoid

    First approximate using spherical trig, then refine
    using Newton's method.

    Uses geographiclib by Charles Karney http://geographiclib.sourceforge.net
    which can be installed using pip.

    Written by PM 2Ring 2015.06.20
'''

import sys, getopt
from operator import itemgetter
from math import degrees, radians, sin, cos, acos, atan2, pi
from random import seed, random

from geographiclib.geodesic import Geodesic

Geo = Geodesic.WGS84

#60 international nautical miles in metres
metres_per_deg = 111120.0

# Tolerance in calculated distances, in metres.
# Try adjusting this value if the geo_newton function fails to converge.
distance_tol = 1E-8

# Tolerance in calculated positions, in degrees.
position_tol = 5E-10

# Crude floating-point equality test
def almost_equal(a, b, tol=5E-12):
    return abs(a - b) < tol


def normalize_lon(x, a):
    ''' reduce longitude to range -a <= x < a '''
    return (x + a) % (2 * a) - a


class LatLong(object):
    ''' latitude & longitude in degrees & radians.
        Also colatitude in radians.
    '''
    def __init__(self, lat, lon, in_radians=False):
        ''' Initialize with latitude and longitude,
            in either radians or degrees
        '''
        if in_radians:
            lon = normalize_lon(lon, pi)
            self.lat = lat
            self.colat = 0.5 * pi - lat
            self.lon = lon
            self.dlat = degrees(lat)
            self.dlon = degrees(lon)
        else:
            lon = normalize_lon(lon, 180.0)
            self.lat = radians(lat)
            self.colat = radians(90.0 - lat)
            self.lon = radians(lon)
            self.dlat = lat
            self.dlon = lon

    def __repr__(self):
        return 'LatLong(%.12f, %.12f, in_radians=True)' % (self.lat, self.lon)

    def __str__(self):
        return 'Lat: %.9f, Lon: %.9f' % (self.dlat, self.dlon)

# -------------------------------------------------------------------
# Spherical triangle solutions based on the spherical cos rule
# cos(c) = cos(a) * cos(b) + sin(a) * sin(b) * cos(C)
# Side calculations use functions that are less susceptible
# to round-off errors, from
# https://en.wikipedia.org/wiki/Solution_of_triangles#Two_sides_and_the_included_angle_given

def opp_angle(a, b, c):
    ''' Given sides a, b & c find C, the angle opposite side c '''
    t = (cos(c) - cos(a) * cos(b)) / (sin(a) * sin(b))
    try:
        C = acos(t)
    except ValueError:
        # "Reflect" t if it's out of bounds due to rounding errors.
        # This happens roughly 1% of the time, on average.
        if t > 1.0:
            t = 2.0 - t
        elif t < -1.0:
            t = -2.0 - t
        C = acos(t)
    return C


def opp_side(a, b, C):
    ''' Find side c given sides a, b and the included angle C '''
    sa, ca = sin(a), cos(a)
    sb, cb = sin(b), cos(b)
    sC, cC = sin(C), cos(C)

    u = sa * cb - ca * sb * cC
    v = sb * sC
    num = (u ** 2 + v ** 2) ** 0.5
    den = ca * cb + sa * sb * cC
    return atan2(num, den)

def opp_side_azi(a, b, C):
    ''' Find side c given sides a, b and the included angle C
        Also find angle A
    '''
    sa, ca = sin(a), cos(a)
    sb, cb = sin(b), cos(b)
    sC, cC = sin(C), cos(C)

    u = sa * cb - ca * sb * cC
    v = sb * sC
    num = (u ** 2 + v ** 2) ** 0.5
    den = ca * cb + sa * sb * cC

    # side c, angle A
    return atan2(num, den), atan2(v, u)


def gc_distance(p, q):
    ''' The great circle distance between two points '''
    return opp_side(p.colat, q.colat, q.lon - p.lon)

def gc_distance_azi(p, q):
    ''' The great circle distance between two points & azimuth at p '''
    return opp_side_azi(p.colat, q.colat, q.lon - p.lon)


def azi_dist(p, azi, dist):
    ''' Find point x given point p, azimuth px, and distance px '''
    x_colat, delta = opp_side_azi(p.colat, dist, azi)
    x = LatLong(0.5 * pi - x_colat, p.lon + delta, in_radians=True)
    return x


def tri_test(*sides):
    ''' Triangle inequality.

        Check that the longest side is not greater than
        the sum of the other 2 sides.
        Return None if triangle ok, otherwise return
        the longest side's index & length and the excess.
    '''
    i, m = max(enumerate(sides), key=itemgetter(1))
    # m > (a + b) -> 2*m > (m + a + b)
    excess = 2 * m - sum(sides)
    if excess > 0:
        return i, m, excess
    return None


def gc_triangulate(a, ax_dist, b, bx_dist, verbose=0):
    ''' Great circle distance triangulation

        Given points a & b find the two points x0 & x1 that
        are both ax_dist & bx_dist, respectively, from a & b.
        Distances are in degrees.
    '''
    # Distance AB, the base of the OAB triangle
    ab_dist, ab_azi = gc_distance_azi(a, b)
    ab_dist_deg = degrees(ab_dist)
    if verbose > 1:
        print ('AB distance: %f\nAB azimuth: %f' %
            (ab_dist_deg, degrees(ab_azi)))

    # Make sure sides of ABX obey triangle inequality
    bad = tri_test(ab_dist_deg, ax_dist, bx_dist)
    if bad is not None:
        print 'Bad gc side length %d: %f, excess = %f' % bad
        raise ValueError

    # Now we need the distance args in radians
    ax_dist, bx_dist = radians(ax_dist), radians(bx_dist)

    # Angle BAX
    bax = opp_angle(ax_dist, ab_dist, bx_dist)
    if verbose > 1:
        print 'Angle BAX: %f\n' % degrees(bax)

    # OAX triangle, towards the pole
    ax_azi = ab_azi - bax
    if verbose > 1:
        print 'AX0 azimuth: %f' % degrees(ax_azi)
    x0 = azi_dist(a, ax_azi, ax_dist)

    # OAX triangle, away from the pole
    ax_azi = ab_azi + bax
    if verbose > 1:
        print 'AX1 azimuth: %f\n' % degrees(ax_azi)
    x1 = azi_dist(a, ax_azi, ax_dist)
    return x0, x1

# -------------------------------------------------------------------
# Geodesic stuff using geographiclib & the WGS84 ellipsoid

# default keys returned by Geo.Inverse
# ['a12', 'azi1', 'azi2', 'lat1', 'lat2', 'lon1', 'lon2', 's12']
# full set of keys
# ['M12', 'M21', 'S12', 'a12', 'azi1', 'azi2',
# 'lat1', 'lat2', 'lon1', 'lon2', 'm12', 's12']

def geo_distance(p, q):
    ''' The geodesic distance between two points '''
    return Geo.Inverse(p.dlat, p.dlon, q.dlat, q.dlon)['s12']


# Meridional radius of curvature and Radius of circle of latitude,
# multiplied by (pi / 180.0) to simplify calculation of partial derivatives
# of geodesic length wrt latitude & longitude in degrees
# Geo._a : equatorial radius of the ellipsoid, in metres
# Geo._e2 : eccentricity ** 2 of the ellipsoid
def rho_R(lat):
    lat = radians(lat)
    w2 = 1.0 - Geo._e2 * sin(lat) ** 2
    w = w2 ** 0.5

    # Meridional radius of curvature
    rho = Geo._a * (1.0 - Geo._e2) / (w * w2)

    # Radius of circle of latitude; a / w is the normal radius of curvature
    R = Geo._a * cos(lat) / w
    return radians(rho), radians(R)


def normalize_lat_lon(lat, lon):
    ''' Fix latitude & longitude if abs(lat) > 90 degrees,
        i.e., the point has gone over the pole.
        Also normalize longitude to -180 <= x < 180
    '''
    if lat > 90.0:
        lat = 180.0 - lat
        lon = -lon
    elif lat < -90.0:
        lat = -180.0 - lat
        lon = -lon
    lon = normalize_lon(lon, 180.0)
    return lat, lon


def geo_newton(a, b, x, ax_dist, bx_dist, verbose=0):
    ''' Solve a pair of simultaneous geodesic distance equations
        using Newton's method.
        Refine an initial approximation of point x such that
        the distance from point a to x is ax_dist, and
        the distance from point b to x is bx_dist
    '''
    # Original approximations
    x_dlat, x_dlon = x.dlat, x.dlon

    # Typically, 4 or 5 loops are adequate.
    for i in xrange(30):
        # Find geodesic distance from a to x, & azimuth at x
        d = Geo.Inverse(a.dlat, a.dlon, x_dlat, x_dlon)
        f0, a_azi = d['s12'], d['azi2']

        # Find geodesic distance from b to x, & azimuth at x
        d = Geo.Inverse(b.dlat, b.dlon, x_dlat, x_dlon)
        g0, b_azi = d['s12'], d['azi2']

        #Current errors in f & g
        delta_f = ax_dist - f0
        delta_g = bx_dist - g0
        if verbose > 1:
            print i, ': delta_f =', delta_f, ', delta_g =', delta_g

        if (almost_equal(delta_f, 0, distance_tol) and
            almost_equal(delta_g, 0, distance_tol)):
            if verbose and i > 9: print 'Loops =', i
            break

        # Calculate partial derivatives of f & g
        # wrt latitude & longitude in degrees
        a_azi = radians(a_azi)
        b_azi = radians(b_azi)
        rho, R = rho_R(x_dlat)
        f_lat = rho * cos(a_azi)
        g_lat = rho * cos(b_azi)
        f_lon = R * sin(a_azi)
        g_lon = R * sin(b_azi)

        # Generate new approximations of x_dlat & x_dlon
        den = f_lat * g_lon - f_lon * g_lat
        dd_lat = delta_f * g_lon - f_lon * delta_g
        dd_lon = f_lat * delta_g - delta_f * g_lat
        x_dlat += dd_lat / den
        x_dlon += dd_lon / den
        x_dlat, x_dlon = normalize_lat_lon(x_dlat, x_dlon)
    else:
        print ('Warning: Newton approximation loop fell through '
        'without finding a solution after %d loops.' % (i + 1))

    return LatLong(x_dlat, x_dlon)


def geo_triangulate(a, ax_dist, b, bx_dist, verbose=0):
    ''' Geodesic Triangulation

        Given points a & b find the two points x0 & x1 that
        are both ax_dist & bx_dist, respectively from a & b.
        Distances are in metres.
    '''
    ab_dist = geo_distance(a, b)
    if verbose:
        print 'ab_dist =', ab_dist

    # Make sure sides of ABX obey triangle inequality
    bad = tri_test(ab_dist, ax_dist, bx_dist)
    if bad is not None:
        print 'Bad geo side length %d: %f, excess = %f' % bad
        raise ValueError

    # Find approximate great circle solutions.
    # Distances need to be given in degrees, so we approximate them
    # using 1 degree = 60 nautical miles
    ax_deg = ax_dist / metres_per_deg
    bx_deg = bx_dist / metres_per_deg
    ab_deg = degrees(gc_distance(a, b))

    # Make sure that the sides of the great circle triangle
    # obey the triangle inequality
    bad = tri_test(ab_deg, ax_deg, bx_deg)
    if bad is not None:
        if verbose > 1:
            print ('Bad gc side length after conversion '
                '%d: %f, excess = %f. Adjusting...' % bad)
        i, _, excess = bad
        if i == 1:
            ax_deg -= excess
            bx_deg += excess
        elif i == 2:
            bx_deg -= excess
            ax_deg += excess
        else:
            ax_deg += excess
            bx_deg += excess
        #bad = tri_test(ab_deg, ax_deg, bx_deg)
        #if bad:
            #print 'BAD ', bad
            #print (ab_deg, ax_deg, bx_deg)
            #print a, ax_dist, b, bx_dist

    x0, x1 = gc_triangulate(a, ax_deg,  b, bx_deg, verbose=verbose)

    if verbose:
        ax = geo_distance(a, x0)
        bx = geo_distance(b, x0)
        print '\nInitial X0:', x0
        print 'ax0 =', ax, ', error =', ax - ax_dist
        print 'bx0 =', bx, ', error =', bx - bx_dist

    # Use Newton's method to get the true position of x0
    x0 = geo_newton(a, b, x0, ax_dist, bx_dist, verbose=verbose)

    if verbose:
        ax = geo_distance(a, x1)
        bx = geo_distance(b, x1)
        print '\nInitial X1:', x1
        print 'ax1 =', ax, ', error =', ax - ax_dist
        print 'bx1 =', bx, ', error =', bx - bx_dist
        print

    # Use Newton's method to get the true position of x1
    x1 = geo_newton(a, b, x1, ax_dist, bx_dist, verbose=verbose)
    return x0, x1

# -------------------------------------------------------------------

def rand_lat_lon(maxlat=90.0, maxlon=180.0):
    lat = 2 * maxlat * random() - maxlat
    lon = 2 * maxlon * random() - maxlon
    return lat, lon

def test(numtests, verbose=0):
    print 'Testing...'
    for i in xrange(1, numtests+1):
        if verbose:
            print '\n### Test %d ###' % i

        # Generate 3 random points
        alat, alon = rand_lat_lon()
        blat, blon = rand_lat_lon()
        xlat, xlon = rand_lat_lon()

        # Skip if any points are too close to each other
        if ((almost_equal(alat, blat, position_tol)
         and almost_equal(alon, blon, position_tol)) or
            (almost_equal(alat, xlat, position_tol)
         and almost_equal(alon, xlon, position_tol)) or
            (almost_equal(blat, xlat, position_tol)
         and almost_equal(blon, xlon, position_tol))):
            continue

        a = LatLong(alat, alon)
        b = LatLong(blat, blon)
        x = LatLong(xlat, xlon)
        ax = geo_distance(a, x)
        bx = geo_distance(b, x)

        if verbose:
            print ('A: %s\nB: %s\nX: %s\nax = %.15f\nbx = %.15f\n' %
            (a, b, x, ax, bx))

        x0, x1 = geo_triangulate(a, ax, b, bx, verbose=verbose)
        if verbose:
            print 'X0', x0
            print 'X1', x1

        if (almost_equal(x0.dlat, x1.dlat, position_tol) and
            almost_equal(x0.dlon, x1.dlon, position_tol)):
            print (' %d Warning: only 1 solution found\n'
                'x0 %.15f, %.15f\nx1 %.15f, %.15f'
                % (i, x0.dlat, x0.dlon, x1.dlat, x1.dlon))

        # Verify that one of the computed x's is (almost) in the
        # same position as the original x
        if not (
            (almost_equal(x0.dlat, xlat, position_tol) and
             almost_equal(x0.dlon, xlon, position_tol)) or
            (almost_equal(x1.dlat, xlat, position_tol) and
             almost_equal(x1.dlon, xlon, position_tol))):
            print (' %d Warning: Bad x\nx  %.15f, %.15f\nx0 %.15f, %.15f\n'
                'x1 %.15f, %.15f\na %.15f %.15f; b %.15f %.15f\n'
                'ax %.15f, bx %.15f' % (i, x.dlat, x.dlon,
                x0.dlat, x0.dlon, x1.dlat, x1.dlon,
                a.dlat, a.dlon, b.dlat, b.dlon, ax, bx))

        # Calculate the distances
        ax0 = geo_distance(a, x0)
        ax1 = geo_distance(a, x1)
        bx0 = geo_distance(b, x0)
        bx1 = geo_distance(b, x1)

        if verbose:
            print 'ax0 = %.15f\nbx0 = %.15f' % (ax0, bx0)
            print 'ax1 = %.15f\nbx1 = %.15f' % (ax1, bx1)

        # Verify the distances
        if not almost_equal(ax0, ax, distance_tol):
            print ' %d ax0 error: %.15f, %.15f' % (i, ax0, ax)

        if not almost_equal(ax1, ax, distance_tol):
            print ' %d ax1 error: %.15f, %.15f' % (i, ax1, ax)

        if not almost_equal(bx0, bx, distance_tol):
            print ' %d bx0 error: %.15f, %.15f' % (i, bx0, bx)

        if not almost_equal(bx1, bx, distance_tol):
            print ' %d bx1 error: %.15f, %.15f' % (i, bx1, bx)

        if not verbose and i % 10 == 0:
            sys.stderr.write('\r %d ' % i)
    if not verbose: sys.stderr.write('\n')

# -------------------------------------------------------------------

def usage(msg=None):
    s = 'Error: %s\n\n' % msg if msg != None else ''
    s += ('Geodesic distance triangulation.\n'
        'Find two locations that are at given WGS84 '
        'geodesic distances from two fixed points.\n\n'

        'Usage\n\n'
        'Print this help:\n'
        ' {0} -h\n\n'

        'Normal mode:\n'
        ' {0} [-v verbosity] -- A_lat A_lon A_dist  B_lat B_lon B_dist  '
        '[X_lat X_lon]...\n\n'

        'Test mode: run tests using sets of 3 random points\n'
        ' {0} [-v verbosity] [-s rand_seed] -t num_tests\n\n'

        'In normal mode, position arguments are in decimal degrees '
        '(N/E +ve, S/W -ve), distances are in metres.\n'
        'The end-of-options marker -- should precede the position arguments '
        'so that negative latitudes or longitudes\n'
        'are not misinterpreted as invalid options.\n\n'

        'This program generates initial approximate solutions using '
        'spherical trigonometry.\n'
        'These solutions are then refined using Newton\'s method to '
        'obtain geodesic solutions.\n'
        'Alternatively, any number of approximate solutions X_lat X_lon '
        'may follow the A & B positions & distances.\n\n'

        'In test mode, if no seed string is given system time is used '
        'to generate a random seed string.\n'
        'This string is printed to simplify repeating such test series.\n\n'

        'The verbosity option is a number from 0 to 2.\n'
        'A non-zero verbosity tells the program to print various significant '
        'parameters; the higher the verbosity, the more gets printed.\n'
        'Please see the source code for details.'.format(sys.argv[0]))
    print s
    exit()

def main():
    # Default args
    verbose = 0
    numtests = 0
    rand_seed = None

    try:
        opts, args = getopt.getopt(sys.argv[1:], "hv:t:s:")
    except getopt.GetoptError as e:
        usage(e.msg)

    for o, a in opts:
        if o == "-h": usage()
        elif o == "-v": verbose = int(a)
        elif o == "-t": numtests = int(a)
        elif o == "-s": rand_seed = a

    # Test mode
    if numtests > 0:
        if rand_seed is None:
            seed(None)
            rand_seed = str(random())
        print 'seed', rand_seed
        seed(rand_seed)
        test(numtests, verbose=verbose)
        exit()

    # Normal mode
    numargs = len(args)
    if numargs < 6 or numargs % 2 != 0:
        usage('Bad number of arguments for normal mode.')

    try:
        args = [float(s) for s in args]
    except ValueError as e:
        usage(e)

    a_lat, a_lon, ax_dist, b_lat, b_lon, bx_dist = args[:6]
    a = LatLong(a_lat, a_lon)
    b = LatLong(b_lat, b_lon)

    args = args[6:]
    if not args:
        # No initial approximations given; (try to) find 2 solutions
        # via great circle geo_triangulation.
        x0, x1 = geo_triangulate(a, ax_dist, b, bx_dist, verbose=verbose)
        for i, x in enumerate((x0, x1)):
            print 'X%d: %s' % (i, x)
            ax = geo_distance(a, x)
            bx = geo_distance(b, x)
            print 'AX=%.15f, BX=%.15f\n' % (ax, bx)
    else:
        #Collect X latitude, longitude pairs into points
        args = zip(*[iter(args)] * 2)
        xlist = [LatLong(*t) for t in args]

        #Process each given initial approximation.
        for i, x in enumerate(xlist):
            print 'Initial X%d: %s' % (i, x)
            x = geo_newton(a, b, x, ax_dist, bx_dist, verbose=verbose)
            print '  Final X%d: %s' % (i, x)
            ax = geo_distance(a, x)
            bx = geo_distance(b, x)
            print 'AX=%.15f, BX=%.15f\n' % (ax, bx)


if __name__ == '__main__':
    main()