经纬度计算地球两点间距离

地面两点A和B的经纬度坐标分别为(Aj,Aw)和(Bj,Bw),地球半径R取平均值6371km。

地球球心为原点O，地轴为Z轴，北极方向为Z轴正方向，赤道平面为X轴和Y轴所在平面，在该平面上地心到零度经线的方向为X轴正方向，根据右手定则确定Y轴正方向。
设点A的三维坐标为(Ax,Ay,Az)，点B的三维坐标为(Bx,By,Bz)

A、B、O三点所在平面与地球相交形成一个半径为R的圆，求AB间的地面距离就是求该圆上圆弧AB的长度。可由弧长等于半径乘以圆心角公式求得。
由于R是确定的，只要获得OA与OB的夹角θ就可以获得弧AB的长度。弧AB=R*θ。
角θ可通过向量公式求得：向量OA*向量OB=|OA||OB|cosθ。
则
cosθ=向量OA*向量OB/|OA||OB|
     =(Ax*Bx+Ay*By+Az*Bz)/R*R

用经纬度坐标表示三维直角坐标：
Ax=R*cosAw*cosAj
Ay=R*cosAw*sinAj
Az=R*sinAw

Bx=R*cosBw*cosBj
By=R*cosBw*sinBj
Bz=R*sinBw

代入可得
cosθ=cosAw*cosAj*cosBw*cosBj+cosAw*sinAj*cosBw*sinBj+sinAw*sinBw
     =cosAw*cosBw(cosAj*cosBj+sinAj*sinBj)+sinAw*sinBw
     =cosAw*cosBw*cos(Aj-Bj)+sinAw*sinBw
θ=arccos[cosAw*cosBw*cos(Aj-Bj)+sinAw*sinBw]

5综上可得根据经纬度计算地面两点间距离的公式：
弧AB=R*arccos[cosAw*cosBw*cos(Aj-Bj)+sinAw*sinBw]
 

代码如下
double GetDistanceA(double Srclon, double Srclat, double Destlon, double Destlat)
{
    double r1 = rad(Srclat);
    double r2 = rad(Srclon);
    double a = rad(Destlat);
    double b = rad(Destlon);
    double s = acos(cos(r1) * cos(a) * cos(r2 - b) + sin(r1) * sin(a)) * EARTH_RADIUS * 1000;
    return s;
}