参考链接:Calculate distance, bearing and more between Latitude/Longitude points
#include <string>
using namespace std;
class PointLatLngAlt
{
public:
double lat;
double lng;
double alt;
string tag;
public:
PointLatLngAlt();
PointLatLngAlt(double dLat, double dLng, double dAlt, string strTag);
double getDistance(PointLatLngAlt &p2);
double getBearing(PointLatLngAlt &p2);
PointLatLngAlt newPos(double bearing, double distance);
double rad2deg(double dRad);
double deg2rad(double dDeg);
};
#include <math.h>
#include <iostream>
using namespace std;
PointLatLngAlt::PointLatLngAlt()
{
}
/**
* 带初始化的构造函数
*/
PointLatLngAlt::PointLatLngAlt(double dLat, double dLng, double dAlt, string strTag)
{
lat = dLat;
lng = dLng;
alt = dAlt;
tag = strTag;
}
/**
* function:返回两对经纬度之间的距离
*/
double PointLatLngAlt::getDistance(PointLatLngAlt &p2)
{
double d = lat * 0.017453292519943295;
double num2 = lng * 0.017453292519943295;
double num3 = p2.lat * 0.017453292519943295;
double num4 = p2.lng * 0.017453292519943295;
double num5 = num4 - num2;
double num6 = num3 - d;
double num7 = pow(sin(num6 / 2.0), 2.0) + ((cos(d) * cos(num3)) * pow(sin(num5 / 2.0), 2.0));
double num8 = 2.0 * atan2(sqrt(num7), sqrt(1.0 - num7));
return (6371 * num8) * 1000.0; // M
}
/**
* function:返回当前经纬度与p2经纬度之间方位角,方位角为与正北方向的夹角
*/
double PointLatLngAlt::getBearing(PointLatLngAlt &p2)
{
double latitude1 = this->deg2rad(lat);
double latitude2 = this->deg2rad(p2.lat);
double longitudeDifference = this->deg2rad(p2.lng - lng);
double y = sin(longitudeDifference) * cos(latitude2);
double x = cos(latitude1) * sin(latitude2) - sin(latitude1) * cos(latitude2) * cos(longitudeDifference);
double dDeg = this->rad2deg(atan2(y,x));
if(dDeg < 0)
{
dDeg += 360;
}
return dDeg;
}
/**
* function:计算当前经纬度在给定方位角和距离下的经纬度
*/
PointLatLngAlt PointLatLngAlt::newPos(double bearing, double distance)
{
// '''extrapolate latitude/longitude given a heading and distance
// thanks to http://www.movable-type.co.uk/scripts/latlong.html
// '''
// from math import sin, asin, cos, atan2, radians, degrees
double radius_of_earth = 6378100.0;//# in meters
double lat1 = this->deg2rad(lat);
double lon1 = this->deg2rad(lng);
double brng = this->deg2rad(bearing);
double dr = distance / radius_of_earth;
double lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1) * sin(dr) * cos(brng));
double lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
cos(dr) - sin(lat1) * sin(lat2));
double latout = this->rad2deg(lat2);
double lngout = this->rad2deg (lon2);
return PointLatLngAlt(latout, lngout, alt, tag);
}
/**
* function:角度转弧度
*/
double PointLatLngAlt::deg2rad(double dDeg)
{
return dDeg * M_PI / 180;
}
/**
* 弧度转角度
*/
double PointLatLngAlt::rad2deg(double dRad)
{
return dRad * 180 / M_PI;
}