#include <adc_tools/gps_distance.h>
#include <math.h>
#define M_PI (3.1415926535897932384626433832795)

// 计算弧度
double iv::decition::rad(double d)
{
    const double PI = 3.1415926535898;
    return d * PI / 180.0;
}

// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
{
    const float EARTH_RADIUS = 6378.137;
    double radLat1 = rad(fLati1);
    double radLat2 = rad(fLati2);
    double a = radLat1 - radLat2;

    double b = rad(fLong1) - rad(fLong2);
    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
    s = s * EARTH_RADIUS;
    //	s = (int)(s * 10000000) / 10000;
    return s;
}


// 从直角坐标两点的直线距离，单位是米
double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
{
    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
    return s;
}


// 从直角坐标计算两点的夹角
double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
{
    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
    return angle;
}