Spherical distance (Vincenty distance) between two geographic points [closed]

Question

I am trying to speed up the code of a R function that calculates a Moran's I autocorrelation coefficient from very large distance matrices between geographic points. So I am exploring approaches to boost my R code using C++. Unfortunately, I am not at all familiar with C++. I found a very interesting C++ code to compute the spherical distance (Vincenty’s distance) between two geographic points. Here is the link and here is the code:

#include <tuple>
#include <pybind11/pybind11.h>

namespace py = pybind11;

std::tuple<double, double> vinc(double latp, double latc, double longp, double longc) {
    constexpr double req = 6378137.0;             //Radius at equator
    constexpr double flat = 1 / 298.257223563;    //flattening of earth
    constexpr double rpol = (1 - flat) * req;

    double sin_sigma, cos_sigma, sigma, sin_alpha, cos_sq_alpha, cos2sigma;
    double C, lam_pre;

    // convert to radians
    latp = M_PI * latp / 180.0;
    latc = M_PI * latc / 180.0;
    longp = M_PI * longp / 180.0;
    longc = M_PI * longc / 180.0;

    const double u1 = atan((1 - flat) * tan(latc));
    const double u2 = atan((1 - flat) * tan(latp));

    double lon = longp - longc;
    double lam = lon;
    constexpr double tol = pow(10., -12.); // iteration tolerance
    double diff = 1.;

    while (abs(diff) > tol) {
        sin_sigma = sqrt(pow((cos(u2) * sin(lam)), 2.) + pow(cos(u1)*sin(u2) - sin(u1)*cos(u2)*cos(lam), 2.));
        cos_sigma = sin(u1) * sin(u2) + cos(u1) * cos(u2) * cos(lam);
        sigma = atan(sin_sigma / cos_sigma);
        sin_alpha = (cos(u1) * cos(u2) * sin(lam)) / sin_sigma;
        cos_sq_alpha = 1 - pow(sin_alpha, 2.);
        cos2sigma = cos_sigma - ((2 * sin(u1) * sin(u2)) / cos_sq_alpha);
        C = (flat / 16) * cos_sq_alpha * (4 + flat * (4 - 3 * cos_sq_alpha));
        lam_pre = lam;
        lam = lon + (1 - C) * flat * sin_alpha * (sigma + C * sin_sigma * (cos2sigma + C * cos_sigma * (2 * pow(cos2sigma, 2.) - 1)));
        diff = abs(lam_pre - lam);
    }

    const double usq = cos_sq_alpha * ((pow(req, 2.) - pow(rpol, 2.)) / pow(rpol ,2.));
    const double A = 1 + (usq / 16384) * (4096 + usq * (-768 + usq * (320 - 175 * usq)));
    const double B = (usq / 1024) * (256 + usq * (-128 + usq * (74 - 47 * usq)));
    const double delta_sig = B * sin_sigma * (cos2sigma + 0.25 * B * (cos_sigma * (-1 + 2 * pow(cos2sigma, 2.)) -
                                                         (1 / 6) * B * cos2sigma * (-3 + 4 * pow(sin_sigma, 2.)) *
                                                         (-3 + 4 * pow(cos2sigma, 2.))));
    const double dis = rpol * A * (sigma - delta_sig);
    const double azi1 = atan2((cos(u2) * sin(lam)), (cos(u1) * sin(u2) - sin(u1) * cos(u2) * cos(lam)));

    return std::make_tuple(dis, azi1);
}

Unfortunately, the code is not enough fast for me because I work with very large datasets (around 1 million of locations with longitude and latitude coordinates). So, is it possible to speed up this C++ code in order to obtain an execution time of around 0.006 microseconds per function call. Any advice would be greatly appreciated. Thanks so much for your help.

Answer 1

Some Minor Points First

This looks very good, but there are a couple of trivial changes up front before we get into the real meat.

Do You Really Need Python Bindings?

