Vincenty's distance Direct formulae numpy

Question

I've refactored a function from the pygc library used to generate the great_circle. The Vincenty's equation below can be found here.

Destination given distance & bearing from start point (direct solution) Again, this presents Vincenty’s formulæ arranged to be close to how they are used in the script.

$$ \begin{align} a, b =&\ \textrm{major & minor semi-axes of the ellipsoid} \\ f =&\ \textrm{flattening }(a−b)\ /\ a \\ φ_1, φ_2 =&\ \textrm{geodetic latitude} \\ L =&\ \textrm{difference in longitude} \\ s =&\ \textrm{length of the geodesic along the surface of the ellipsoid (in the same units as }a \textrm{ & } b \textrm{)} \\ α1, α2 =&\ \textrm{azimuths of the geodesic (initial/final bearing)} \\ \\ \tan U_1 =&\ (1−f) \cdot \tan φ_1 &\textrm{(U is ‘reduced latitude’)} \\ \cos U_1 =&\ 1 / \sqrt{1 + \tan^2 U_1},\qquad \sin U_1 = \tan U_1 \cdot \cos U_1 &\textrm{(trig identities; §6)} \\ \sigma_1 =&\ \arctan(\tan U_1 / \cos \alpha_1) \\ \sin α =&\ \cos U_1 \cdot \sin α_1 &(2) \\ \cos^2 α =&\ 1 − \sin^2 α &\textrm{(trig identity; §6)} \\ u^2 =&\ \cos^2 α \cdot \frac{a^2−b^2}{b^2} \\ A =&\ 1 + \frac{u^2}{16384} \cdot \left\{4096 + u^2 \cdot [−768 + u^2 · (320 − 175 \cdot u^2)]\right\} & (3) \\ B =&\ u^2/1024 \cdot \left\{256 + u^2 · [−128 + u^2 · (74 − 47 · u^2)]\right\} & (4) \\ σ =&\ \frac{s}{b \cdot A} & \textrm{(first approximation)} \\ \end{align} \\ $$

iterate until change in σ is negligible (e.g. 10^-12 ≈ 0.006mm) { $$ \begin{align} \cos 2σ_m =&\ \cos(2σ_1 + σ) & (5) \\ Δσ =&\ B \cdot \sin σ \cdot \left\{\cos 2σ_m + B/4 · [\cos σ · (−1 + 2 \cdot \cos^2 2σ_m) \\ − B/6 \cdot \cos 2σ_m · (−3 + 4 \cdot \sin^2 σ) · (−3 + 4 · cos² 2σ_m)]\right\} & (6) \\ σʹ =&\ s / b·A + Δσ & (7) \\ \end{align} $$ } $$ \begin{align} φ_2 =&\ \arctan(\sin U_1 \cdot \cos σ + \cos U_1 \cdot \sin σ \cdot \cos α_1 \\ &\ / (1−f) \cdot \sqrt{\sin^2 α + (\sin U_1 \cdot \sin σ − \cos U_1 \cdot \cos σ · \cos α_1)^2}) & (8) \\ λ =&\ \arctan(\sin σ · \sin α_1 / \cos U_1 · \cos σ − \sin U_1 · \sin σ \cdot \cos α_1) & (9) \\ C =&\ f/16 \cdot \cos^2 α · [4 + f · (4 − 3 \cdot \cos^2 α)] & (10) \\ L =&\ λ − (1−C) \cdot f \cdot \sin α · \left\{σ + C \cdot \sin σ \cdot [\cos 2σ_m + C · \cos σ · (−1 + 2 \cdot \cos^2 2σ_m)]\right\} & (11) \\ λ_2 =&\ λ_1 + L \\ α_2 =&\ \arctan\left( \frac{\sin α}{−(\sin U_1 · \sin σ − \cos U_1 · \cos σ \cdot \cos α_1)}\right) & (12) \\ \end{align} $$

Where:

• \$φ_2, λ_2\$ is destination point
• \$α_2\$ is final bearing (in direction \$p_1 \rightarrow p_2\$)

constants and imports

