UTM.cpp 12.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
// UTM.c

// Original Javascript by Chuck Taylor
// Port to C++ by Alex Hajnal
//
// *** THIS CODE USES 32-BIT FLOATS BY DEFAULT ***
// *** For 64-bit double-precision edit UTM.h: undefine FLOAT_32 and define FLOAT_64
// 
// This is a simple port of the code on the Geographic/UTM Coordinate Converter (1) page from Javascript to C++.
// Using this you can easily convert between UTM and WGS84 (latitude and longitude).
// Accuracy seems to be around 50cm (I suspect rounding errors are limiting precision).
// This code is provided as-is and has been minimally tested; enjoy but use at your own risk!
// The license for UTM.cpp and UTM.h is the same as the original Javascript: 
// "The C++ source code in UTM.cpp and UTM.h may be copied and reused without restriction."
// 
// 1) http://home.hiwaay.net/~taylorc/toolbox/geography/geoutm.html

18 19
// QGC Note: This file has been slightly modified to prevent possible conflicts with other parts of the system

20 21
#include "UTM.h"

22 23 24 25 26 27 28 29 30 31 32
#include <math.h>

#define pi 3.14159265358979

/* Ellipsoid model constants (actual values here are for WGS84) */
#define sm_a 6378137.0
#define sm_b 6356752.314
#define sm_EccSquared 6.69437999013e-03

#define UTMScaleFactor 0.9996

33 34
// DegToRad
// Converts degrees to radians.
35
double DegToRad(double deg) {
36 37 38 39 40 41
  return (deg / 180.0 * pi);
}


// RadToDeg
// Converts radians to degrees.
42
double RadToDeg(double rad) {
43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61
  return (rad / pi * 180.0);
}

// ArcLengthOfMeridian
// Computes the ellipsoidal distance from the equator to a point at a
// given latitude.
// 
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
// 
// Inputs:
//     phi - Latitude of the point, in radians.
// 
// Globals:
//     sm_a - Ellipsoid model major axis.
//     sm_b - Ellipsoid model minor axis.
// 
// Returns:
//     The ellipsoidal distance of the point from the equator, in meters.
62 63 64
double ArcLengthOfMeridian (double phi) {
  double alpha, beta, gamma, delta, epsilon, n;
  double result;
65 66 67 68 69 70
  
  /* Precalculate n */
  n = (sm_a - sm_b) / (sm_a + sm_b);
  
  /* Precalculate alpha */
  alpha = ((sm_a + sm_b) / 2.0)
71
        * (1.0 + (pow(n, 2.0) / 4.0) + (pow(n, 4.0) / 64.0));
72 73
  
  /* Precalculate beta */
74 75
  beta = (-3.0 * n / 2.0) + (9.0 * pow(n, 3.0) / 16.0)
       + (-3.0 * pow(n, 5.0) / 32.0);
76 77
  
  /* Precalculate gamma */
78 79
  gamma = (15.0 * pow(n, 2.0) / 16.0)
        + (-15.0 * pow(n, 4.0) / 32.0);
80 81
  
  /* Precalculate delta */
82 83
  delta = (-35.0 * pow(n, 3.0) / 48.0)
      + (105.0 * pow(n, 5.0) / 256.0);
84 85
  
  /* Precalculate epsilon */
86
  epsilon = (315.0 * pow(n, 4.0) / 512.0);
87 88 89
  
  /* Now calculate the sum of the series and return */
  result = alpha
90 91 92 93
         * (phi + (beta * sin(2.0 * phi))
         + (gamma * sin(4.0 * phi))
         + (delta * sin(6.0 * phi))
         + (epsilon * sin(8.0 * phi)));
94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
  
  return result;
}



// UTMCentralMeridian
// Determines the central meridian for the given UTM zone.
//
// Inputs:
//     zone - An integer value designating the UTM zone, range [1,60].
//
// Returns:
//   The central meridian for the given UTM zone, in radians
//   Range of the central meridian is the radian equivalent of [-177,+177].
109 110 111
double UTMCentralMeridian(int zone) {
  double cmeridian;
  cmeridian = DegToRad(-183.0 + ((double)zone * 6.0));
112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
  
  return cmeridian;
}



