Monday, September 15, 2008

WGS to UTM, UTM to WGS conversions, geo distance, azimuth and other geographical calculations in C#

"The reports of my death are greatly exaggerated"

Since my post about leaving consulting field, I got huge amount of email with questions about all community projects, I'm leading, blogging in general and specific to the future of this blog.

To make things clear, I leaved consulting, and now, I have less time and reasons to blog, however, I'm keep blogging and maintaining almost all of my community projects (see the left side of the main page to list of most of those projects). Also, I try to answer all questions, I got via emails, however it's too much to handle, thus be prepared for delays.

To be sure, I'm alive, you can follow me at twitter (it demands less time to write) :) Anyway, that you for reading my blog and supporting me

Now it's good time to write something useful for you :)

If you ever wrote GIS programs, you, probably, know, that every time we forget how to convert latlon (Latitude-Longitude or, simpler World Geographical System) coordinates into Universal Transverse Mercator coordinates and vise verse, how to calculate geographical distance from point to point, line or segment to point and line to line, how to calculate azimuth between two geo points, or how to calculate destination point, based on start coordinate, distance and bearing.

image

In order to make our life easier, I decided to post number of methods in C#, I always use to perform such GIS calculations. I believe, that those classes will help you to perform your own geographical calculations. Also, do not forget two handy functions to convert degrees to radians and vv

public static double DegToRad(double deg) { return (deg / 180.0 * Math.PI); }
public static double RadToDeg(double rad) { return (rad / Math.PI * 180.0); }

Also, there are some constants should be used, if you're calculating geo information in the Earth

const double sm_a = 6378137.0;
const double sm_b = 6356752.314;
const double sm_EccSquared = 6.69437999013e-03;
const double UTMScaleFactor = 0.9996;

Conversion WGS2UTM (LatLon2UTM)

Fist of all, we should calculate UTM zone. This one is simple

int zone = (int)(Math.Floor((latlon.Longitude + 180.0) / 6) + 1);

Now, when we have zone, we should calculate UTM central meridian, footprint of latitude and arc length of the meridian

public static double UTMCentralMeridian(int zone) { return DegToRad(-183.0 + (zone * 6.0)); }

public static double FootpointLatitude(double y) {
         /* Precalculate n (Eq. 10.18) */
         var n = (sm_a - sm_b) / (sm_a + sm_b);

         /* Precalculate alpha_ (Eq. 10.22) */
         /* (Same as alpha in Eq. 10.17) */
         var alpha_ = ((sm_a + sm_b) / 2.0) * (1 + (Math.Pow(n, 2.0) / 4) + (Math.Pow(n, 4.0) / 64));

         /* Precalculate y_ (Eq. 10.23) */
         var y_ = y / alpha_;

         /* Precalculate beta_ (Eq. 10.22) */
         var beta_ = (3.0 * n / 2.0) + (-27.0 * Math.Pow(n, 3.0) / 32.0) + (269.0 * Math.Pow(n, 5.0) / 512.0);

         /* Precalculate gamma_ (Eq. 10.22) */
         var gamma_ = (21.0 * Math.Pow(n, 2.0) / 16.0) + (-55.0 * Math.Pow(n, 4.0) / 32.0);

         /* Precalculate delta_ (Eq. 10.22) */
         var delta_ = (151.0 * Math.Pow(n, 3.0) / 96.0) + (-417.0 * Math.Pow(n, 5.0) / 128.0);

         /* Precalculate epsilon_ (Eq. 10.22) */
         var epsilon_ = (1097.0 * Math.Pow(n, 4.0) / 512.0);

         /* Now calculate the sum of the series (Eq. 10.21) */
         return y_ + (beta_ * Math.Sin(2.0 * y_)) + (gamma_ * Math.Sin(4.0 * y_)) + (delta_ * Math.Sin(6.0 * y_)) + (epsilon_ * Math.Sin(8.0 * y_));
      }