You include the interface for Python support and then never use it. If this was just a roundabout way to include the math library,

Declare the Standard Headers You Use

Your math functions are in <cmath> or <math.h>. If you #include <math.h>, it just works. If you #include <cmath>, you should also, for maximum portability, import the specific functions you use:

using std::abs, std::atan, std::atan2, std::cos, std::pow, std::sin, std::sqrt;

Most actually-existing compilers let you skip this (although not using std::abs can cause a bug on some).

There is a Standard Way to Get π

You currently use M_PI, which is an extension many compilers add, but not part of the Standard Library. The official way to do it is:

#include <numbers>

constexpr double pi = std::numbers::pi_v<double>;

Avoid pow if Possible

One line in your program has an actual bug related to this:

constexpr double tol = pow(10., -12.); // iteration tolerance

On some compilers, including Clang 16.0.0, this will give you an error that pow is not a constexpr function. Change this to,

constexpr double tol = 1e-12;

Your other uses of pow are either squares or square roots, and would be more efficiently represented as multiplication or sqrt. However, LLVM compilers seem to be able to optimize these calls away.

Put a Decimal Point After Floating-Point Literals

It’s fine to write pi/180 or 1 - flat—up until you accidentally write 1/2, and the compiler gives you the int value 0. Code defensively and be consistent.

(Within reason. I sometimes write pi/2.)

Use constexpr and noexcept Where Appropriate

You will generate better code if you declare your functions constexpr. This tells the compiler that it can aggressively inline the function and fold constants.

Sometimes, you will get an error message saying that something your function does is not allowed because it's constexpr. This is usually, but not always, a statement that the compiler can’t optimize well inside a loop anyway.

So,

constexpr std::tuple<double, double> vinc(double latp, double latc, double longp, double longc)

Prefer a struct to std::tuple

You can give struct members meaningful names instead of trying to remember whether the first or second double in the std::tuple<double, double> is the distance. Also, if you have a couple of different types with the same layout, two different types of struct will have better type safety. (For example, coordinates in different systems.)

Refactor into Smaller Functions

This is more of a personal preference than an optimization, but you can make your code much easier to read, maintain and unit-test by decomposing it into small functions that each do one thing, and composing larger functions from those building-blocks.

Now, Actual Optimizations

Use a Good Compiler and Math Library

For example, Intel’s ICX for x86_64 has better math libraries than either GCC, Clang or MSVC, and can sometimes vectorize a loop that they cannot. In particular, the math libraries GCC and Clang use cannot (as of 2023) vectorize calls to trigonometric functions, but Intel’s can.

Use Appropriate Compiler Options

If you’re running on your own computer, tell the compiler that it’s allowed to use vector instructions! (On Clang, GCC or ICX, you want -march=native, to optimize for the same computer you’re compiling on, unless you are going to run the same binary on a different model of CPU.) By default, the compiler will assume you want the code to be able to run on the lowest common denominator.

You should also enable the version of the standard you want (such as -std=c++20) and turn on whole-program optimizations.

You want to enable all the useful warnings, and rewrite until you get none, unless you have a good reason to turn some warning off. (Disable warnings from system headers, of course.) I normally use -Wall -Wextra -Wpedantic -Wconversion -Wdeprecated.

You can sometimes improve performance with profile-guided optimization. Your compiler manual will tell you more.

Lay Out Your Data for Efficiency

To enable SIMD, you want your inputs and outputs arranged in arrays the same size (a structure-of-arrays). Here’s a wrapper function that calls yours on every element of the four input arrays, and writes them to the two output arrays.

#include <cassert>
#include <concepts>
#include <cstddef>
#include <ranges>

namespace ranges = std::ranges;

using std::size_t;

