地球任意两点求方位角 俯仰角

方位角

方位角（azimuthangle）：从某点的指北方向线起，依顺时针方向到目标方向线之间的水平夹角，叫方位角。

地球上任意两点距离计算公式为 ：

d=111.12cos1/(sinΦAsinΦB十cosΦAcosΦBcos(λB—λA))

其中A点的经、纬度分别为λA和ΦA，B点的经、纬度分别为λB和ΦB，d为距离。

俯仰角

俯仰角：坐标系y轴与水平面的夹角。当坐标系的y轴在惯性坐标系XOY平面上方时，俯仰角为正，否则为负。

方位角，距离，海拔在线计算器 (ab126.com)

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;
}