public static double ArcLengthOfMeridian(double phi) {
         /* Precalculate n */
         var n = (sm_a - sm_b) / (sm_a + sm_b);

         /* Precalculate alpha */
         var alpha = ((sm_a + sm_b) / 2.0) * (1.0 + (Math.Pow(n, 2.0) / 4.0) + (Math.Pow(n, 4.0) / 64.0));

         /* Precalculate beta */
         var beta = (-3.0 * n / 2.0) + (9.0 * Math.Pow(n, 3.0) / 16.0) + (-3.0 * Math.Pow(n, 5.0) / 32.0);

         /* Precalculate gamma */
         var gamma = (15.0 * Math.Pow(n, 2.0) / 16.0) + (-15.0 * Math.Pow(n, 4.0) / 32.0);

         /* Precalculate delta */
         var delta = (-35.0 * Math.Pow(n, 3.0) / 48.0) + (105.0 * Math.Pow(n, 5.0) / 256.0);

         /* Precalculate epsilon */
         var epsilon = (315.0 * Math.Pow(n, 4.0) / 512.0);

         /* Now calculate the sum of the series and return */
         return alpha * (phi + (beta * Math.Sin(2.0 * phi)) + (gamma * Math.Sin(4.0 * phi)) + (delta * Math.Sin(6.0 * phi)) + (epsilon * Math.Sin(8.0 * phi)));
      }

Now, we have everything to calculate UTM

public static GeoPoint MapLatLonToXY(double phi, double lambda, double lambda0) {
         /* Precalculate ep2 */
         var ep2 = (Math.Pow(sm_a, 2.0) - Math.Pow(sm_b, 2.0)) / Math.Pow(sm_b, 2.0);

         /* Precalculate nu2 */
         var nu2 = ep2 * Math.Pow(Math.Cos(phi), 2.0);

         /* Precalculate N */
         var N = Math.Pow(sm_a, 2.0) / (sm_b * Math.Sqrt(1 + nu2));

         /* Precalculate t */
         var t = Math.Tan(phi);
         var t2 = t * t;
         var tmp = (t2 * t2 * t2) - Math.Pow(t, 6.0);

         /* Precalculate l */
         var 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 */
         var l3coef = 1.0 - t2 + nu2;

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

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

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

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

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

         var xy = new GeoPoint();
         /* Calculate easting (x) */
         xy.X = N * Math.Cos(phi) * l + (N / 6.0 * Math.Pow(Math.Cos(phi), 3.0) * l3coef * Math.Pow(l, 3.0)) + (N / 120.0 * Math.Pow(Math.Cos(phi), 5.0) * l5coef * Math.Pow(l, 5.0)) + (N / 5040.0 * Math.Pow(Math.Cos(phi), 7.0) * l7coef * Math.Pow(l, 7.0));

         /* Calculate northing (y) */
         xy.Y = ArcLengthOfMeridian(phi) + (t / 2.0 * N * Math.Pow(Math.Cos(phi), 2.0) * Math.Pow(l, 2.0)) + (t / 24.0 * N * Math.Pow(Math.Cos(phi), 4.0) * l4coef * Math.Pow(l, 4.0)) + (t / 720.0 * N * Math.Pow(Math.Cos(phi), 6.0) * l6coef * Math.Pow(l, 6.0)) + (t / 40320.0 * N * Math.Pow(Math.Cos(phi), 8.0) * l8coef * Math.Pow(l, 8.0));

         return xy;
      }

