src/UTM.cpp

@@ -15,18 +15,31 @@
 // 
 // 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.
-FLOAT DegToRad(FLOAT deg) {
+double DegToRad(double deg) {
   return (deg / 180.0 * pi);
 }
 
 
 // RadToDeg
 // Converts radians to degrees.
-FLOAT RadToDeg(FLOAT rad) {
+double RadToDeg(double rad) {
   return (rad / pi * 180.0);
 }
 
@@ -46,38 +59,38 @@ FLOAT RadToDeg(FLOAT rad) {
 // 
 // Returns:
 //     The ellipsoidal distance of the point from the equator, in meters.
-FLOAT ArcLengthOfMeridian (FLOAT phi) {
-  FLOAT alpha, beta, gamma, delta, epsilon, n;
-  FLOAT result;
+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));
+        * (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);
+  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);
+  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);
+  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);
+  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)));
+         * (phi + (beta * sin(2.0 * phi))
+         + (gamma * sin(4.0 * phi))
+         + (delta * sin(6.0 * phi))
+         + (epsilon * sin(8.0 * phi)));
   
   return result;
 }
@@ -93,9 +106,9 @@ FLOAT ArcLengthOfMeridian (FLOAT phi) {
 // Returns:
 //   The central meridian for the given UTM zone, in radians
 //   Range of the central meridian is the radian equivalent of [-177,+177].
-FLOAT UTMCentralMeridian(int zone) {
-  FLOAT cmeridian;
-  cmeridian = DegToRad(-183.0 + ((FLOAT)zone * 6.0));
+double UTMCentralMeridian(int zone) {
+  double cmeridian;
+  cmeridian = DegToRad(-183.0 + ((double)zone * 6.0));
   
   return cmeridian;
 }
@@ -115,9 +128,9 @@ FLOAT UTMCentralMeridian(int zone) {
 //
 // Returns:
 //   The footpoint latitude, in radians.
-FLOAT FootpointLatitude(FLOAT y) {
-  FLOAT y_, alpha_, beta_, gamma_, delta_, epsilon_, n;
-  FLOAT result;
+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);
@@ -125,31 +138,31 @@ FLOAT FootpointLatitude(FLOAT y) {
   /* 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));
+         * (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);
+  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);
+  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);
+  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);
+  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_));
+  result = y_ + (beta_ * sin(2.0 * y_))
+         + (gamma_ * sin(4.0 * y_))
+         + (delta_ * sin(6.0 * y_))
+         + (epsilon_ * sin(8.0 * y_));
   
   return result;
 }
@@ -175,24 +188,24 @@ FLOAT FootpointLatitude(FLOAT y) {
 //
 // Returns:
 //    The function does not return a value.
-void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y) {
-    FLOAT N, nu2, ep2, t, t2, l;
-    FLOAT l3coef, l4coef, l5coef, l6coef, l7coef, l8coef;
-    //FLOAT tmp; // Unused
+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);
+    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);
+    nu2 = ep2 * pow(cos(phi), 2.0);
 
     /* Precalculate N */
-    N = POW(sm_a, 2.0) / (sm_b * SQRT(1 + nu2));
+    N = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nu2));
 
     /* Precalculate t */
-    t = TAN(phi);
+    t = tan(phi);
     t2 = t * t;
-    //tmp = (t2 * t2 * t2) - POW(t, 6.0); // Unused
+    //tmp = (t2 * t2 * t2) - pow(t, 6.0); // Unused
 
     /* Precalculate l */
     l = lambda - lambda0;
@@ -216,17 +229,17 @@ void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y)
     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));
+    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));
+        + (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;
 }
@@ -260,31 +273,31 @@ void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y)
 //
 //   x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and
 //   to optimize computations.
-void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda)
+void MapXYToLatLon (double x, double y, double lambda0, double& phi, double& lambda)
 {
-  FLOAT phif, Nf, Nfpow, nuf2, ep2, tf, tf2, tf4, cf;
-  FLOAT x1frac, x2frac, x3frac, x4frac, x5frac, x6frac, x7frac, x8frac;
-  FLOAT x2poly, x3poly, x4poly, x5poly, x6poly, x7poly, x8poly;
+  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);
+  ep2 = (pow(sm_a, 2.0) - pow(sm_b, 2.0))
+      / pow(sm_b, 2.0);
     
   /* Precalculate cos (phif) */
-  cf = COS(phif);
+  cf = cos(phif);
     
   /* Precalculate nuf2 */
-  nuf2 = ep2 * POW(cf, 2.0);
+  nuf2 = ep2 * pow(cf, 2.0);
     
   /* Precalculate Nf and initialize Nfpow */
-  Nf = POW(sm_a, 2.0) / (sm_b * SQRT(1 + nuf2));
+  Nf = pow(sm_a, 2.0) / (sm_b * sqrt(1 + nuf2));
   Nfpow = Nf;
     
   /* Precalculate tf */
-  tf = TAN(phif);
+  tf = tan(phif);
   tf2 = tf * tf;
   tf4 = tf2 * tf2;
   
