2点GPS坐标求方位角

#include <stdio.h>#include <stdlib.h>#include <math.h>#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 = %lfn",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 = %lfn",d); return d;}int main(int argc, char gsdbbargv){ 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 = %lfn",d); return 0;}