计算两个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;
}