1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
#include <kooling/geo/algorithm.h>

#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/GeodesicLine.hpp>
#include <GeographicLib/Gnomonic.hpp>

#include <cmath>

namespace kooling::geo {

distance_t distance(const coordinate& a, const coordinate& b)
{
    // https://www.movable-type.co.uk/scripts/latlong.html

    const double a_lat{ deg2rad(a.lat()) };
    const double b_lat{ deg2rad(b.lat()) };
    const double a_lon{ deg2rad(a.lon()) };
    const double b_lon{ deg2rad(b.lon()) };
    const double delta_lon{ a_lon - b_lon };
    const double delta_lat{ a_lat - b_lat };
    const double sin_half_delta_lat{ std::sin(delta_lat / 2.) };
    const double sin_half_delta_lon{ std::sin(delta_lon / 2.) };

    const double hav{
        sin_half_delta_lat * sin_half_delta_lat +
        std::cos(a_lat) * std::cos(b_lat) *
        sin_half_delta_lon * sin_half_delta_lon };

    const double c{ 2. * std::atan2(std::sqrt(hav), std::sqrt(1 - hav)) };

    return kooling::datamodel::distance::from_m(k_earth_mean_radius.m() * c);
}

std::pair<coordinate, bool> intercept(const coordinate& a, const coordinate& b, const coordinate& p)
{
    // https://sourceforge.net/p/geographiclib/discussion/1026621/thread/21aaff9f/#8a93

    struct vector3
    {
        vector3(double x, double y, double z = 1)
            : x{ x }
            , y{ y }
            , z{ z }
        {}
        
        vector3(const vector3&) = delete;
        vector3& operator=(const vector3&) = delete;
        vector3(vector3&&) = default;
        vector3& operator=(vector3&&) = default;

        vector3 cross(const vector3& b) const
        {
            return { y * b.z - z * b.y,
                     z * b.x - x * b.z,
                     x * b.y - y * b.x };
        }

        vector3 cross_norm(const vector3& b) const
        {
            const double new_z{ x * b.y - y * b.x };
            return { (y * b.z - z * b.y) / new_z,
                     (z * b.x - x * b.z) / new_z,
                     1. };
        }

        double x, y, z;
    };

    static const GeographicLib::Gnomonic proj{ GeographicLib::Geodesic::WGS84() };

    double lata{ a.lat() };
    double lona{ a.lon() };
    double latb{ b.lat() };
    double lonb{ b.lon() };
    double latp{ p.lat() };
    double lonp{ p.lon() };

    double lat0{ (lata + latb) / 2 };
    double lon0{ (lona + lonb) / 2 }; // Possibly need to deal with longitudes wrapping around
    vector3 p0{ 0, 0 };<--- Variable 'p0' is assigned a value that is never used.

    for (int i = 0; i < 10; ++i)
    {
        double xa1, ya1, xa2, ya2;
        double xb1, yb1;
        proj.Forward(lat0, lon0, lata, lona, xa1, ya1);
        proj.Forward(lat0, lon0, latb, lonb, xa2, ya2);
        proj.Forward(lat0, lon0, latp, lonp, xb1, yb1);
        // See Hartley and Zisserman, Multiple View Geometry, Sec. 2.2.1
        vector3 va1(xa1, ya1); vector3 va2(xa2, ya2);
        // la is homogeneous representation of line A1,A2
        vector3 la = va1.cross(va2);
        vector3 vb1(xb1, yb1);<--- Variable 'vb1' is assigned a value that is never used.
        // lb is homogeneous representation of line thru B1 perpendicular to la
        vector3 lb(la.y, -la.x, la.x * yb1 - la.y * xb1);
        // p0 is homogeneous representation of intersection of la and lb
        p0 = la.cross_norm(lb);
        double lat1, lon1;
        proj.Reverse(lat0, lon0, p0.x, p0.y, lat1, lon1);
        bool done{ std::abs(lat1 - lat0) < 1 };
        lat0 = lat1;
        lon0 = lon1;
        if (done)
        {
            break;
        }
    }

    // If the intercept point doesn't lie on the segment, pick the closest end.
    coordinate intr{ lat0, lon0 };
    const auto d1{ distance(a, intr) };
    const auto d2{ distance(b, intr) };
    const auto d3{ distance(a, b) };
    if (d1 + d2 > distance_t::from_m(d3.m() * 1.01))
    {
        return { d1 < d2 ? a : b, false };
    }
    else
    {
        return { intr, true };
    }
}

line_string interpolate(const coordinate& a, const coordinate& b, const distance_t& d)
{
    line_string line;
    const auto nof_points{ static_cast<int>(std::floor(distance(a, b).m() / d.m())) };
    line.points.reserve(nof_points + 1);
    line.points.push_back(a);
    if (nof_points > 0)
    {
        constexpr auto mask{ GeographicLib::Geodesic::LATITUDE | GeographicLib::Geodesic::LONGITUDE | GeographicLib::Geodesic::DISTANCE_IN };
        const auto geodesic{ GeographicLib::Geodesic::WGS84().InverseLine(a.lat(), a.lon(), b.lat(), b.lon(), mask) };
        for (int i = 0; i != nof_points; ++i)
        {
            double lat, lon, dummy;
            geodesic.Position(d.m() * (i + 1), lat, lon, dummy, dummy, dummy, dummy, dummy);
            line.points.emplace_back(lat, lon);
        }
    }

    return line;
}

} // namespace kooling::geo