public static GeoCoord MapXYToLatLon(double x, double y, double lambda0) {
   /* Get the value of phif, the footpoint latitude. */
   double phif = FootpointLatitude(y);

   /* Precalculate ep2 */
   double ep2 = (Math.Pow(sm_a, 2.0) - Math.Pow(sm_b, 2.0)) / Math.Pow(sm_b, 2.0);

   /* Precalculate cos (phif) */
   var cf = Math.Cos(phif);

   /* Precalculate nuf2 */
   var nuf2 = ep2 * Math.Pow(cf, 2.0);

   /* Precalculate Nf and initialize Nfpow */
   var Nf = Math.Pow(sm_a, 2.0) / (sm_b * Math.Sqrt(1 + nuf2));
   var Nfpow = Nf;

   /* Precalculate tf */
   var tf = Math.Tan(phif);
   var tf2 = tf * tf;
   var tf4 = tf2 * tf2;

   /* Precalculate fractional coefficients for x**n in the equations
      below to simplify the expressions for latitude and longitude. */
   var x1frac = 1.0 / (Nfpow * cf);

   Nfpow *= Nf;   /* now equals Nf**2) */
   var x2frac = tf / (2.0 * Nfpow);

   Nfpow *= Nf;   /* now equals Nf**3) */
   var x3frac = 1.0 / (6.0 * Nfpow * cf);

   Nfpow *= Nf;   /* now equals Nf**4) */
   var x4frac = tf / (24.0 * Nfpow);

   Nfpow *= Nf;   /* now equals Nf**5) */
   var x5frac = 1.0 / (120.0 * Nfpow * cf);

   Nfpow *= Nf;   /* now equals Nf**6) */
   var x6frac = tf / (720.0 * Nfpow);

   Nfpow *= Nf;   /* now equals Nf**7) */
   var x7frac = 1.0 / (5040.0 * Nfpow * cf);

   Nfpow *= Nf;   /* now equals Nf**8) */
   var x8frac = tf / (40320.0 * Nfpow);

   /* Precalculate polynomial coefficients for x**n.
      -- x**1 does not have a polynomial coefficient. */
   var x2poly = -1.0 - nuf2;

   var x3poly = -1.0 - 2 * tf2 - nuf2;

   var x4poly = 5.0 + 3.0 * tf2 + 6.0 * nuf2 - 6.0 * tf2 * nuf2 - 3.0 * (nuf2 * nuf2) - 9.0 * tf2 * (nuf2 * nuf2);

   var x5poly = 5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * nuf2 + 8.0 * tf2 * nuf2;

   var x6poly = -61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * nuf2 + 162.0 * tf2 * nuf2;

   var x7poly = -61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2);

   var x8poly = 1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2);

   var philambda = new GeoCoord();
   /* Calculate latitude */
   philambda.Latitude = phif + x2frac * x2poly * (x * x) + x4frac * x4poly * Math.Pow(x, 4.0) + x6frac * x6poly * Math.Pow(x, 6.0) + x8frac * x8poly * Math.Pow(x, 8.0);

   /* Calculate longitude */
   philambda.Longitude = lambda0 + x1frac * x + x3frac * x3poly * Math.Pow(x, 3.0) + x5frac * x5poly * Math.Pow(x, 5.0) + x7frac * x7poly * Math.Pow(x, 7.0);

   return philambda;
}

We done, the only thing, should be adjusted is easting and northing for UTM system

xy.X = xy.X * UTMScaleFactor + 500000.0;
xy.Y = xy.Y * UTMScaleFactor;
if (xy.Y < 0.0) xy.Y += 10000000.0;

Conversion UTM2WGS (UTM2LatLon)

After al had all thin math in previous chapter, now we should calculate opposite conversion

First adjust

x -= 500000.0;
x /= UTMScaleFactor;

/* If in southern hemisphere, adjust y accordingly. */
if (southhemi) y -= 10000000.0;

y /= UTMScaleFactor;

var cmeridian = UTMCentralMeridian(zone);

Now calculate

public static GeoCoord MapXYToLatLon(double x, double y, double lambda0) {
         /* Get the value of phif, the footpoint latitude. */
         double phif = FootpointLatitude(y);

         /* Precalculate ep2 */
         double ep2 = (Math.Pow(sm_a, 2.0) - Math.Pow(sm_b, 2.0)) / Math.Pow(sm_b, 2.0);

         /* Precalculate cos (phif) */
         var cf = Math.Cos(phif);

         /* Precalculate nuf2 */
         var nuf2 = ep2 * Math.Pow(cf, 2.0);

         /* Precalculate Nf and initialize Nfpow */
         var Nf = Math.Pow(sm_a, 2.0) / (sm_b * Math.Sqrt(1 + nuf2));
         var Nfpow = Nf;

         /* Precalculate tf */
         var tf = Math.Tan(phif);
         var tf2 = tf * tf;
         var tf4 = tf2 * tf2;

         /* Precalculate fractional coefficients for x**n in the equations
            below to simplify the expressions for latitude and longitude. */
         var x1frac = 1.0 / (Nfpow * cf);

         Nfpow *= Nf;   /* now equals Nf**2) */
         var x2frac = tf / (2.0 * Nfpow);

         Nfpow *= Nf;   /* now equals Nf**3) */
         var x3frac = 1.0 / (6.0 * Nfpow * cf);

         Nfpow *= Nf;   /* now equals Nf**4) */
         var x4frac = tf / (24.0 * Nfpow);

         Nfpow *= Nf;   /* now equals Nf**5) */
         var x5frac = 1.0 / (120.0 * Nfpow * cf);

         Nfpow *= Nf;   /* now equals Nf**6) */
         var x6frac = tf / (720.0 * Nfpow);

         Nfpow *= Nf;   /* now equals Nf**7) */
         var x7frac = 1.0 / (5040.0 * Nfpow * cf);

         Nfpow *= Nf;   /* now equals Nf**8) */
         var x8frac = tf / (40320.0 * Nfpow);

         /* Precalculate polynomial coefficients for x**n.
            -- x**1 does not have a polynomial coefficient. */
         var x2poly = -1.0 - nuf2;

         var x3poly = -1.0 - 2 * tf2 - nuf2;

         var x4poly = 5.0 + 3.0 * tf2 + 6.0 * nuf2 - 6.0 * tf2 * nuf2 - 3.0 * (nuf2 * nuf2) - 9.0 * tf2 * (nuf2 * nuf2);

         var x5poly = 5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * nuf2 + 8.0 * tf2 * nuf2;

         var x6poly = -61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * nuf2 + 162.0 * tf2 * nuf2;

         var x7poly = -61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2);

         var x8poly = 1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2);

         var philambda = new GeoCoord();
         /* Calculate latitude */
         philambda.Latitude = phif + x2frac * x2poly * (x * x) + x4frac * x4poly * Math.Pow(x, 4.0) + x6frac * x6poly * Math.Pow(x, 6.0) + x8frac * x8poly * Math.Pow(x, 8.0);

         /* Calculate longitude */
         philambda.Longitude = lambda0 + x1frac * x + x3frac * x3poly * Math.Pow(x, 3.0) + x5frac * x5poly * Math.Pow(x, 5.0) + x7frac * x7poly * Math.Pow(x, 7.0);

         return philambda;
      }

