2点GPS坐标求方位角

#include 
#include 
#include 

#define PI 3.1415926

#define rad(x) (x * PI / 180.0)


double gps2d(double lata, double lona, double latb, double lonb)
{
    double d  = 0;
    double lta = lata * PI/180;
    double lga = lona * PI/180;
    double ltb = latb * PI/180;
    double lgb = lonb * PI/180;

    d = sin(lta)* sin(ltb) + cos(lta)*cos(ltb)*cos(lgb-lga);
    d = sqrt(1-d*d);
    d = cos(ltb)*sin(lgb-lga)/d;
    d = asin(d)*180.0/PI;


    printf("d = %lf\n",d);
    return d;
}

double gps12d(double lata, double lona, double latb, double lonb)
{
    double d  = 0;
   double radlta = rad(lata);
    double radlna = rad(lona);
    double radltb = rad(latb);
    double radlnb = rad(lonb);

    double dlon = radlnb - radlna;
    double y = sin(dlon) * cos(radlta);
    double x = cos(radlta) * sin(radltb) - sin(radlta) * cos(radltb) * cos(dlon);
//    d = atan2(y,x) * 180.0 / PI;
    if ( y > 0 ){
        if (x > 0)
            d = atan2(y,x);
        else if ( x == 0 )
            d = 90;
        else
            d = 180 - atan2(-y,x);
    } 
    else if ( y == 0)
    {
        if (x > 0)
            d = 0;
        else if ( x == 0 )
            d = 0;
        else
            d = 180;
    }   
    else 
    {
        if (x > 0)
            d = -atan2(-y,x);
        else if ( x == 0 )
            d = 270;
        else
            d = atan2(y,x)-180;
    }   

    d = d * 180.0 / PI;
//    d = (int)(d + 360)%360;

    printf("d = %lf\n",d);
    return d;
}


int main(int argc, char **argv)
{
    double d  = 0;
    d = gps2d(29538171, 1066030353, 295381761, 1066039656);
    d = gps12d(29538171, 1066030353, 295381761, 1066039656);
    d = atan2((29.538176-29.538176), (106.6030353-106.6039656))/PI*180.0;
    printf("d = %lf\n",d);

    return 0;
}