"""Vincenty'distance Direct formulae"""
from typing import Tuple
from numpy import (
    vectorize,
    arctan,
    arctan2,
    tan,
    sin,
    cos,
    sqrt,
    pi
)
# a: Semi-major axis = 6 378 137.0  metres
A = 6_378_137.0
# b: Semi-minor axis ≈ 6 356 752.314 245    metres
B = 6_356_752.314_245
# f = flattening (a−b)/a
F = (A - B) / A
# (a²−b²)/b² ( lat reduction )
F2 = (pow(A, 2) - pow(B, 2)) / pow(B, 2)

TWO_PI = 2.0 * pi

function

upsilon2: Callable[[float], Tuple[float, float]] = (lambda u2: (
    # A = 1 + u²/16384 · {4096 + u² · [−768 + u² · (320 − 175 · u²)]}
    (1 + (u2 / 16384) * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))),
    # B = u²/1024 · {256 + u² · [−128 + u² · (74 − 47 · u²)]}
    ((u2 / 1024) * (256 + u2 * (-128 + u2 * (74 - 47 * u2))))
))


def vincenty_direct(
        latitude: float,
        longitude: float,
        azimuth: float,
        distance: float) -> Tuple[float, float]:
    """
    Returns: lat and long of projected point
    """

    azimuth = azimuth + TWO_PI if azimuth < 0.0 else(
        azimuth - TWO_PI if azimuth > TWO_PI else azimuth
    )
    # tan U1 = (1−f) · tan φ1
    tan_u1 = (1 - F) * tan(latitude)
    # cos U1 = 1 / √1 + tan² U1, sin U1 = tan U1 · cos U1
    cos_u1 = arctan(tan_u1)
    # σ1 = atan(tan U1 / cos α1)
    sigma1 = arctan2(tan_u1, cos(azimuth))
    # sin α = cos U1 · sin α1
    sin_alpha = cos(cos_u1) * sin(azimuth)
    # cos² α = 1 − sin² α
    cos2_alpha = 1 - pow(sin_alpha, 2)
    (
        # A = 1 + u²/16384 · {4096 + u² · [−768 + u² · (320 − 175 · u²)]}
        alpha,
        # B = u²/1024 · {256 + u² · [−128 + u² · (74 − 47 · u²)]}   (4)
        beta
        # u² = cos² α · (a²−b²)/b²
    ) = upsilon2(cos2_alpha * F2)

    def sigma_recursion(sigma_0: float) -> Tuple[float, float]:
        # cos 2σm = cos(2σ1 + σ)
        cos_2sigma_m: float = cos(2 * sigma1 + sigma_0)
        # Δσ = B · sin σ · {cos 2σm + B/4 · [cos σ · (−1 + 2 · cos² 2σm)
        # − B/6 · cos 2σm · (−3 + 4 · sin² σ) · (−3 + 4 · cos² 2σm)]}
        delta_sigma: float = (
            # B · sin σ ·
            beta * sin(sigma_0) *
            # cos 2σm + B/4 ·
            (cos_2sigma_m + (beta / 4) *
             # [cos σ ·
             (cos(sigma_0) *
              #  (−1 + 2 · cos² 2σm) -
              (-1 + 2 * pow(cos_2sigma_m, 2) -
               #   − B/6 · cos 2σm ·
               (beta / 6) * cos_2sigma_m *
               # (−3 + 4 · sin² σ) ·
               (-3 + 4 * pow(sin(sigma_0), 2)) *
               # (−3 + 4 · cos² 2σm)]
               (-3 + 4 * pow(cos_2sigma_m, 2)))
              )
             ))
        # σʹ = s / b·A + Δσ
        sigma = (distance / (B * alpha)) + delta_sigma
        # iterate until change in σ is negligible (e.g. 10-12 ≈ 0.006mm)
        if abs((sigma_0 - sigma) / sigma) > 1.0e-9:
            sigma_recursion(sigma)
        return sigma, cos_2sigma_m

    sigma, cos_2sigma_m = sigma_recursion(
        # σ = s / (b·A) (first approximation)
        (distance / (B * alpha)))

    # φ2 = atan(sin U1 · cos σ + cos U1 · sin σ · cos α1 /
    latitude = arctan2(
        (sin(cos_u1) * cos(sigma) + cos(cos_u1) * sin(sigma) * cos(azimuth)),
        # (1−f) · √sin² α + (sin U1 · sin σ − cos U1 · cos σ · cos α1)² )
        ((1 - F) * sqrt(pow(sin_alpha, 2) +
         pow(sin(cos_u1) * sin(sigma) - cos(cos_u1) * cos(sigma) * cos(azimuth), 2)))
    )

    def omega():
        # λ = atan(sin σ · sin α1 / cos U1 · cos σ − sin U1 · sin σ · cos α1)
        _lambda = arctan2(
            (sin(sigma) * sin(azimuth)),
            (cos(cos_u1) * cos(sigma) - sin(cos_u1) * sin(sigma) * cos(azimuth))
        )
        # C = f/16 · cos² α · [4 + f · (4 − 3 · cos² α)]
        c_sigma = (
            (F / 16) * cos2_alpha *
            (4 + F * (4 - 3 * cos2_alpha))
        )
        # L = λ − (1−C) · f · sin α · {σ + C · sin σ · [cos 2σm + C · cos σ · (−1 + 2 · cos² 2σm)]}
        return (
            _lambda - (1 - c_sigma) * F * sin_alpha * (
                sigma + c_sigma * sin(sigma) * (
                    cos_2sigma_m + c_sigma *
                    cos(sigma) * (-1 + 2 * pow(cos_2sigma_m, 2))
                )))
        # return longitude + omega

    return latitude, longitude + omega()


