orbital.py

from collections import defaultdict, namedtuple
import numpy as np

def visible_cap_central_angle(altitude):
    earth_radius = 6371.0
    return np.arctan2(np.sqrt(altitude * (altitude + 2 * earth_radius)),
                      earth_radius)

def hav(x):
    return np.sin(x/2)**2

def inv_hav(x):
    return np.arcsin(np.sqrt(x))*2

def angle_between(location0, location1):
    lat0, lon0 = location0
    lat1, lon1 = location1
    x = hav(lat1 - lat0) + np.cos(lat0) * np.cos(lat1) * hav(lon1 - lon0)
    return inv_hav(x)

Satellite = namedtuple('Satellite', ['lat', 'lon', 'name', 'central_angle'])

def sat(name, lat, lon, alt):
    return Satellite(name=name, lat=lat, lon=lon,
                     central_angle=visible_cap_central_angle(alt))

def visible(x, y):
    angle = angle_between((x.lat, x.lon), (y.lat, y.lon))
    return angle < x.central_angle + y.central_angle

def read_data(filename):
    nodes = set()
    with open(filename) as data:
        for line in data:
            if line.startswith('#'):
                continue
            fields = line.split(',')
            if fields[0].startswith('SAT'):
                lat, lon, alt = map(float, fields[1:])
                nodes.add(sat(fields[0], np.radians(lat), np.radians(lon), alt))
            elif fields[0] == 'ROUTE':
                lat0, lon0, lat1, lon1 = map(np.radians, map(float, fields[1:]))
                nodes.add(sat('source', lat0, lon0, 0))
                nodes.add(sat('destination', lat1, lon1, 0))
            else:
                assert False, line

    adjacent = defaultdict(list)
    for x in nodes:
        for y in nodes - set(x):
            if visible(x, y):
                adjacent[x.name].append(y.name)
    return adjacent

def dijkstra(adjacent, source, destination):
    unvisited = set(adjacent.keys())
    distance = { k: np.Infinity for k in adjacent.keys() }
    path = { k: None for k in adjacent.keys() }
    distance[source] = 0
    path[source] = []
    
    while unvisited:
        min_dist = min(distance[n] for n in unvisited)
        node = [ n for n in unvisited if distance[n] == min_dist ][0]
        unvisited.remove(node)
        
        for neighbor in adjacent[node]:
            new_dist = distance[node] + 1
            if new_dist < distance[neighbor]:
                distance[neighbor] = new_dist
                path[neighbor] = path[node] + [node]
    
    return distance[destination], path[destination]

if __name__ == '__main__':
    adjacent = read_data('data.csv')
    dist, path = dijkstra(adjacent, 'source', 'destination')
    print(dist, ','.join(path[1:]))