CASL 计算两GPS坐标的距离和方位

最新推荐文章于 2021-11-19 22:18:17 发布

团圆吧

最新推荐文章于 2021-11-19 22:18:17 发布

阅读量383

点赞数

分类专栏： CASL 文章标签： c语言

本文链接：https://blog.csdn.net/weixin_42148809/article/details/121253783

版权

CASL 专栏收录该内容

8 篇文章 0 订阅

订阅专栏

该代码实现了一个名为gpsDst_m的功能，用于根据WGS坐标系下的经纬度计算两点间的距离。它支持三种输出类型：相对于参考点的距离、相对于前一帧的距离和指北针角度。利用地球平均半径和Haversine公式，计算出两点间的最短距离，并在考虑最大速度限制的情况下进行输出。此外，还提供了计算方向的角度信息。

摘要由CSDN通过智能技术生成

function gpsDst_m (var gpsLat_wgs,var gpsLng_wgs)
{
  //! 输出方式:0-相对参考点距离，1－相对前一帧距离，2-指北针角度
  int outPutType=0;
  //! 参考点经度，默认0
  double gpsLngRef_wgs=0;
  //! 参考点纬度，默认0
  double gpsLatRef_wgs=0;
  //! 地球平均半径: 6371km
  double EARTH_RADIUS=6371;
  //! 最大距离值，只输出最大距离值内的距离变化情况
  double maxDst_m=1000;
  //! 最大可能速度，outPutType＝1时，可能允许的最大相邻GPS点移动速率值，超过参考速度的gpsDst值将被设置为负值
  double maxGPSSpd_kph=5000;

  double PI=3.1415926;

  double lat0,lat1,lng0,lng1;
  double dlng,dlat;
  double h;
  double distance_m;
  double maxDst_m_tmp;
  double timeDelta;

  double deltaLat;
  double deltaLng;
  double theta,theta_l;

  int firstFramsCnt=1;
  if (firstFramsCnt>0)
  {
    maxDst_m_tmp=maxDst_m;
    --firstFramsCnt;
  }

  if (outPutType>0)
  {
    if (outPutType==2)
    {
      gpsLatRef_wgs=gpsLat_wgs[-5];
      gpsLngRef_wgs=gpsLng_wgs[-5];
      timeDelta=(time(gpsLat_wgs)-time(gpsLat_wgs[-5]));
    }
    else
    {
      gpsLatRef_wgs=gpsLat_wgs[-1];
      gpsLngRef_wgs=gpsLng_wgs[-1];
      timeDelta=(time(gpsLat_wgs)-time(gpsLat_wgs[-1]));
    }

    timeDelta=max(0.05,timeDelta);//防止出现0值
  }

  //用haversine公式计算球面两点间的距离。
  //经纬度转换成弧度
  lat0 = gpsLatRef_wgs * PI / 180;
  lat1 = gpsLat_wgs * PI / 180;
  lng0 = gpsLngRef_wgs * PI / 180;
  lng1 = gpsLng_wgs * PI / 180;

  deltaLat=lat1-lat0;
  deltaLng=lng1-lng0;

  if (deltaLng==0)
  {
    if (deltaLat>0)
    {
      theta=0;
    }
    else if(deltaLat<0)
    {
      theta=180;
    }
    else
    {
      theta=theta_l;
    }
  }
  else
  {
    if (deltaLat==0)
    {
      if (deltaLng>0)
      {
        theta=-90;
      }
      else
      { 
        theta=90;
      }
    }
    else
    {
      theta=atan(abs(deltaLat/deltaLng));
      if (deltaLng>0)
      {
        if (deltaLat>0)
        {
          theta=theta*180/PI-90;
        }
        else
        {
          theta=-(theta*180/PI+90);
        }
      }
      else
      {
        if (deltaLat>0)
        {
          theta=90-theta*180/PI;
        }
        else
        {
          theta=90+theta*180/PI;
        }
      }
    }
  }
  theta_l=theta;

  dlng = abs(lng0 - lng1);
  dlat = abs(lat0 - lat1);

  h = pow(sin(dlat*0.5),2) + cos(lat0) * cos(lat1) * pow(sin(dlng*0.5),2);
  
  distance_m = 2 * EARTH_RADIUS * asin(sqrt(h))*1000;
  
  if (outPutType>0)
  {
    maxDst_m_tmp=min(maxDst_m,maxGPSSpd_kph/3.6*timeDelta);
    
    if (outPutType==2)//输出向北的夹角
    {
      return theta;//gps递推方向与纬度北向的夹角，逆时针为正，顺时针为负
    }
  }
  distance_m =min(maxDst_m_tmp,distance_m);

  return distance_m;
}