template<typename InputT, typename OutputT>
    requires ranges::input_range<InputT> &&
             ranges::range<OutputT> &&
             std::same_as<ranges::range_value_t<InputT>, double> &&
             std::same_as<ranges::range_value_t<OutputT>, double> &&
             ranges::sized_range<InputT> &&
             ranges::sized_range<OutputT> &&
             ranges::random_access_range<InputT> &&
             ranges::random_access_range<OutputT>
void vincs( OutputT& dists,
                      OutputT& azims,
                      const InputT& latps,
                      const InputT& longps,
                      const InputT& latcs,
                      const InputT& longcs ) noexcept
{
    const size_t n = ranges::size(dists);
/* It is a logic error for the input and output arrays not to be the same size! */
    assert( ranges::size(azims) == n &&
            ranges::size(latps) == n &&
            ranges::size(longps) == n &&
            ranges::size(latcs) == n &&
            ranges::size(longcs) == n );

    for (size_t i = 0; i < n; ++i) {
        const auto [dist, azim] = vinc( latps[i], latcs[i], longps[i], longcs[i] );
        dists[i] = dist;
        azims[i] = azim;
    }
}

This sample code has some requires clauses that amount to, “The input and output types must behave like arrays of double.” You can pass it a double[1000], a std::vector<double>, a std::array<double>, a std::span, a std::ranges::subrange, a std::valarray, or so on. If you know what one type you actually need, you could make it a lot shorter.

Edit: In your post, you say your actual problem needs to work on pairs of geographic locations from the same array, meaning that you actually want to work on two slices of the same array, which you advance in a double loop, and write the results to a matrix.

Write Static Single Assignments

The way to speed up most scientific computations is to vectorize your loops. Fortunately, this one is “embarrassingly parallel”: each computation pf the ith outputs depends only on the ith inputs. That will let us use a lot of tricks.

Once you have the right data structures set up, getting the compiler to optimize well can get complicated. But here’s the best general, introductory advice I have.

The body of your loop should consist entirely of const or constexpr variable declarations, and one return statement. On the right-hand side of your assignments, call only built-in operators, ? expressions, and constexpr functions that follow the same restrictions themselves. All computations should depend only on a single element of each of your input arrays, constants, and loop indices if you are using them. As of 2023, compilers are very good at optimizing code that follows those rules. (I’ll give an example at the end.)

The part of the algorithm that doesn’t work for is the while loop where you re-run the computation until the difference is less than your error tolerance. I haven’t tested, but you might be able to get a speedup from running a fixed number of iterations instead. If the computation runs the same number of times for each element, the compiler might be able to run them in parallel.

Parallelize with OpenMP

The one line of code that will speed up your scientific computations the most is #pragma omp parallel for. (You might try adding simd.) This will have multiple threads execute your loop in parallel.

This loop is embarrassingly-parallel enough that the compiler doesn’t need any extra hints to be able to do it.

#include <cassert>
#include <concepts>
#include <cstddef>
#include <omp.h>
#include <ranges>

namespace ranges = std::ranges;

using std::size_t;

template<typename InputT, typename OutputT>
    requires ranges::input_range<InputT> &&
             ranges::range<OutputT> &&
             std::same_as<ranges::range_value_t<InputT>, double> &&
             std::same_as<ranges::range_value_t<OutputT>, double> &&
             ranges::sized_range<InputT> &&
             ranges::sized_range<OutputT> &&
             ranges::random_access_range<InputT> &&
             ranges::random_access_range<OutputT>
void vincs( OutputT& dists,
                      OutputT& azims,
                      const InputT& latps,
                      const InputT& longps,
                      const InputT& latcs,
                      const InputT& longcs ) noexcept
{
    const size_t n = ranges::size(dists);
/* It is a logic error for the input and output arrays not to be the same size! */
    assert( ranges::size(azims) == n &&
            ranges::size(latps) == n &&
            ranges::size(longps) == n &&
            ranges::size(latcs) == n &&
            ranges::size(longcs) == n );

    # pragma omp parallel for schedule(static)
    for (size_t i = 0; i < n; ++i) {
        const auto [dist, azim] = vinc( latps[i], latcs[i], longps[i], longcs[i] );
        dists[i] = dist;
        azims[i] = azim;
    }
}