// FootpointLatitude
//
// Computes the footpoint latitude for use in converting transverse
// Mercator coordinates to ellipsoidal coordinates.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
//   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//   y - The UTM northing coordinate, in meters.
//
// Returns:
//   The footpoint latitude, in radians.
131 132 133
double FootpointLatitude(double y) {
  double y_, alpha_, beta_, gamma_, delta_, epsilon_, n;
  double result;
134 135 136 137 138 139 140
  
  /* Precalculate n (Eq. 10.18) */
  n = (sm_a - sm_b) / (sm_a + sm_b);
    
  /* Precalculate alpha_ (Eq. 10.22) */
  /* (Same as alpha in Eq. 10.17) */
  alpha_ = ((sm_a + sm_b) / 2.0)
141
         * (1 + (pow(n, 2.0) / 4) + (pow(n, 4.0) / 64));
142 143 144 145 146
  
  /* Precalculate y_ (Eq. 10.23) */
  y_ = y / alpha_;
  
  /* Precalculate beta_ (Eq. 10.22) */
147 148
  beta_ = (3.0 * n / 2.0) + (-27.0 * pow(n, 3.0) / 32.0)
        + (269.0 * pow(n, 5.0) / 512.0);
149 150
  
  /* Precalculate gamma_ (Eq. 10.22) */
151 152
  gamma_ = (21.0 * pow(n, 2.0) / 16.0)
         + (-55.0 * pow(n, 4.0) / 32.0);
153 154
    
  /* Precalculate delta_ (Eq. 10.22) */
155 156
  delta_ = (151.0 * pow(n, 3.0) / 96.0)
         + (-417.0 * pow(n, 5.0) / 128.0);
157 158
    
  /* Precalculate epsilon_ (Eq. 10.22) */
159
  epsilon_ = (1097.0 * pow(n, 4.0) / 512.0);
160 161
    
  /* Now calculate the sum of the series (Eq. 10.21) */
162 163 164 165
  result = y_ + (beta_ * sin(2.0 * y_))
         + (gamma_ * sin(4.0 * y_))
         + (delta_ * sin(6.0 * y_))
         + (epsilon_ * sin(8.0 * y_));
166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
  
  return result;
}



// MapLatLonToXY
// Converts a latitude/longitude pair to x and y coordinates in the
// Transverse Mercator projection.  Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//    phi - Latitude of the point, in radians.
//    lambda - Longitude of the point, in radians.
//    lambda0 - Longitude of the central meridian to be used, in radians.
//
// Outputs:
//    x - The x coordinate of the computed point.
//    y - The y coordinate of the computed point.
//
// Returns:
//    The function does not return a value.
191 192 193 194
void MapLatLonToXY (double phi, double lambda, double lambda0, double &x, double &y) {
    double N, nu2, ep2, t, t2, l;
    double l3coef, l4coef, l5coef, l6coef, l7coef, l8coef;
    //double tmp; // Unused
195 196

    /* Precalculate ep2 */
197
    ep2 = (pow(sm_a, 2.0) - pow(sm_b, 2.0)) / pow(sm_b, 2.0);
198 199

    /* Precalculate nu2 */
200
    nu2 = ep2 * pow(cos(phi), 2.0);
201 202

    /* Precalculate N */
203
    N = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nu2));
204 205

    /* Precalculate t */
206
    t = tan(phi);
207
    t2 = t * t;
208
    //tmp = (t2 * t2 * t2) - pow(t, 6.0); // Unused
