计算球面两点间的航向角度

function Between(const Value, LowGate, HighGate: Double): Boolean; overload;
{
 判断Value是否在[LowGate,HighGate]内，是：返回True，否，返回False;
}
begin
 Result := (Value >= LowGate) and (Value <= HighGate);
end;
function SphericalMod(X: Extended): Extended;
begin
 Result := X;
 if X < -Pi then
   Result := X + Pi + Pi
 else
   if X > Pi then
     Result := X - Pi - Pi;
end;
//输入为经纬度的2D点坐标，单位为十进制度
//输出为两点大圆同正北轴的夹角，单位为十进制度
function GetDirection(Lat1, Long1, Lat2, Long2: Double): Double;
var
 SLat1, SLat2, CLat1, CLat2: Extended;
 SLonDiff, CLonDiff: Extended;
begin
 Result := -1;
 Assert(Between(Lat1, -90, 90) and Between(Long1, -180, 180)
        and Between(Lat2, -90, 90) and Between(Long2, -180, 180),
        'Error GPS data');
 // 静止不动
 if SameValue(Lat1, Lat2) and SameValue(Long1,Long2) then Exit;
 SinCos(degtorad(Lat1), SLat1, CLat1);
 SinCos(degtorad(Lat2), SLat2, CLat2);
 SinCos(degtorad((Long2 - Long1)), SLonDiff, CLonDiff);
 Result := RadToDeg(SphericalMod(ArcTan2(SLonDiff * CLat2, CLat1 * SLat2 - SLat1 * CLat2 * CLonDiff)));
end;
procedure TForm1.btn1Click(Sender: TObject);
begin
 Caption := FloatToStr(GetDirection(StrToFloat(edt1.Text),
                                    StrToFloat(edt2.Text),
                                    StrToFloat(edt3.Text),
                                    StrToFloat(edt4.Text)));
end;