Measure the distance done by a channel swimmer

channel_distance.cpp

// code adopted from https://stackoverflow.com/a/53147219/272733

#include <iostream>
#include <cmath>

const double degToRad = std::acos(-1) / 180;

struct vec3
{
    double x, y, z;
    vec3(double xd, double yd, double zd) : x(xd), y(yd), z(zd) {}
    double length()
    {
        return std::sqrt(x*x + y*y + z*z);
    }
    void normalize()
    {
        double len = length();
        x = x / len;
        y = y / len;
        z = z / len;
    } 
};

vec3 cross(const vec3& v1, const vec3& v2)
{
    return vec3( v1.y * v2.z - v2.y * v1.z,
                 v1.z * v2.x - v2.z * v1.x,
                 v1.x * v2.y - v2.x * v1.y );
}

double dot(const vec3& v1, const vec3& v2)
{
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

double GCDistance(const vec3& v1, const vec3& v2, double R)
{
    //normalize, so we can pass any vectors
    vec3 v1n = v1;
    v1n.normalize();
    vec3 v2n = v2;
    v2n.normalize();
    vec3 tmp = cross(v1n, v2n);
    //minimum distance may be in one direction or the other
    double d1 = std::abs(R * std::atan2(tmp.length() , dot(v1n, v2n)));
    double d2 = std::abs(R * std::atan2(tmp.length() , -dot(v1n, v2n)));

    return std::min(std::abs(d1), std::abs(d2));
}                  

int main()
{
    //Points A, B, and P
    // A: Samphire Hoe
    double lon1 = 1.260743  * degToRad;
    double lat1 = 51.102227 * degToRad;
    // B: Cap Gris-Nez
    double lon2 = 1.581832  * degToRad;
    double lat2 = 50.871726 * degToRad;
    // C: current position (to be entered)
    double lon3;
    double lat3;

    std::cout << "Enter the latitude: " << std::flush;
    std::cin >> lat3;
    std::cout << "Enter the longitude: " << std::flush;
    std::cin >> lon3;
    lat3 *= degToRad;
    lon3 *= degToRad;

    //Let's work with a sphere of R = 1
    vec3 OA(std::cos(lat1) * std::cos(lon1), std::cos(lat1) * std::sin(lon1), std::sin(lat1));
    vec3 OB(std::cos(lat2) * std::cos(lon2), std::cos(lat2) * std::sin(lon2), std::sin(lat2));
    vec3 OP(std::cos(lat3) * std::cos(lon3), std::cos(lat3) * std::sin(lon3), std::sin(lat3));
    //plane OAB, defined by its perpendicular vector pp1
    vec3 pp1 = cross(OA, OB);
    //plane OPC
    vec3 pp2 = cross(pp1, OP);
    //planes intersection, defined by a line whose vector is ppi
    vec3 ppi = cross(pp1, pp2);
    ppi.normalize(); //unitary vector

    //Radious or Earth
    double R = 6371; //mean value. For more precision, data from a reference ellipsoid is required

    std::cout << "Distance of the crossing = " << GCDistance(OA, OB, R) << std::endl;
    std::cout << "Distance from Samphire Hoe to current position = " << GCDistance(OA, OP, R) << std::endl;
    std::cout << "Distance from current position to Cap Gris-Nez = " << GCDistance(OB, OP, R) << std::endl;
    std::cout << "Distance from Samphire Hoe to the projected point on the straight line = " << GCDistance(OA, ppi, R) << std::endl;
    std::cout << "Distance from the projected point on the straight line to Cap Gris-Nez = " << GCDistance(OB, ppi, R) << std::endl;
    std::cout << "Distance away from the straight line = " << GCDistance(OP, ppi, R) << std::endl;
}