已知两点经纬度求航向

http://blog.sina.com.cn/s/blog_658a93570101hynw.html

//###################################################################################################
//求线AB与正北夹角，即方位角
//###################################################################################################
public double getAzimuth(PointLB pA, PointLB pB)
{
    double a = (90 - pB.lat) * Math.PI / 180;
    double b = (90 - pA.lat) * Math.PI / 180;
    double AOC_BOC = (pB.lon - pA.lon) * Math.PI / 180;
    double cosc = Math.Cos(a) * Math.Cos(b) + Math.Sin(a) * Math.Sin(b) * Math.Cos(AOC_BOC);
    double sinc = Math.Sqrt(1 - cosc * cosc);
    double sinA = Math.Sin(a) * Math.Sin(AOC_BOC) / sinc;
    double A = Math.Asin(sinA) * 180 / Math.PI;
    double res = 0;
    if (pB.lon > pA.lon && pB.lat > pA.lat) res = A;
    else if (pB.lon > pA.lon && pB.lat < pA.lat) res = 180 - A;
    else if (pB.lon < pA.lon && pB.lat < pA.lat) res = 180 - A;
    else if (pB.lon < pA.lon && pB.lat > pA.lat) res = 360 + A;
    else if (pB.lon > pA.lon && pB.lat == pA.lat) res = 90;
    else if (pB.lon < pA.lon && pB.lat == pA.lat) res = 270;
    else if (pB.lon == pA.lon && pB.lat > pA.lat) res = 0;
    else if (pB.lon == pA.lon && pB.lat < pA.lat) res = 180;

    return res;
}