direct = vectorize(vincenty_direct)

usage

import numpy as np
from vincenty import direct

def dev_dist(project_seconds=np.array([900, 1800, 2700, 3600])):
    """dev"""
    # position
    latitude = 33.01
    longitude = -98.94
    # direction
    azimuth = -171.95
    # speed
    meters_per_second = 11
    # distance projection
    distance = meters_per_second*project_seconds
    # in rads
    rad_lat, rad_lon, rad_azi = np.deg2rad((latitude, longitude, azimuth))
    # 
    degs = np.around(np.rad2deg(
        direct(rad_lat, rad_lon, rad_azi, distance)), decimals=2)

    points = np.swapaxes(degs, 0, 1)

    times = [f"+{x:02.0f}min"for x in project_seconds/60]

    projection = dict(zip(times, points.tolist()))


if __name__ == '__main__':
    dev_dist()

distance (meters)
>>> [ 9900 19800 29700 39600]
degs 
>>> [[ 32.92  32.83  32.74  32.66]
     [-98.95 -98.97 -98.98 -99.  ]]
points
>>> [[ 32.92 -98.95]
     [ 32.83 -98.97]
     [ 32.74 -98.98]
     [ 32.66 -99.  ]]
projection
>>> {'+15min': [32.92, -98.95], '+30min': [32.83, -98.97], '+45min': [32.74, -98.98], '+60min': [32.66, -99.0]}

Answer 1

Not a great idea to from numpy import, since there are so many symbols needed that that can lead to significant namespace pollution. The typical approach is just import numpy as np.

Rather than pow(, 2) you can just **2.

upsilon2 should not be a lambda, and should be a regular function instead.

The angle normalisation of

    azimuth = azimuth + TWO_PI if azimuth < 0.0 else(
        azimuth - TWO_PI if azimuth > TWO_PI else azimuth
    )

is not necessary, and even if it were, you should just do

azimuth = np.mod(azimuth, TWO_PI)

Don't recurse! Python has a very shallow stack capacity and no tail optimization. Just iterate - and this is quite easy with your existing function.

Your indentation sometimes deviates from 4 spaces. Try to keep this consistent.

Rather than _lambda, to escape a keyword it's more frequent to see lambda_.

Don't vectorize(vincenty_direct). You can write an actual vectorised implementation with very little modification to your original implementation.