There are two lines added to this version: #include <omp.h> and #pragma omp parallel for schedule(static) (which divides the input arrays into equal sections and assigns each section to a different thread). Most compilers also need a flag to enable OpenMP. For most compilers other than Microsoft’s, that’s -fopenmp.

Use an Alias for the Element Type

A comment by J.H. made me realize that you might consider changing the precision of the elements. This is much easier if you refer to it as something like elem_t and write, at the top of the file,

using elem_t = double;

You can now test changing it to another type, and back, in one place. The code below could operate on eight float values at once, if you don’t need double precision.

Putting it All Together

For demonstration purposes, I wrote an extremely dumbed-down version that only calculates spherical distance, with no correction for the eccentricity of the Earth. This code is untested and certainly will have at least one bug somewhere. I share it as a demonstration of what modern compilers can do.

#include <cassert>
#include <cmath>
#include <concepts>
#include <cstddef>
#include <numeric>
#include <omp.h>
#include <ranges>

namespace ranges = std::ranges;

using std::acos, std::cos, std::sin, std::size_t;

using elem_t = double;

struct coord_spherical {
    elem_t r;
    elem_t theta;
    elem_t phi;
};

struct coord_cart3 {
    elem_t x;
    elem_t y;
    elem_t z;
};

constexpr elem_t pi = std::numbers::pi_v<elem_t>;
constexpr elem_t r = 6378.0; // Equatorial radius of the Earth.



constexpr coord_spherical spherical_from_lat_long( const elem_t latitude,
                                                   const elem_t longitude
                                                 ) noexcept
{
    return coord_spherical{ r,
                            longitude*pi/180.0f,
                            pi/2 - latitude*pi/180.8f };
}


constexpr coord_cart3 unit_sphere_cart3(const coord_spherical sp) noexcept
{
    const elem_t sin_phi = sin(sp.phi);
    return coord_cart3{ cos(sp.theta)*sin_phi,
                        sin(sp.theta)*sin_phi,
                        cos(sp.phi) };
}


constexpr elem_t dot_product( const coord_cart3 u,
                              const coord_cart3 v
                            ) noexcept
{
    return u.x*v.x + u.y*v.y + u.z*v.z;
}                            


constexpr elem_t sphere_dist( const elem_t lat1,
                              const elem_t long1,
                              const elem_t lat2,
                              const elem_t long2
                            ) noexcept
{
    return r * acos(dot_product(
        unit_sphere_cart3(spherical_from_lat_long(lat1, long1)),
        unit_sphere_cart3(spherical_from_lat_long(lat2, long2))));
}


template<typename InputT, typename OutputT>
    requires ranges::input_range<InputT> &&
             ranges::range<OutputT> &&
             std::same_as<ranges::range_value_t<InputT>, elem_t> &&
             std::same_as<ranges::range_value_t<OutputT>, elem_t> &&
             ranges::sized_range<InputT> &&
             ranges::sized_range<OutputT> &&
             ranges::random_access_range<InputT> &&
             ranges::random_access_range<OutputT>
void sphere_dists( OutputT& dists,
                   const InputT& latps,
                   const InputT& longps,
                   const InputT& latcs,
                   const InputT& longcs
                 ) noexcept
{
    const size_t n = ranges::size(dists);
/* It is a logic error for the input and output arrays not to be the same size! */
    assert( ranges::size(latps) == n &&
            ranges::size(longps) == n &&
            ranges::size(latcs) == n &&
            ranges::size(longcs) == n );

    # pragma omp parallel for simd schedule(static)
    for (size_t i = 0; i < n; ++i) {
        dists[i] = sphere_dist( latps[i], longps[i], latcs[i], longcs[i] );
    }
}


