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