At the end do not forget to return Latitude and Longitude in degrees, rather, then in radians

Azimuth calculation

We done with coordinates, now azimuth

public static double GetAzimuth(WGSCoord c1, WGSCoord c2) {
         var lat1 = DegToRad(c1.Latitude);
         var lon1 = DegToRad(c1.Longitude);
         var lat2 = DegToRad(c2.Latitude);
         var lon2 = DegToRad(c2.Longitude);

         return RadToDeg(Math.Asin(Math.Sin(lon1 - lon2) * Math.Cos(lat2) / Math.Sin(Math.Acos(Math.Sin(lat2) * Math.Sin(lat1) + Math.Cos(lat1) * Math.Cos(lat2) * Math.Cos(lon2 - lon1)))));
      }

Distance calculations

public static double GetDistance(WGSCoord c1, WGSCoord c2) {
         var dLat = DegToRad(c2.Latitude - c1.Latitude);
         var dLon = DegToRad(c2.Longitude - c2.Longitude);
         var a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Cos(DegToRad(c1.Latitude)) * Math.Cos(DegToRad(c2.Latitude)) * Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
         var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
         return sm_a * c;
      }

Final destination coordinates, based on start coordinate, bearing and distance

public static WGSCoord GetDestination(WGSCoord start, double distance, double azimuth) {
         var lat = RadToDeg(Math.Asin(Math.Sin(DegToRad(start.Latitude)) * Math.Cos(DegToRad(distance / sm_a)) + Math.Cos(DegToRad(start.Latitude)) * Math.Sin(DegToRad(distance / sm_a)) * Math.Cos(DegToRad(azimuth))));
         var lon = start.Longitude + DegToRad(Math.Atan2(Math.Sin(DegToRad(azimuth)) * Math.Sin(DegToRad(distance / sm_a)) * Math.Cos(DegToRad(start.Latitude)), Math.Cos(DegToRad(distance / sm_a)) - Math.Sin(DegToRad(start.Latitude)) * Math.Sin(DegToRad(lat))));
         return new WGSCoord(lon,lat);

      }

Midpoint calculation

public static WGSCoord GetMidpoint(WGSCoord start, WGSCoord end) {
         var dLat = DegToRad(end.Latitude - start.Latitude);
         var dLon = DegToRad(end.Longitude - start.Longitude);
         var lat2 = DegToRad(end.Latitude);
         var lat1 = DegToRad(start.Latitude);
         var lon1 = DegToRad(start.Longitude);

         var Bx = Math.Cos(lat2) * Math.Cos(dLon);
         var By = Math.Cos(lat2) * Math.Sin(dLon);
         var lat3 = Math.Atan2(Math.Sin(lat1) + Math.Sin(lat2), Math.Sqrt((Math.Cos(lat1) + Bx) * (Math.Cos(lat1) + Bx) + By * By));
         var lon3 = lon1 + Math.Atan2(By, Math.Cos(lat1) + Bx);
         return new WGSCoord(RadToDeg(lon3), RadToDeg(lat3));
      }

Rhumb lines calculation

First of all, what is rhumb lines? Rhumb lines or loxodrome is a path of constant bearing, which crosses all meridians at the same angle. This calculation is very useful, if you want to follow constant compass bearing, instead of continually adjustment of it.

