Вычислить пробег по точкам

/// Calculate distance in meters between points (lat1, lon1) and (lat2, lon2)
ADF_GIS_EXPORT double adf_calc_distance(double lat1, double lon1, double lat2, double lon2, bool draft_calc)
{
    const double k = 3.14159265358979 / 180;
    const double ff = 1 / 298.257;
    double c, d, f, g, h1, h2, l, o, r, s, w1, w2, sg, cg, sf, cf, sl, cl;

    if (lat1 == lat2 && lon1 == lon2)
        return 0;

    if (draft_calc)
        return sqrt((lat2 - lat1)*(lat2 - lat1) + (lon2 - lon1)*(lon2 - lon1)) * 111000.0;

    // Compute auxiliary angles
    f = (lat1 + lat2) / 2;
    g = (lat1 - lat2) / 2;
    l = (lon1 - lon2) / 2;

    // Compute sines and cosines of auxiliary angles
    sg = sin(g * k);
    cg = cos(g * k);
    sf = sin(f * k);
    cf = cos(f * k);
    sl = sin(l * k);
    cl = cos(l * k);

    w1 = pow(sg * cl, 2);
    w2 = pow(cf * sl, 2);
    s = w1 + w2;

    w1 = pow(cg * cl, 2);
    w2 = pow(sf * sl, 2);
    c = w1 + w2;

    o = atan(sqrt(s / c));
    r = sqrt(s * c) / o;
    d = 2 * o * 6378.14;

    h1 = (3 * r - 1) / (2 * c);
    h2 = (3 * r + 1) / (2 * s);

    // Compute the angle between the points on a
    // synthetic sphere connecting the two points.
    w1 = sf * cg;
    w1 = w1 * w1 * h1 * ff + 1;
    w2 = cf * sg;
    w2 = w2 * w2 * h2 * ff;

    return d * (w1 - w2) * 1000;
}