using data_t = elem_t[1000];
extern const data_t latps, latcs, longps, longcs;
extern data_t dists;

int main()
{
    sphere_dists( dists, latps, longps, latcs, longcs );
}

ICX 2022.2.1 with the flags -std=c++20 -march=x86-64-v3 -O3 -fp-model=fast -static -fiopenmp compiles the loop to this assembly code:

.LBB1_3:                                # =>This Inner Loop Header: Depth=1
        mov     r15, rsp
        vbroadcastsd    ymm12, qword ptr [rip + .LCPI1_0] # ymm12 = [1.7453292519943295E-2,1.7453292519943295E-2,1.7453292519943295E-2,1.7453292519943295E-2]
        vmulpd  ymm10, ymm12, ymmword ptr [rbx + 8*rsi + longps]
        vmovupd ymm11, ymmword ptr [rbx + 8*rsi + latps]
        vbroadcastsd    ymm14, qword ptr [rip + .LCPI1_1] # ymm14 = [1.7376065561890447E-2,1.7376065561890447E-2,1.7376065561890447E-2,1.7376065561890447E-2]
        vbroadcastsd    ymm15, qword ptr [rip + .LCPI1_2] # ymm15 = [1.5707963267948966E+0,1.5707963267948966E+0,1.5707963267948966E+0,1.5707963267948966E+0]
        vfnmadd132pd    ymm11, ymm15, ymm14     # ymm11 = -(ymm11 * ymm14) + ymm15
        vmovapd ymm0, ymm11
        call    rbp
        vmovapd ymm8, ymm0
        vmovapd ymm0, ymm10
        call    rdi
        vmovapd ymm9, ymm0
        vmovapd ymm0, ymm10
        call    rbp
        vmovapd ymm10, ymm0
        vmovapd ymm0, ymm11
        call    rdi
        vmovapd ymm11, ymm0
        vmulpd  ymm12, ymm12, ymmword ptr [rbx + 8*rsi + longcs]
        vmovupd ymm13, ymmword ptr [rbx + 8*rsi + latcs]
        vfnmadd132pd    ymm13, ymm15, ymm14     # ymm13 = -(ymm13 * ymm14) + ymm15
        vmovapd ymm0, ymm13
        call    rbp
        vmovapd ymm14, ymm0
        vmovapd ymm0, ymm12
        call    rdi
        vmovapd ymm15, ymm0
        vmovapd ymm0, ymm12
        call    rbp
        vmovapd ymm12, ymm0
        vmovapd ymm0, ymm13
        call    rdi
        vmulpd  ymm1, ymm12, ymm10
        vmulpd  ymm2, ymm11, ymm0
        vfmadd231pd     ymm1, ymm9, ymm15       # ymm1 = (ymm9 * ymm15) + ymm1
        vmulpd  ymm0, ymm14, ymm8
        vfmadd213pd     ymm0, ymm1, ymm2        # ymm0 = (ymm1 * ymm0) + ymm2
        call    qword ptr [rip + __svml_acos4_l9@GOTPCREL]
        vbroadcastsd    ymm1, qword ptr [rip + .LCPI1_3] # ymm1 = [6.378E+3,6.378E+3,6.378E+3,6.378E+3]
        vmulpd  ymm0, ymm0, ymm1
        mov     rsp, r15
        vmovupd ymmword ptr [rbx + 8*rsi + dists], ymm0
        add     rsi, 4
        cmp     rsi, r12
        jb      .LBB1_3

I’m not going to look at this in-depth, but the main thing to notice here is that all the operations are on ymm registers. That is, the compiler was able to auto-vectorize this version of the loop to use SIMD instructions on four packed doubles at once.

Edit: With the -fiopenmp flag, ICX 2022.2.1 is capable of both multithreading this code and using SIMD, at the same time.

You can try out this code on Godbolt.org and see what changing it around does.