Calculate spherical distance and azimuth of waypoints

calculate.py

#!/usr/bin/env python3

import numpy as np


def calc_params(points):
    """Calculate some shared intermediate parameters
    """
    points = np.array(points)
    curr_points, next_points = points[:-1, :], points[1:, :]

    Δ = next_points - curr_points
    Δ𝜑, Δ𝜆 = Δ[:, 0], Δ[:, 1]

    𝜑1, 𝜑2 = curr_points[:, 0], next_points[:, 0]

    sin𝜑1, cos𝜑1 = np.sin(𝜑1), np.cos(𝜑1)
    sin𝜑2, cos𝜑2 = np.sin(𝜑2), np.cos(𝜑2)

    return Δ𝜑, Δ𝜆, sin𝜑1, cos𝜑1, sin𝜑2, cos𝜑2


def calc_angle(points, *, decimals = 1, premable = True):
    """Calculate azimuth of waypoints

    Parameters:
        points:   2-dimentional array(N x 2), each line is a waypoint
        
        decimals: Significant digits count of the result
        premable: Add premable 0 to the first point

    Returns:
        Azimuth to next waypoint
    """
    Δ𝜑, Δ𝜆, sin𝜑1, cos𝜑1, sin𝜑2, cos𝜑2 = calc_params(points)

    sinΔ𝜆, cosΔ𝜆 = np.sin(Δ𝜆), np.cos(Δ𝜆)

    𝜃 = np.arctan2(sinΔ𝜆 * cos𝜑2, cos𝜑1 * sin𝜑2 - sin𝜑1 * cos𝜑2 * cosΔ𝜆) * 180 / np.pi
    𝜃 = np.where(𝜃 < 0, 𝜃 + 360, 𝜃)

    return np.around(np.insert(𝜃, 0, 0) if premable else 𝜃, decimals = decimals)


def calc_distance(points, *, decimals = 1, premable = True, radius = 6370):
    """Calculate spherical distance of waypoints

    Parameters:
        points:   2-dimentional array(N x 2), each line is a waypoint
        
        decimals: Significant digits count of the result
        premable: Add premable 0 to the first point
        radius:   Radius of pherical in kilometers

    Returns:
        Spherical distance to next waypoint in kilometers
    """
    Δ𝜑, Δ𝜆, sin𝜑1, cos𝜑1, sin𝜑2, cos𝜑2 = calc_params(points)

    𝑎 = np.sin(Δ𝜑 / 2) ** 2 + cos𝜑1 * cos𝜑2 * np.sin(Δ𝜆 / 2) ** 2
    𝛿 = np.arctan2(np.sqrt(𝑎), np.sqrt(1 - 𝑎)) * 2
    𝑑 = 𝛿 * radius 

    return np.around(np.insert(𝑑, 0, 0) if premable else 𝑑, decimals = decimals)


def print_distance_and_azimuth(points):
    points = np.array(points)
    angle, distance = calc_angle(points).astype(str), calc_distance(points).astype(str)

    centerize = lambda *strs, width = 14: [s.center(width) for s in strs]    

    print(*centerize('Waypoint', 'Distance', 'Azimuth'))
    
    for index, point in enumerate(zip(distance, angle)):
        print(*centerize(str(index + 1), point[0] + 'km', point[1] + '°'), end = ' ')
        print()