209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231

    /* Precalculate l */
    l = lambda - lambda0;

    /* Precalculate coefficients for l**n in the equations below
       so a normal human being can read the expressions for easting
       and northing
       -- l**1 and l**2 have coefficients of 1.0 */
    l3coef = 1.0 - t2 + nu2;

    l4coef = 5.0 - t2 + 9 * nu2 + 4.0 * (nu2 * nu2);

    l5coef = 5.0 - 18.0 * t2 + (t2 * t2) + 14.0 * nu2
        - 58.0 * t2 * nu2;

    l6coef = 61.0 - 58.0 * t2 + (t2 * t2) + 270.0 * nu2
        - 330.0 * t2 * nu2;

    l7coef = 61.0 - 479.0 * t2 + 179.0 * (t2 * t2) - (t2 * t2 * t2);

    l8coef = 1385.0 - 3111.0 * t2 + 543.0 * (t2 * t2) - (t2 * t2 * t2);

    /* Calculate easting (x) */
232 233 234 235
    x = N * cos(phi) * l
        + (N / 6.0 * pow(cos(phi), 3.0) * l3coef * pow(l, 3.0))
        + (N / 120.0 * pow(cos(phi), 5.0) * l5coef * pow(l, 5.0))
        + (N / 5040.0 * pow(cos(phi), 7.0) * l7coef * pow(l, 7.0));
236 237 238

    /* Calculate northing (y) */
    y = ArcLengthOfMeridian (phi)
239 240 241 242
        + (t / 2.0 * N * pow(cos(phi), 2.0) * pow(l, 2.0))
        + (t / 24.0 * N * pow(cos(phi), 4.0) * l4coef * pow(l, 4.0))
        + (t / 720.0 * N * pow(cos(phi), 6.0) * l6coef * pow(l, 6.0))
        + (t / 40320.0 * N * pow(cos(phi), 8.0) * l8coef * pow(l, 8.0));
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275

    return;
}



// MapXYToLatLon
// Converts x and y coordinates in the Transverse Mercator projection to
// a latitude/longitude pair.  Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
//   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//   x - The easting of the point, in meters.
//   y - The northing of the point, in meters.
//   lambda0 - Longitude of the central meridian to be used, in radians.
//
// Outputs:
//   phi    - Latitude in radians.
//   lambda - Longitude in radians.
//
// Returns:
//   The function does not return a value.
//
// Remarks:
//   The local variables Nf, nuf2, tf, and tf2 serve the same purpose as
//   N, nu2, t, and t2 in MapLatLonToXY, but they are computed with respect
//   to the footpoint latitude phif.
//
//   x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and
//   to optimize computations.
276
void MapXYToLatLon (double x, double y, double lambda0, double& phi, double& lambda)
277
{
278 279 280
  double phif, Nf, Nfpow, nuf2, ep2, tf, tf2, tf4, cf;
  double x1frac, x2frac, x3frac, x4frac, x5frac, x6frac, x7frac, x8frac;
  double x2poly, x3poly, x4poly, x5poly, x6poly, x7poly, x8poly;
281 282 283 284 285
  
  /* Get the value of phif, the footpoint latitude. */
  phif = FootpointLatitude (y);
    
  /* Precalculate ep2 */
286 287
  ep2 = (pow(sm_a, 2.0) - pow(sm_b, 2.0))
      / pow(sm_b, 2.0);
288 289
    
  /* Precalculate cos (phif) */
290
  cf = cos(phif);
291 292
    
  /* Precalculate nuf2 */
293
  nuf2 = ep2 * pow(cf, 2.0);
294 295
    
  /* Precalculate Nf and initialize Nfpow */
296
  Nf = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nuf2));
297 298 299
  Nfpow = Nf;
    
  /* Precalculate tf */
300
  tf = tan(phif);
