#include <iostream>
using namespace std;
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/algorithms/buffer.hpp>
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
typedef bg::model::box<BoostPoint> BoostBox;
typedef bg::model::linestring<BoostPoint> BoostLineString;
float haversine_distance(const BoostPoint& p1, const BoostPoint& p2) {
const float R = 6371000.0f;
float lat1 = bg::get<0>(p1) * M_PI / 180.0f;
float lon1 = bg::get<1>(p1) * M_PI / 180.0f;
float lat2 = bg::get<0>(p2) * M_PI / 180.0f;
float lon2 = bg::get<1>(p2) * M_PI / 180.0f;
float dLat = lat2 - lat1;
float dLon = lon2 - lon1;
float a = std::sin(dLat/2) * std::sin(dLat/2) +
std::cos(lat1) * std::cos(lat2) *
std::sin(dLon/2) * std::sin(dLon/2);
float c = 2 * std::atan2(std::sqrt(a), std::sqrt(1-a));
return R * c;
}
int main(){
std::string wkt = "LINESTRING (37.419817 55.690967, 37.419938 55.690744)";
BoostLineString l1;
bg::read_wkt(wkt, l1);
std::cout << haversine_distance(l1.front(), l1.back()) << std::endl;
return 0;
}