@@ -333,15 +346,15 @@ void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda)
     
   /* 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);
+      + 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);
+         + x3frac * x3poly * pow(x, 3.0)
+         + x5frac * x5poly * pow(x, 5.0)
+         + x7frac * x7poly * pow(x, 7.0);
     
   return;
 }
@@ -366,9 +379,9 @@ void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda)
 //
 // Returns:
 //   The UTM zone used for calculating the values of x and y.
-int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& 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;
+    zone = floor((lon + 180.0) / 6) + 1;
   
   MapLatLonToXY (DegToRad(lat), DegToRad(lon), UTMCentralMeridian(zone), x, y);
   
@@ -401,8 +414,8 @@ int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& y) {
 // 
 // Returns:
 // The function does not return a value.
-void UTMXYToLatLon (FLOAT x, FLOAT y, int zone, bool southhemi, FLOAT& lat, FLOAT& lon) {
-  FLOAT cmeridian;
+void UTMXYToLatLon (double x, double y, int zone, bool southhemi, double& lat, double& lon) {
+  double cmeridian;
     
   x -= 500000.0;
   x /= UTMScaleFactor;

src/UTM.h

@@ -3,9 +3,6 @@
 // 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 this file: undefine FLOAT_32 and define FLOAT_64 (see below)
-// 
 // 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).
@@ -15,58 +12,18 @@
 // 
 // 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
+
 #ifndef UTM_H
 #define UTM_H
 
-// Choose floating point precision:
-
-// 32-bit (for Teensy 3.5/3.6 ARM boards, etc.)
-#define FLOAT_64
-
-// 64-bit (for desktop/server use)
-//#define FLOAT_64
-
-#ifdef FLOAT_64
-#define FLOAT double
-#define SIN sin
-#define COS cos
-#define TAN tan
-#define POW pow
-#define SQRT sqrt
-#define FLOOR floor
-
-#else
-#ifdef FLOAT_32
-#define FLOAT float
-#define SIN sinf
-#define COS cosf
-#define TAN tanf
-#define POW powf
-#define SQRT sqrtf
-#define FLOOR floorf
-
-#endif
-#endif
-
-
-#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.
-FLOAT DegToRad(FLOAT deg);
+double DegToRad(double deg);
 
 // RadToDeg
 // Converts radians to degrees.
-FLOAT RadToDeg(FLOAT rad);
+double RadToDeg(double rad);
 
 // ArcLengthOfMeridian
 // Computes the ellipsoidal distance from the equator to a point at a
@@ -84,7 +41,7 @@ FLOAT RadToDeg(FLOAT rad);
 // 
 // Returns:
 //     The ellipsoidal distance of the point from the equator, in meters.
-FLOAT ArcLengthOfMeridian (FLOAT phi);
+double ArcLengthOfMeridian (double phi);
 
 // UTMCentralMeridian
 // Determines the central meridian for the given UTM zone.
@@ -95,7 +52,7 @@ FLOAT ArcLengthOfMeridian (FLOAT phi);
 // Returns:
 //   The central meridian for the given UTM zone, in radians
 //   Range of the central meridian is the radian equivalent of [-177,+177].
-FLOAT UTMCentralMeridian(int zone);
+double UTMCentralMeridian(int zone);
 
 // FootpointLatitude
 //
@@ -110,7 +67,7 @@ FLOAT UTMCentralMeridian(int zone);
 //
 // Returns:
 //   The footpoint latitude, in radians.
-FLOAT FootpointLatitude(FLOAT y);
+double FootpointLatitude(double y);
 
 // MapLatLonToXY
 // Converts a latitude/longitude pair to x and y coordinates in the
@@ -131,7 +88,7 @@ FLOAT FootpointLatitude(FLOAT y);
 //
 // Returns:
 //    The function does not return a value.
-void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y);
+void MapLatLonToXY (double phi, double lambda, double lambda0, double &x, double &y);
 
 // MapXYToLatLon
 // Converts x and y coordinates in the Transverse Mercator projection to
@@ -160,7 +117,7 @@ void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y);
 //
 //   x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and
 //   to optimize computations.
-void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda);
+void MapXYToLatLon (double x, double y, double lambda0, double& phi, double& lambda);
 
 // LatLonToUTMXY
 // Converts a latitude/longitude pair to x and y coordinates in the
@@ -179,7 +136,7 @@ void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda);
 //
 // Returns:
 //   The UTM zone used for calculating the values of x and y.
-int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& y);
+int LatLonToUTMXY (double lat, double lon, int zone, double& x, double& y);
 
 // UTMXYToLatLon
 //
@@ -200,7 +157,7 @@ int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& y);
 // 
 // Returns:
 // The function does not return a value.
-void UTMXYToLatLon (FLOAT x, FLOAT y, int zone, bool southhemi, FLOAT& lat, FLOAT& lon);
+void UTMXYToLatLon (double x, double y, int zone, bool southhemi, double& lat, double& lon);
 
 #endif