方位角
方位角(azimuthangle):从某点的指北方向线起,依顺时针方向到目标方向线之间的水平夹角,叫方位角。
地球上任意两点距离计算公式为 :
d=111.12cos1/(sinΦAsinΦB十cosΦAcosΦBcos(λB—λA))
其中A点的经、纬度分别为λA和ΦA,B点的经、纬度分别为λB和ΦB,d为距离。
俯仰角
俯仰角:坐标系y轴与水平面的夹角。当坐标系的y轴在惯性坐标系XOY平面上方时,俯仰角为正,否则为负。
C++ —— 两经纬度计算距离、方位角_automoblie0的博客-CSDN博客
#include <iostream>
// PI
#define M_PI 3.14159265358979323846
// 地球半径
const double EARTH_RADIUS = 6371000;
// 角度转弧度
double A2R(double d)
{
return d * M_PI / 180.0;
}
// 弧度转角度
double R2A(double d)
{
return d / M_PI * 180.0;
}
// 获取两经纬度之间的距离(m)
double GetDistanceCpp(double lat1, double lng1, double lat2, double lng2)
{
double radLat1 = A2R(lat1);
double radLat2 = A2R(lat2);
double a = radLat1 - radLat2;
double b = A2R(lng1) - A2R(lng2);
double s = 2 * asin(sqrt(pow(sin(a/2),2) +cos(radLat1)*cos(radLat2)*pow(sin(b/2),2)));
s = s * EARTH_RADIUS;
return s;
}
// 获取两经纬度之间的方位角(°) 纬度 精度 纬度 经度
double GetYaw(double lat1, double lon1, double lat2, double lon2)
{
double result = 0.0;
int ilat1 = (int) (0.50 + lat1 * 360000.0);
int ilat2 = (int) (0.50 + lat2 * 360000.0);
int ilon1 = (int) (0.50 + lon1 * 360000.0);
int ilon2 = (int) (0.50 + lon2 * 360000.0);
lat1 = A2R(lat1);
lon1 = A2R(lon1);
lat2 = A2R(lat2);
lon2 = A2R(lon2);
if ((ilat1 == ilat2) && (ilon1 == ilon2)){return result;}
else if (ilon1 == ilon2){if (ilat1 > ilat2){result = 180.0;}}
else
{
double c = acos(sin(lat2) * sin(lat1) + cos(lat2)* cos(lat1) * cos((lon2 - lon1)));
double A = asin(cos(lat2) * sin((lon2 - lon1)) / sin(c));
result = R2A(A);
if ((ilat2 > ilat1) && (ilon2 > ilon1)){}
else if ((ilat2 < ilat1) && (ilon2 < ilon1)){result = 180.0 - result;}
else if ((ilat2 < ilat1) && (ilon2 > ilon1)){result = 180.0 - result;}
else if ((ilat2 > ilat1) && (ilon2 < ilon1)){result += 360.0;}
}
if(result<0){result +=360.0;}
if(result>360){result -=360.0;}
return result;
}
int main(int argc, char *argv[])
{
//double ln_1 = 119.2928,la_1 = 26.07907;
//double ln_2 = 114.0538,la_2 = 22.54151;
//double ln_1 = 123.449,la_1 = 41.72306;
//double ln_2 = 123.4494,la_2 = 41.72217;
double delta1=0.00001,delta2=0.0001; //偏差
double ln_1 = 118.4723,la_1 = 32.65736;
double ln_2 = 118.4715,la_2 = 32.65258;
double height=150;
// 计算距离
double dis =GetDistanceCpp(la_1,ln_1,la_2,ln_2);
double dis1 =GetDistanceCpp(la_1+delta1,ln_1,la_2,ln_2);
std::cout << dis << "m\n";
double hudu=atan2(height,dis);
double jiaodu=R2A(hudu);
double hudu1=atan2(height,dis1);
double jiaodu1=R2A(hudu1);
std::cout << "俯仰角"<<jiaodu << "°\n";
std::cout << "俯仰角偏差"<<jiaodu1- jiaodu<< "°\n";
// 计算方位角
std::cout << GetYaw(la_1,ln_1,la_2,ln_2);
return 0;
}