art_common: art_common: UTM.h Source File

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