c++ 计算两个gps之间的距离

计算两个gps之间的距离（单位:m）

只计算经纬距离

double CalDistance(double lat1, double lon1, double lat2, double lon2) {
    const double EARTH_RADIUS = 6371000; // major axis长度为 6378137，平均长度为6371000

    double radLat1 = DEG2RAD(lat1);
    double radLat2 = DEG2RAD(lat2);
    double a = radLat1 - radLat2;
    double b = DEG2RAD(lon1) - DEG2RAD(lon2);
    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 CalDistance(double lat1, double lon1, double h1, double lat2, double lon2, double h2) {
    const double EARTH_RADIUS = 6378137;
    double radLat1 = DEG2RAD(lat1);
    double radLat2 = DEG2RAD(lat2);
    double a = radLat1 - radLat2;
    double b = DEG2RAD(lon1) - DEG2RAD(lon2);
    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;
    s = sqrt(pow(s, 2) + pow((h1 - h2), 2));
    return s;
}