![]() |
ATC
Advanced Trip Computer
|
00001 00004 #include <math.h> 00005 00006 #define PI 3.141592653589793 00007 00008 #define WGS84_E2 0.006694379990197 00009 #define WGS84_E4 WGS84_E2*WGS84_E2 00010 #define WGS84_E6 WGS84_E4*WGS84_E2 00011 #define WGS84_SEMI_MAJOR_AXIS 6378137.0 00012 #define WGS84_SEMI_MINOR_AXIS 6356752.314245 00013 #define UTM_LONGITUDE_OF_ORIGIN 3.0/180.0*PI 00014 #define UTM_LATITUDE_OF_ORIGIN 0.0 00015 #define UTM_FALSE_EASTING 500000.0 00016 #define UTM_FALSE_NORTHING_N 0.0 00017 #define UTM_FALSE_NORTHING_S 10000000.0 00018 #define UTM_SCALE_FACTOR 0.9996 00019 00020 00021 double m_calc(double latitude) 00022 { 00023 return (1.0 - WGS84_E2/4.0 - 3.0*WGS84_E4/64.0 - 5.0*WGS84_E6/256.0)*latitude - (3.0*WGS84_E2/8.0 + 3.0*WGS84_E4/32.0 + 45.0*WGS84_E6/1024.0)*sin(2.0*latitude) + (15.0*WGS84_E4/256.0 + 45.0*WGS84_E6/1024.0)*sin(4.0*latitude) - (35.0*WGS84_E6/3072.0)*sin(6.0*latitude); 00024 } 00025 00033 void GPS2UTM(double latitude, double longitude, double* east, double* north) 00034 { 00035 int int_zone; 00036 double M, M_origin, A, A2, e2_prim, C, T, v; 00037 00038 int_zone = (int)(longitude/6.0); 00039 if (longitude < 0) 00040 int_zone--; 00041 longitude -= (double)(int_zone)*6.0; 00042 longitude *= PI/180.0; 00043 latitude *= PI/180.0; 00044 M = WGS84_SEMI_MAJOR_AXIS*m_calc(latitude); 00045 M_origin = WGS84_SEMI_MAJOR_AXIS*m_calc(UTM_LATITUDE_OF_ORIGIN); 00046 A = (longitude - UTM_LONGITUDE_OF_ORIGIN)*cos(latitude); 00047 A2 = A*A; 00048 e2_prim = WGS84_E2/(1.0 - WGS84_E2); 00049 C = e2_prim*pow(cos(latitude),2.0); 00050 T = tan(latitude); 00051 T *= T; 00052 v = WGS84_SEMI_MAJOR_AXIS/sqrt(1.0 - WGS84_E2*pow(sin(latitude),2.0)); 00053 *north = UTM_SCALE_FACTOR*(M - M_origin + v*tan(latitude)*(A2/2.0 + (5.0 - T + 9.0*C + 4.0*C*C)*A2*A2/24.0 + (61.0 - 58.0*T + T*T + 600.0*C - 330.0*e2_prim)*A2*A2*A2/720.0)); 00054 if (latitude < 0) 00055 *north += UTM_FALSE_NORTHING_S; 00056 *east = UTM_FALSE_EASTING + UTM_SCALE_FACTOR*v*(A + (1.0 - T + C)*A2*A/6.0 + (5.0 - 18.0*T + T*T + 72.0*C - 58.0*e2_prim)*A2*A2*A/120.0); 00057 return; 00058 }