根据wgs84 gps坐标信号，计算GPS推进距离

 以下CASL代码，用于vSignalyzer中，根据wgs84坐标的经度和纬度信号，计算gps轨迹的推进距离。

function gpsDst_m (var gpsLat_wgs,var gpsLng_wgs)
{
  //! 输出方式:0-相对参考点距离，1－相对前一帧距离，2-指北针角度
  int outPutType=0;
  //! 0:引用gpsDst_refLon/Lat，1:使用gpsLng/latRef_wgs
  int refGpsType=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;

    if(refGpsType==0)
    {
      gpsLngRef_wgs = gpsDst_refLon;
      gpsLatRef_wgs = gpsDst_refLat;
    }

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