You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
434 lines
13 KiB
434 lines
13 KiB
// 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 |
|
|
|
// QGC Note: This file has been slightly modified to prevent possible conflicts with other parts of the system |
|
|
|
#include "UTM.h" |
|
|
|
#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 |
|
|
|
// DegToRad |
|
// Converts degrees to radians. |
|
double DegToRad(double deg) { |
|
return (deg / 180.0 * pi); |
|
} |
|
|
|
|
|
// RadToDeg |
|
// Converts radians to degrees. |
|
double RadToDeg(double rad) { |
|
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. |
|
double ArcLengthOfMeridian (double phi) { |
|
double alpha, beta, gamma, delta, epsilon, n; |
|
double result; |
|
|
|
/* Precalculate n */ |
|
n = (sm_a - sm_b) / (sm_a + sm_b); |
|
|
|
/* Precalculate alpha */ |
|
alpha = ((sm_a + sm_b) / 2.0) |
|
* (1.0 + (pow(n, 2.0) / 4.0) + (pow(n, 4.0) / 64.0)); |
|
|
|
/* Precalculate beta */ |
|
beta = (-3.0 * n / 2.0) + (9.0 * pow(n, 3.0) / 16.0) |
|
+ (-3.0 * pow(n, 5.0) / 32.0); |
|
|
|
/* Precalculate gamma */ |
|
gamma = (15.0 * pow(n, 2.0) / 16.0) |
|
+ (-15.0 * pow(n, 4.0) / 32.0); |
|
|
|
/* Precalculate delta */ |
|
delta = (-35.0 * pow(n, 3.0) / 48.0) |
|
+ (105.0 * pow(n, 5.0) / 256.0); |
|
|
|
/* Precalculate epsilon */ |
|
epsilon = (315.0 * pow(n, 4.0) / 512.0); |
|
|
|
/* Now calculate the sum of the series and return */ |
|
result = alpha |
|
* (phi + (beta * sin(2.0 * phi)) |
|
+ (gamma * sin(4.0 * phi)) |
|
+ (delta * sin(6.0 * phi)) |
|
+ (epsilon * sin(8.0 * phi))); |
|
|
|
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]. |
|
double UTMCentralMeridian(int zone) { |
|
double cmeridian; |
|
cmeridian = DegToRad(-183.0 + ((double)zone * 6.0)); |
|
|
|
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. |
|
double FootpointLatitude(double y) { |
|
double y_, alpha_, beta_, gamma_, delta_, epsilon_, n; |
|
double result; |
|
|
|
/* 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) |
|
* (1 + (pow(n, 2.0) / 4) + (pow(n, 4.0) / 64)); |
|
|
|
/* Precalculate y_ (Eq. 10.23) */ |
|
y_ = y / alpha_; |
|
|
|
/* Precalculate beta_ (Eq. 10.22) */ |
|
beta_ = (3.0 * n / 2.0) + (-27.0 * pow(n, 3.0) / 32.0) |
|
+ (269.0 * pow(n, 5.0) / 512.0); |
|
|
|
/* Precalculate gamma_ (Eq. 10.22) */ |
|
gamma_ = (21.0 * pow(n, 2.0) / 16.0) |
|
+ (-55.0 * pow(n, 4.0) / 32.0); |
|
|
|
/* Precalculate delta_ (Eq. 10.22) */ |
|
delta_ = (151.0 * pow(n, 3.0) / 96.0) |
|
+ (-417.0 * pow(n, 5.0) / 128.0); |
|
|
|
/* Precalculate epsilon_ (Eq. 10.22) */ |
|
epsilon_ = (1097.0 * pow(n, 4.0) / 512.0); |
|
|
|
/* Now calculate the sum of the series (Eq. 10.21) */ |
|
result = y_ + (beta_ * sin(2.0 * y_)) |
|
+ (gamma_ * sin(4.0 * y_)) |
|
+ (delta_ * sin(6.0 * y_)) |
|
+ (epsilon_ * sin(8.0 * y_)); |
|
|
|
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. |
|
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 |
|
|
|
/* Precalculate ep2 */ |
|
ep2 = (pow(sm_a, 2.0) - pow(sm_b, 2.0)) / pow(sm_b, 2.0); |
|
|
|
/* Precalculate nu2 */ |
|
nu2 = ep2 * pow(cos(phi), 2.0); |
|
|
|
/* Precalculate N */ |
|
N = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nu2)); |
|
|
|
/* Precalculate t */ |
|
t = tan(phi); |
|
t2 = t * t; |
|
//tmp = (t2 * t2 * t2) - pow(t, 6.0); // Unused |
|
|
|
/* 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) */ |
|
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)); |
|
|
|
/* Calculate northing (y) */ |
|
y = ArcLengthOfMeridian (phi) |
|
+ (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)); |
|
|
|
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. |
|
void MapXYToLatLon (double x, double y, double lambda0, double& phi, double& lambda) |
|
{ |
|
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; |
|
|
|
/* Get the value of phif, the footpoint latitude. */ |
|
phif = FootpointLatitude (y); |
|
|
|
/* Precalculate ep2 */ |
|
ep2 = (pow(sm_a, 2.0) - pow(sm_b, 2.0)) |
|
/ pow(sm_b, 2.0); |
|
|
|
/* Precalculate cos (phif) */ |
|
cf = cos(phif); |
|
|
|
/* Precalculate nuf2 */ |
|
nuf2 = ep2 * pow(cf, 2.0); |
|
|
|
/* Precalculate Nf and initialize Nfpow */ |
|
Nf = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nuf2)); |
|
Nfpow = Nf; |
|
|
|
/* Precalculate tf */ |
|
tf = tan(phif); |
|
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) |
|
+ x4frac * x4poly * pow(x, 4.0) |
|
+ x6frac * x6poly * pow(x, 6.0) |
|
+ x8frac * x8poly * pow(x, 8.0); |
|
|
|
/* Calculate longitude */ |
|
lambda = lambda0 + x1frac * x |
|
+ x3frac * x3poly * pow(x, 3.0) |
|
+ x5frac * x5poly * pow(x, 5.0) |
|
+ x7frac * x7poly * pow(x, 7.0); |
|
|
|
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. |
|
int LatLonToUTMXY (double lat, double lon, int zone, double& x, double& y) { |
|
if ( (zone < 1) || (zone > 60) ) |
|
zone = floor((lon + 180.0) / 6) + 1; |
|
|
|
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. |
|
void UTMXYToLatLon (double x, double y, int zone, bool southhemi, double& lat, double& lon) { |
|
double cmeridian; |
|
|
|
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; |
|
} |
|
|
|
|