Add some tests. The only ones that I have shown are to test against regression.

Don't np.around. Keep full precision. There are better ways to truncate precision for print if that's what you're looking for.

Your use of .swapaxes can be replaced with .T.

You would benefit from being a little more granular with your functions, cutting them out of closure scope to make dependencies more explicit. Other benefits are that profiling - if that ever becomes necessary - is much easier.

Suggested

"""Vincenty's distance Direct formulae"""
from pprint import pprint
import numpy as np

# a: Semi-major axis = 6_378_137.0  metres
A = 6_378_137.0
# b: Semi-minor axis ≈ 6_356_752.314_245    metres
B = 6_356_752.314_245
# f = flattening (a−b)/a
F = 1 - B/A
# (a²−b²)/b² ( lat reduction )
F2 = (A / B)**2 - 1

TWO_PI = 2 * np.pi


def get_upsilon2(u2: float) -> tuple[float, float]:
    # u² = cos² α · (a²−b²)/b²
    return (
        # A = 1 + u²/16384 · {4096 + u² · [−768 + u² · (320 − 175 · u²)]}
        1 + u2/16384 * (4096 + u2 * (-768 + u2 * (320 - 175*u2))),
        # B = u²/1024 · {256 + u² · [−128 + u² · (74 − 47 · u²)]}
        u2/1024 * (256 + u2 * (-128 + u2 * (74 - 47*u2))),
    )


def sigma_iteration(sigma_0: np.ndarray, sigma1, alpha, beta, distance) -> tuple[np.ndarray, np.ndarray]:
    # cos 2σm = cos(2σ1 + σ)
    cos_2sigma_m = np.cos(2 * sigma1 + sigma_0)
    # Δσ = B · sin σ · {cos 2σm + B/4 · [cos σ · (−1 + 2 · cos² 2σm)
    # − B/6 · cos 2σm · (−3 + 4 · sin² σ) · (−3 + 4 · cos² 2σm)]}
    delta_sigma = (
        # B · sin σ ·
        beta * np.sin(sigma_0) *
        # cos 2σm + B/4 ·
        (
            cos_2sigma_m + beta/4 *
            # [cos σ ·
            np.cos(sigma_0) *
            (
                #  (−1 + 2 · cos² 2σm) -
                -1 + 2*cos_2sigma_m**2 -
                #   − B/6 · cos 2σm ·
                beta/6 * cos_2sigma_m *
                # (−3 + 4 · sin² σ) ·
                (-3 + 4*np.sin(sigma_0)**2) *
                # (−3 + 4 · cos² 2σm)]
                (-3 + 4*cos_2sigma_m**2)
            )
        )
    )
    # σʹ = s / b·A + Δσ
    sigma = distance/B/alpha + delta_sigma
    return sigma, cos_2sigma_m


def get_sigma(distance: np.ndarray, alpha: float, beta: float, sigma1: float) -> tuple[np.ndarray, np.ndarray]:
    # σ = s / (b·A) (first approximation)
    sigma = distance / B / alpha

    # iterate until change in σ is negligible (e.g. 10-12 ≈ 0.006mm)
    while True:
        old_sigma = sigma
        sigma, cos_2sigma_m = sigma_iteration(old_sigma, sigma1, alpha, beta, distance)
        if np.all(np.abs(old_sigma/sigma - 1) <= 1e-12):
            return sigma, cos_2sigma_m


def get_omega(
    azimuth: float, sigma: np.ndarray, cos_u1: float,
    sin_alpha: float, cos_2sigma_m: np.ndarray, cos2_alpha: float,
) -> np.ndarray:
    # λ = atan(sin σ · sin α1 / cos U1 · cos σ − sin U1 · sin σ · cos α1)
    lambda_ = np.arctan2(
        (np.sin(sigma) * np.sin(azimuth)),
        (np.cos(cos_u1) * np.cos(sigma) - np.sin(cos_u1) * np.sin(sigma) * np.cos(azimuth))
    )
    # C = f/16 · cos² α · [4 + f · (4 − 3 · cos² α)]
    c_sigma = (
        F/16 * cos2_alpha *
        (4 + F*(4 - 3*cos2_alpha))
    )
    # L = λ − (1−C) · f · sin α · {σ + C · sin σ · [cos 2σm + C · cos σ · (−1 + 2 · cos² 2σm)]}
    return (
        lambda_ - (1 - c_sigma)*F*sin_alpha * (
            sigma + c_sigma * np.sin(sigma) * (
                    cos_2sigma_m + c_sigma *
                    np.cos(sigma) * (-1 + 2 * cos_2sigma_m**2)
            )
        )
    )