301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348
  tf2 = tf * tf;
  tf4 = tf2 * tf2;
  
  /* Precalculate fractional coefficients for x**n in the equations
     below to simplify the expressions for latitude and longitude. */
  x1frac = 1.0 / (Nfpow * cf);
  
  Nfpow *= Nf;   /* now equals Nf**2) */
  x2frac = tf / (2.0 * Nfpow);
  
  Nfpow *= Nf;   /* now equals Nf**3) */
  x3frac = 1.0 / (6.0 * Nfpow * cf);
  
  Nfpow *= Nf;   /* now equals Nf**4) */
  x4frac = tf / (24.0 * Nfpow);
  
  Nfpow *= Nf;   /* now equals Nf**5) */
  x5frac = 1.0 / (120.0 * Nfpow * cf);
  
  Nfpow *= Nf;   /* now equals Nf**6) */
  x6frac = tf / (720.0 * Nfpow);
  
  Nfpow *= Nf;   /* now equals Nf**7) */
  x7frac = 1.0 / (5040.0 * Nfpow * cf);
  
  Nfpow *= Nf;   /* now equals Nf**8) */
  x8frac = tf / (40320.0 * Nfpow);
  
  /* Precalculate polynomial coefficients for x**n.
     -- x**1 does not have a polynomial coefficient. */
  x2poly = -1.0 - nuf2;
  
  x3poly = -1.0 - 2 * tf2 - nuf2;
  
  x4poly = 5.0 + 3.0 * tf2 + 6.0 * nuf2 - 6.0 * tf2 * nuf2
         - 3.0 * (nuf2 *nuf2) - 9.0 * tf2 * (nuf2 * nuf2);
  
  x5poly = 5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * nuf2 + 8.0 * tf2 * nuf2;
  
  x6poly = -61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * nuf2
         + 162.0 * tf2 * nuf2;
  
  x7poly = -61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2);
  
  x8poly = 1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2);
    
  /* Calculate latitude */
  phi = phif + x2frac * x2poly * (x * x)
349 350 351
      + x4frac * x4poly * pow(x, 4.0)
      + x6frac * x6poly * pow(x, 6.0)
      + x8frac * x8poly * pow(x, 8.0);
352 353 354
    
  /* Calculate longitude */
  lambda = lambda0 + x1frac * x
355 356 357
         + x3frac * x3poly * pow(x, 3.0)
         + x5frac * x5poly * pow(x, 5.0)
         + x7frac * x7poly * pow(x, 7.0);
358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381
    
  return;
}




// LatLonToUTMXY
// Converts a latitude/longitude pair to x and y coordinates in the
// Universal Transverse Mercator projection.
//
// Inputs:
//   lat - Latitude of the point, in radians.
//   lon - Longitude of the point, in radians.
//   zone - UTM zone to be used for calculating values for x and y.
//          If zone is less than 1 or greater than 60, the routine
//          will determine the appropriate zone from the value of lon.
//
// Outputs:
//   x - The x coordinate (easting) of the computed point. (in meters)
//   y - The y coordinate (northing) of the computed point. (in meters)
//
// Returns:
//   The UTM zone used for calculating the values of x and y.
382
int LatLonToUTMXY (double lat, double lon, int zone, double& x, double& y) {
383
  if ( (zone < 1) || (zone > 60) )
384
    zone = floor((lon + 180.0) / 6) + 1;
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416
  
  MapLatLonToXY (DegToRad(lat), DegToRad(lon), UTMCentralMeridian(zone), x, y);
  
  /* Adjust easting and northing for UTM system. */
  x = x * UTMScaleFactor + 500000.0;
  y = y * UTMScaleFactor;
  if (y < 0.0)
    y = y + 10000000.0;
  
  return zone;
}



// UTMXYToLatLon
//
// Converts x and y coordinates in the Universal Transverse Mercator
// projection to a latitude/longitude pair.
//
// Inputs:
// x - The easting of the point, in meters.
// y - The northing of the point, in meters.
// zone - The UTM zone in which the point lies.
// southhemi - True if the point is in the southern hemisphere;
//               false otherwise.
//
// Outputs:
// lat - The latitude of the point, in radians.
// lon - The longitude of the point, in radians.
// 
// Returns:
// The function does not return a value.
417 418
void UTMXYToLatLon (double x, double y, int zone, bool southhemi, double& lat, double& lon) {
  double cmeridian;
419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434
    
  x -= 500000.0;
  x /= UTMScaleFactor;
    
  /* If in southern hemisphere, adjust y accordingly. */
  if (southhemi)
    y -= 10000000.0;
      
  y /= UTMScaleFactor;
  
  cmeridian = UTMCentralMeridian (zone);
  MapXYToLatLon (x, y, cmeridian, lat, lon);
    
  return;
}