gps_common: conversions.h Source File

00001 /* Taken from utexas-art-ros-pkg:art_vehicle/applanix */
00002 
00003 /*
00004  * Conversions between coordinate systems.
00005  *
00006  * Includes LatLong<->UTM.
00007  */
00008 
00009 #ifndef _UTM_H
00010 #define _UTM_H
00011 
00023 #include <cmath>
00024 #include <stdio.h>
00025 #include <stdlib.h>
00026 
00027 namespace gps_common
00028 {
00029 
00030 const double RADIANS_PER_DEGREE = M_PI/180.0;
00031 const double DEGREES_PER_RADIAN = 180.0/M_PI;
00032 
00033 // WGS84 Parameters
00034 const double WGS84_A = 6378137.0;               // major axis
00035 const double WGS84_B = 6356752.31424518;        // minor axis
00036 const double WGS84_F = 0.0033528107;            // ellipsoid flattening
00037 const double WGS84_E = 0.0818191908;            // first eccentricity
00038 const double WGS84_EP = 0.0820944379;           // second eccentricity
00039 
00040 // UTM Parameters
00041 const double UTM_K0 = 0.9996;                   // scale factor
00042 const double UTM_FE = 500000.0;         // false easting
00043 const double UTM_FN_N = 0.0;                    // false northing on north hemisphere
00044 const double UTM_FN_S = 10000000.0;             // false northing on south hemisphere
00045 const double UTM_E2 = (WGS84_E*WGS84_E);        // e^2
00046 const double UTM_E4 = (UTM_E2*UTM_E2);          // e^4
00047 const double UTM_E6 = (UTM_E4*UTM_E2);          // e^6
00048 const double UTM_EP2 = (UTM_E2/(1-UTM_E2));     // e'^2
00049 
00057 static inline void UTM(double lat, double lon, double *x, double *y)
00058 {
00059   // constants
00060   const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
00061   const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
00062   const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
00063   const static double m3 = -(35*UTM_E6/3072);
00064 
00065   // compute the central meridian
00066   int cm = ((lon >= 0.0)
00067             ? ((int)lon - ((int)lon)%6 + 3)
00068             : ((int)lon - ((int)lon)%6 - 3));
00069 
00070   // convert degrees into radians
00071   double rlat = lat * RADIANS_PER_DEGREE;
00072   double rlon = lon * RADIANS_PER_DEGREE;
00073   double rlon0 = cm * RADIANS_PER_DEGREE;
00074 
00075   // compute trigonometric functions
00076   double slat = sin(rlat);
00077   double clat = cos(rlat);
00078   double tlat = tan(rlat);
00079 
00080   // decide the false northing at origin
00081   double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
00082 
00083   double T = tlat * tlat;
00084   double C = UTM_EP2 * clat * clat;
00085   double A = (rlon - rlon0) * clat;
00086   double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
00087                         + m2*sin(4*rlat) + m3*sin(6*rlat));
00088   double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
00089 
00090   // compute the easting-northing coordinates
00091   *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6
00092                               + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120);
00093   *y = fn + UTM_K0 * (M + V * tlat * (A*A/2
00094                                       + (5-T+9*C+4*C*C)*pow(A,4)/24
00095                                       + ((61-58*T+T*T+600*C-330*UTM_EP2)
00096                                          * pow(A,6)/720)));
00097 
00098   return;
00099 }
00100 
00101 
00110 static inline char UTMLetterDesignator(double Lat)
00111 {
00112         char LetterDesignator;
00113 
00114         if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
00115         else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
00116         else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
00117         else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
00118         else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
00119         else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
00120         else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
00121         else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
00122         else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
00123         else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
00124         else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
00125         else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
00126         else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
00127         else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
00128         else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
00129         else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
00130         else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
00131         else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
00132         else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
00133         else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
00134         // 'Z' is an error flag, the Latitude is outside the UTM limits
00135         else LetterDesignator = 'Z';
00136         return LetterDesignator;
00137 }
00138 
00148 static inline void LLtoUTM(const double Lat, const double Long,
00149                            double &UTMNorthing, double &UTMEasting,
00150                            char* UTMZone)
00151 {
00152         double a = WGS84_A;
00153         double eccSquared = UTM_E2;
00154         double k0 = UTM_K0;
00155 
00156         double LongOrigin;
00157         double eccPrimeSquared;
00158         double N, T, C, A, M;
00159 
00160         //Make sure the longitude is between -180.00 .. 179.9
00161         double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
00162 
00163         double LatRad = Lat*RADIANS_PER_DEGREE;
00164         double LongRad = LongTemp*RADIANS_PER_DEGREE;
00165         double LongOriginRad;
00166         int    ZoneNumber;
00167 
00168         ZoneNumber = int((LongTemp + 180)/6) + 1;
00169 
00170         if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
00171                 ZoneNumber = 32;
00172 
00173         // Special zones for Svalbard
00174         if( Lat >= 72.0 && Lat < 84.0 )
00175         {
00176           if(      LongTemp >= 0.0  && LongTemp <  9.0 ) ZoneNumber = 31;
00177           else if( LongTemp >= 9.0  && LongTemp < 21.0 ) ZoneNumber = 33;
00178           else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
00179           else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
00180          }
00181         // +3 puts origin in middle of zone
00182         LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
00183         LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
00184 
00185         //compute the UTM Zone from the latitude and longitude
00186         snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
00187 
00188         eccPrimeSquared = (eccSquared)/(1-eccSquared);
00189 
00190         N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
00191         T = tan(LatRad)*tan(LatRad);
00192         C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
00193         A = cos(LatRad)*(LongRad-LongOriginRad);
00194 
00195         M = a*((1       - eccSquared/4          - 3*eccSquared*eccSquared/64    - 5*eccSquared*eccSquared*eccSquared/256)*LatRad
00196                                 - (3*eccSquared/8       + 3*eccSquared*eccSquared/32    + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
00197                                                                         + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
00198                                                                         - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
00199 
00200         UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
00201                                         + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
00202                                         + 500000.0);
00203 
00204         UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
00205                                  + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
00206         if(Lat < 0)
00207                 UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
00208 }
00209 
00210 static inline void LLtoUTM(const double Lat, const double Long,
00211                            double &UTMNorthing, double &UTMEasting,
00212                            std::string &UTMZone) {
00213   char zone_buf[] = {0, 0, 0, 0};
00214 
00215   LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf);
00216 
00217   UTMZone = zone_buf;
00218 }
00219 
00220 
00230 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
00231                            const char* UTMZone, double& Lat,  double& Long )
00232 {
00233         double k0 = UTM_K0;
00234         double a = WGS84_A;
00235         double eccSquared = UTM_E2;
00236         double eccPrimeSquared;
00237         double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
00238         double N1, T1, C1, R1, D, M;
00239         double LongOrigin;
00240         double mu, phi1, phi1Rad;
00241         double x, y;
00242         int ZoneNumber;
00243         char* ZoneLetter;
00244         int NorthernHemisphere; //1 for northern hemispher, 0 for southern
00245 
00246         x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
00247         y = UTMNorthing;
00248 
00249         ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
00250         if((*ZoneLetter - 'N') >= 0)
00251                 NorthernHemisphere = 1;//point is in northern hemisphere
00252         else
00253         {
00254                 NorthernHemisphere = 0;//point is in southern hemisphere
00255                 y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
00256         }
00257 
00258         LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;  //+3 puts origin in middle of zone
00259 
00260         eccPrimeSquared = (eccSquared)/(1-eccSquared);
00261 
00262         M = y / k0;
00263         mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
00264 
00265         phi1Rad = mu    + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
00266                                 + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
00267                                 +(151*e1*e1*e1/96)*sin(6*mu);
00268         phi1 = phi1Rad*DEGREES_PER_RADIAN;
00269 
00270         N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
00271         T1 = tan(phi1Rad)*tan(phi1Rad);
00272         C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
00273         R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
00274         D = x/(N1*k0);
00275 
00276         Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
00277                                         +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
00278         Lat = Lat * DEGREES_PER_RADIAN;
00279 
00280         Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
00281                                         *D*D*D*D*D/120)/cos(phi1Rad);
00282         Long = LongOrigin + Long * DEGREES_PER_RADIAN;
00283 
00284 }
00285 
00286 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
00287     std::string UTMZone, double& Lat, double& Long) {
00288   UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
00289 }
00290 
00291 } // end namespace UTM
00292 
00293 #endif // _UTM_H