def get_latitude(azimuth: float, sigma: np.ndarray, cos_u1: float, sin_alpha: float) -> np.ndarray:
    # φ2 = atan(sin U1 · cos σ + cos U1 · sin σ · cos α1 /
    latitude = np.arctan2(
        (np.sin(cos_u1) * np.cos(sigma) + np.cos(cos_u1) * np.sin(sigma) * np.cos(azimuth)),
        # (1−f) · √sin² α + (sin U1 · sin σ − cos U1 · cos σ · cos α1)² )
        (
            (1 - F) * np.sqrt(
                sin_alpha**2 +
                (
                    np.sin(cos_u1) * np.sin(sigma) - np.cos(cos_u1) * np.cos(sigma) * np.cos(azimuth)
                )**2
            )
        )
    )
    return latitude


def vincenty_direct(
    latitude: float,
    longitude: float,
    azimuth: float,
    distance: np.ndarray,
) -> tuple[np.ndarray, np.ndarray]:
    """
    Returns: lat and long of projected point
    """

    # Not necessary:
    azimuth = np.mod(azimuth, TWO_PI)

    # tan U1 = (1−f) · tan φ1
    tan_u1 = (1 - F) * np.tan(latitude)
    # cos U1 = 1 / √1 + tan² U1, sin U1 = tan U1 · cos U1
    cos_u1 = np.arctan(tan_u1)
    # σ1 = atan(tan U1 / cos α1)
    sigma1 = np.arctan2(tan_u1, np.cos(azimuth))
    # sin α = cos U1 · sin α1
    sin_alpha = np.cos(cos_u1) * np.sin(azimuth)
    # cos² α = 1 − sin² α
    cos2_alpha = 1 - sin_alpha**2

    alpha, beta = get_upsilon2(cos2_alpha * F2)
    sigma, cos_2sigma_m = get_sigma(distance, alpha, beta, sigma1)
    latitude = get_latitude(azimuth, sigma, cos_u1, sin_alpha)
    omega = get_omega(azimuth, sigma, cos_u1, sin_alpha, cos_2sigma_m, cos2_alpha)

    return latitude, longitude + omega


def dev_dist(project_seconds=None) -> None:
    if project_seconds is None:
        project_seconds = np.linspace(900, 3600, 4)

    # position
    latitude = 33.01
    longitude = -98.94
    # direction
    azimuth = -171.95
    # speed
    meters_per_second = 11
    # distance projection
    distance = meters_per_second * project_seconds
    # in rads
    rad_lat, rad_lon, rad_azi = np.deg2rad((latitude, longitude, azimuth))

    degs = np.rad2deg(
        vincenty_direct(rad_lat, rad_lon, rad_azi, distance)
    )

    points = degs.T

    assert np.allclose(
        points,
        (
            (32.921612216500890, -98.95482179485320),
            (32.833221427146340, -98.96961414990395),
            (32.744827642960495, -98.98437722206711),
            (32.656430874926910, -98.99911116737137),
        ),
        atol=1e-12, rtol=0,
    )

    times = (f"+{x:02.0f}min" for x in project_seconds / 60)

    projection = dict(zip(times, points.tolist()))
    pprint(projection)


if __name__ == '__main__':
    dev_dist()

Output

{'+15min': [32.92161221650089, -98.9548217948532],
 '+30min': [32.83322142714634, -98.96961414990395],
 '+45min': [32.744827642960495, -98.98437722206711],
 '+60min': [32.65643087492691, -98.99911116737137]}