C++ 计算3个地理坐标点之间的夹角 和航点之间的角度

背景

 来了个需求，需要知道大疆OSDK执行WaypointV2Mission时候，航点之间的转向角是顺时针还是逆时针的(一开始以为WaypointV2是随机顺逆旋转到下一个航点的，那么自然想到计算航点之间的角度，然后改变turnmode模式，所以就踏上了自己计算航点之间的角度之路)。

计算3个地理坐标点之间的夹角

 思考不清楚，以为通过三个GPS位置就能知道航点之间的角度，其实我最终只能通过余弦定理得到夹角的角度，这个角度是(0,180)。
 接下来开始讲解如何通过三个GPS位置得到这个夹角。

1. 先参考文章
2. 坐标拾取
3. 参考这篇文章,用java写的代码
4. 修改成c++的代码并验证

/*算三个航点的夹角*/
#include <iostream>
#include <math.h>
#include <vector>
using namespace std;

const long double pi = 3.14159265;

struct table_lat_long_height
{
   long double x;
   long double y;
   long double z;
};
table_lat_long_height table_long_lat_height;


// lat,lng为弧度表示的经纬度，r为地球半径，由于是算夹角，r是多少不重要
table_lat_long_height ball2xyz(long double lat, long double lng)
{
   long double arc = 6371.393;
   table_long_lat_height.x = arc * cos(lat) * cos(lng);
   table_long_lat_height.y = arc * cos(lat) * sin(lng);
   table_long_lat_height.z = arc * sin(lat);
   return table_long_lat_height;

}



// https://blog.csdn.net/reborn_lee/article/details/82497577
// 将地理经纬度转换成笛卡尔坐标系
table_lat_long_height geo2xyz(long double latitude, long double longitude)
{
   long double thera = (pi * latitude) / 180;
   long double fie = (pi * longitude) / 180;
   return ball2xyz(thera, fie);
}


// 计算3个地理坐标点之间的夹角
long double angleoflocation(vector<long double> vector_latitude, vector<long double> vector_longitude)
{
   auto p1 = geo2xyz(vector_latitude[0], vector_longitude[0]);
   auto p2 = geo2xyz(vector_latitude[1], vector_longitude[1]);
   auto p3 = geo2xyz(vector_latitude[2], vector_longitude[2]);

   cout << vector_latitude[0] << "  " << vector_longitude[0] << endl;
   cout << vector_latitude[1] << "  " << vector_longitude[1] << endl;
   cout << vector_latitude[2] << "  " << vector_longitude[2] << endl;

   cout << p1.x <<"  "<< p1.y <<"  "<< p1.z << endl;
   cout << p2.x <<"  "<< p2.y <<"  "<< p2.z << endl;
   cout << p3.x <<"  "<< p3.y <<"  "<< p3.z << endl;


   long double x1 = p1.x; 
   long double y1 = p1.y;
   long double z1 = p1.z;

   long double x2 = p2.x;
   long double y2 = p2.y;
   long double z2 = p2.z;
   
   long double x3 = p3.x;
   long double y3 = p3.y;
   long double z3 = p3.z;


   // 计算向量 p2p1 和 p2p3 的夹角 https://www.zybang.com/question/3379a30c0dd3041b3ef966803f0bf758.html
   // 计算p2p1模,p2p3模
   long double _p1p2 = sqrt(pow((x1 - x2),2) + pow((y1 - y2),2)+ pow((z1 - z2),2));
   long double _p2p3 = sqrt(pow((x3 - x2),2) + pow((y3 - y2),2)+ pow((z3 - z2),2));
   long double p = (x1 - x2) * (x3 - x2) + (y1 - y2) * (y3 - y2) + (z1 - z2) * (z3 - z2); //计算p2p1向量*p2p3向量




   cout <<"p:                         "<< p <<endl;
   cout <<"_p1p2:                     "<< _p1p2 << endl;
   cout <<"_p2p3:                     "<< _p2p3 << endl;
   cout <<"_p1p2 * _p2p3:             "<<(_p1p2 * _p2p3) << endl;
   cout <<"acos(p / (_p1p2 * _p2p3)): "<< acos(p / (_p1p2 * _p2p3)) << endl;

   //cout << acos(-1) << endl;
   //cout << acos(0) << endl;
   //cout << acos(1) << endl;

   return (acos(p/ (_p1p2 * _p2p3)) / pi) * 180;
}