public static double GetRhumbDistance(WGSCoord start, WGSCoord end) {
   var dLat = DegToRad(end.Latitude - start.Latitude);
   var dLon = DegToRad(end.Longitude - start.Longitude);
   var lat1 = DegToRad(start.Latitude);
   var lon1 = DegToRad(start.Longitude);
   var lat2 = DegToRad(end.Latitude);
   var lon2 = DegToRad(end.Longitude);

   var dPhi = Math.Log(Math.Tan(lat2 / 2 + Math.PI / 4) / Math.Tan(lat1 / 2 + Math.PI / 4));
   var q = (Math.Abs(dLat) > 1e-10) ? dLat / dPhi : Math.Cos(lat1);
   if (Math.Abs(dLon) > Math.PI) {
      dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
   }
   return Math.Sqrt(dLat * dLat + q * q * dLon * dLon) * sm_a;
}

public static double GetRhumbBearing(WGSCoord start, WGSCoord end) {
   var dLat = DegToRad(end.Latitude - start.Latitude);
   var dLon = DegToRad(end.Longitude - start.Longitude);

   var lat1 = DegToRad(start.Latitude);
   var lon1 = DegToRad(start.Longitude);
   var lat2 = DegToRad(end.Latitude);
   var lon2 = DegToRad(end.Longitude);

   var dPhi = Math.Log(Math.Tan(lat2 / 2 + Math.PI / 4) / Math.Tan(lat1 / 2 + Math.PI / 4));
   if (Math.Abs(dLon) > Math.PI) {
      dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
   }
   return Math.Atan2(dLon, dPhi);
}

public static WGSCoord GetRhumbDestination(WGSCoord start, WGSCoord end) {
         var lat1 = DegToRad(start.Latitude);
         var lon1 = DegToRad(start.Longitude);
         var lat2 = DegToRad(end.Latitude);
         var lon2 = DegToRad(end.Longitude);

         var d = GetRhumbDistance(start, end);
         var b = GetRhumbBearing(start, end);
         lat2 = lat1 +  d * Math.Cos(b);
         var dPhi = Math.Log(Math.Tan(lat2 / 2 + Math.PI / 4) / Math.Tan(lat1 / 2 + Math.PI / 4));
         var q = (Math.Abs(lat2 - lat1) > 1e-10) ? (lat2 - lat1) / dPhi : Math.Cos(lat1);
         var dLon = d * Math.Sin(b) / q;
         if (Math.Abs(lat2) > Math.PI / 2) lat2 = lat2 > 0 ? Math.PI - lat2 : -Math.PI - lat2;
         lon2 = (lon1 + dLon + Math.PI) % (2 * Math.PI) - Math.PI;

         return new WGSCoord(RadToDeg(lon2), RadToDeg(lat2));
      }

Conversion between decimal degrees to degrees-minutes-seconds

public static Pair<double,double> ToD_Mm(this double decCoord) {
         var degSeg = (int)decCoord;
         return new Pair<double, double>(degSeg, decCoord % degSeg * 60);
      }

Now it's good time for small math (if there were not enough until now) :) All following methods are not geo spatial oriented. All those are only geometry. The only helper method, you need here is Dot vector operation

private static double dot(Vector v1, Vector v2) { return (v1.X * v2.X + v1.Y * v2.Y); }

Get the distance between two points

private static double distance(Point p1, Point p2) {
         var a = Math.Pow(p1.X, 2) - Math.Pow(p2.X, 2);
         var b = Math.Pow(p1.Y, 2) - Math.Pow(p2.Y, 2);
         return Math.Sqrt(Math.Abs(a + b));
      }

Get the shortest distance between point and line segment

private static double distance(Point p1, Point p2, Point p) {
         var v = p1 - p2;
         var w = p - p2;

         var c1 = dot(w, v);
         if (c1 <= 0) return distance(p, p1);

         var c2 = dot(v, v);
         if (c2 <= c1) return distance(p, p2);

         var b = c1 / c2;
         Point Pb = p1 + b * v;
         return distance(p, Pb);
      }

Isn't it enough math for now? It is. Have a nice day and be good people. Next time, we'll speak about different path finding algorithms, edges, nodes and other fun math optimizations.

2 comments:

chadbr said...

very very nice -- thanks!

Anonymous said...

That's easy. Just plug in all these variables into the formula found at http://jslim28.blogspot.com/2011/10/how-to-calculate-distance-between-two.html