distance.cpp

// distance.cpp
#include <cmath>
#include <iostream>

const float radius = 3959.9;

float DegreeToRadians(float n)
{
    float s;
    s = n* (M_PI/180);
    return s;
}

float DistanceOfTwoLocations(float lat1, float lon1, float lat2, float lon2)
{

    float a, c, d;
    float phi1, phi2, dphi, dlam;
    // phi1 = lat1 * (M_PI/180); 
    phi1 = DegreeToRadians(lat1);
    // std::cout << phi1 << std::endl;
    // phi2 = lat2 * (M_PI/180);
    phi2 = DegreeToRadians(lat2);
    // std::cout << phi2 << std::endl;
    dphi = phi2 - phi1;
    // std::cout << dphi << std::endl;
    // dlam = (lon2-lon1) * (M_PI/180);
    dlam = DegreeToRadians(lon2-lon1);
    // std::cout << dlam << std::endl;

    a = std::sin(dphi/2) * std::sin(dphi/2) + 
        std::cos(phi1) * std::cos(phi2) *
        std::sin(dlam/2) * std::sin(dlam/2);
    c = 2 * std::atan2(std::sqrt(a),std::sqrt(1-a));
    d = radius * c;
    return d;
}