1. 首页
  2. 技术
  3. Linux

c++根据经纬度计算航向算法

struct PointLB {
double lat;
double lon;
};
double getAzimuth(PointLB pA, PointLB pB)
{
  double a = (90 - pB.lat) * PI / 180;
  double b = (90 - pA.lat) * PI / 180;
  double AOC_BOC = (pB.lon - pA.lon) * PI / 180;
  double cosc = cos(a) * cos(b) + sin(a) * sin(b) * cos(AOC_BOC);
  double sinc = sqrt(1 - cosc * cosc);
  double sinA = sin(a) * sin(AOC_BOC) / sinc;
  double A = asin(sinA) * 180 / PI;
  double res = 0;
  if (pB.lon > pA.lon && pB.lat > pA.lat) res = A;
  else if (pB.lon > pA.lon && pB.lat < pA.lat) res = 180 - A;
  else if (pB.lon < pA.lon && pB.lat < pA.lat) res = 180 - A;
  else if (pB.lon < pA.lon && pB.lat > pA.lat) res = 360 + A;
  else if (pB.lon > pA.lon && pB.lat == pA.lat) res = 90;
  else if (pB.lon < pA.lon && pB.lat == pA.lat) res = 270;
  else if (pB.lon == pA.lon && pB.lat > pA.lat) res = 0;
  else if (pB.lon == pA.lon && pB.lat < pA.lat) res = 180;

  return res;
}

 

原创文章,作者:admin,如若转载,请注明出处:https://www.huiyingwu.com/4241/

发表评论

您的电子邮箱地址不会被公开。 必填项已用*标注