int main()
{
   vector<long double> vector_latitude;
   vector<long double> vector_longitude;
   vector_latitude.push_back(40.823978);
   vector_latitude.push_back(40.71986);
   vector_latitude.push_back(40.484791);

   vector_longitude.push_back(124.639313);
   vector_longitude.push_back(124.78175);
   vector_longitude.push_back(124.81823);
   cout <<angleoflocation(vector_latitude, vector_longitude) << endl;
   return 0;
}

步骤是：先知道经度是x，维度是y。将经纬度转弧度，然后得出坐标对应的x，y，z。然后利用余弦定理，通过acos反求角度，再将弧度转回角度。

通过坐标拾取网站查这三个的地址，然后用高德地图查找对应位置，然后再用PS的尺子得到近似的角度，验证这个方法是否正确。


很好，得出的角度跟这个是对应上的，说明这个算法是正确的。
下面我将用我的航点来计算，看看能不能得出航点之间的夹角，图如下：

想法：以航点1为原点，10为x轴一个坐标系，通过两个航点GPS坐标得到对应向量，求得∠012，然后再以航点2为原点，以平行10的为坐标系，然后再通过判断1在2的哪个象限从而判断对应度数。

我能通过航点012对应GPS转成三个向量得到∠012、∠123，但也解决不了飞机在航点1到航点2的顺逆时针旋转的问题，为什么？是因为坐标系不一样！问题在于这个象限在哪里判断不了，因为坐标系不一样所以不是简单判断x1，x2，y1，y2就能判断的。

航点之间的角度

所以开始了新的想法，如图所示：

想法：以北极为起点，然后通过旋转的角度，得到对应的度数（对代码中那条公式未能理解）

参考链接：
1.计算距离、方位和更多经纬度之间的点
2.ATan2与ATan的区别
3.两坐标角度验证
4.参考代码的博主

#include <iostream>
#include <math.h>

using namespace std;

#define _PI 3.1415926
#define PI_180 0.01745329   //pi/180  角度转弧度
#define Earth_R 6372.795477598

void getDistanceBearing(double StartLat,double StartLng, double EndLat,double EndLng, double dis = 0,double bearing = 0)
{
    //Fai对应纬度 Lat，temp对应经度差 longitude
    double Fai1 = StartLat * PI_180;
    double Fai2 = EndLat * PI_180;
    double temp = (EndLng - StartLng) * PI_180;

    dis = Earth_R * acos(sin(Fai1) * sin(Fai2) + cos(Fai1)*cos(Fai2)*cos(temp)) * 1000;//已经换算成m了
    bearing = atan2(sin(temp)*cos(Fai2),cos(Fai1)*sin(Fai2)-sin(Fai1)*cos(Fai2)*cos(temp))/PI_180;//弧度, atan2(y,x)
	if(bearing < 0)
		bearing += 360;
    cout << "dis:" << dis << endl;
    cout << "bearing:" << bearing<<endl;

}
int main()
{
    //test_2
    getDistanceBearing(23.1690954,113.3342307,23.1693954,113.3340307);
    getDistanceBearing(23.1693954,113.3340307,23.1692634,113.3342328);
    getDistanceBearing(23.1692634,113.3342328,23.1693725,113.3340454);
    getDistanceBearing(23.1693725,113.3340454,23.1693096,113.3338454);
    getDistanceBearing(23.1693096,113.3338454,23.1692956,113.3343745);
    return 0;
}

航点图如下：可以看到0到1是顺时针，1到2是顺时针，2到3是逆时针，3到4是逆时针。

在waypoint任务里对角度<180° 或者>180°进行判断。

终端看看输出结果，结果是顺顺逆逆，很好。



 但无奈的是，大疆waypointV2会对该航点到下一个航点自动按照最近角度去旋转，不管在该航点是否转动飞机偏航角，都会按该航点飞机最后的机头角度跟下一个航点方向以最小角度去旋转。
 也就是我做了无用功,不明白turnmode用法，不知道其实只要设置headingmode = 0就可以让航点之间以最小角度方向旋转然后后直线飞到下一个航点了。
 关于turnmode用法，是要先设置headingmode = 3情况下，设置waypointV2.heading =90 是以顺时针旋转90度，turnmode = 0是顺时针旋转，
 如果turnmode = 1是逆时针旋转90。这个heading不能每个航点一样，否则方向都是一样的，只有首航点才会旋转，之后航点因为飞机朝向都是一样的，根本就不会旋转了。所以我才会用0，90，180，这样来验证。

 至此重新复习了一下高中数学。对于这些坐标系统转换的其实还要进一步学习（学习链接），了解北京54、西安80、WGS84、国家大地坐标系 CGCS2000的由来和转换。

最后再来给个介绍M300 RTK的链接