C#, GPS Receivers, and Geocaching: Vincenty’s Formula

Save/Share Google Yahoo! Digg It Reddit del.icio.us
My Zimbio

Vincenty’s Formula is an iterative solution for calculating the distance and direction between two points along the surface of Earth. For clarity, I’ve stripped out portions of the code I’ve put up for discussion, but you can download the entire C# source code from here. If you prefer Java, please see the Java version of this discussion.

Several years ago, I stumbled on a great pastime called “geocaching.” It’s a worldwide treasure hunting game where participants use handheld GPS receivers to find hidden “caches” – small boxes filled with prizes, trinkets, and “travel bugs“. The caches are hidden by other participants who post nothing more than the latitude and longitude on a website like Geocaching.com. My children and I have had a blast. It’s a great way for a grown man to justify playing in the woods (and buying an expensive gadget!) under the pretense of “playing with the kids.” With over 420,000 caches in 222 countries on all continents (including Antarctica!) there are bound to be several near you.

Want to know more? Check out this video of my son on a geocache hunt.

So, being the true geek I am, I coded up a bunch of applications to record where we’ve been, where we’d like to go, and interesting waypoints along the way. Often, I’d ask the question “How far is it for here to there?”

The Geodetic Inverse Problem

“How far?” turns out to be a fairly complicated question. For centuries, humankind has known Earth is round – but everything else about the shape of Earth is less understood. In fact, an entire field called “geodesy“, or “geodetics,” focuses on understanding the shape of Earth.

If we assume Earth is a perfect sphere, the math is easy. Given the size of Earth and the latitude and longitude of two points, we can quickly calculate the “great circle” distance and direction from one point to the next.

But, Earth is not a perfect sphere. It’s an irregular shape geodesists call the “geoid”. Because precise measurements of the geoid and the corresponding distance calculations would be impractical, the geoid is often modeled as an ellipsoid – an object resembling a sphere but slightly flattened at the poles. It’s still an inexact model, but it performs far better than the spherical model.

That’s where the math gets hard. The Geodetic Inverse Problem – finding the distance and direction from one point to another along an ellipsoid – does not have a closed form solution. However, in 1975, Thaddeus Vincenty developed an extremely accurate iterative solution for which he won the Department of Commerce Medal for Meritorious Service. If you want to sprain your brain, you can read the full publication of his solution.

Or, you can just download my C# implementation of Vincenty’s Formula.

Building the code

I’ll start with a convenient type for encapsulating angles. We’ll need this for latitudes and longitudes. It might seem to be overkill since we could simply uses double instead of a custom Angle type, but humans think in “degrees” while computers do math in “radians”. Rather than risk confusion and sprinkle conversion code throughout, I thought this would be easier:

namespace Gavaghan.Geodesy

{

public struct Angle : IComparable<Angle>

{

private const double PiOver180 = Math.PI / 180.0;

private double mDegrees;

static public readonly Angle Zero = new Angle(0);

static public readonly Angle Angle180 = new Angle(180);

public Angle(double degrees)

{

mDegrees = degrees;

}

public double Degrees

{

get { return mDegrees; }

set { mDegrees = value; }

}

public double Radians

{

get { return mDegrees * PiOver180; }

set { mDegrees = value / PiOver180; }

}

//

// Equals, IComparable<T> implementation and

// comparison operators omitted for clarity.

// See full source code download

//

}

}

Some critical portions of the Angle type above are omitted for clarity – like all of the comparison operators. You can check out the complete implementation in the source code download.

Next, we have a class that encapsulates a location on an ellipsoid.

namespace Gavaghan.Geodesy

{

public struct GlobalCoordinates : IComparable<GlobalCoordinates>

{

private Angle mLatitude;

private Angle mLongitude;

public GlobalCoordinates(Angle latitude, Angle longitude)

{

mLatitude = latitude;

mLongitude = longitude;

}

public Angle Latitude

{

get { return mLatitude; }

set { mLatitude = value; }

}

public Angle Longitude

{

get { return mLongitude; }

set { mLongitude = value; }

}

}

}

Negative latitudes are in the southern hemisphere, and negative longitudes are in the western hemisphere. Once again, the IComparable<T> implementation is omitted for clarity.

Now, we start getting to the fun part. The next type encapsulates the “geodetic curve.”

namespace Gavaghan.Geodesy

{

public struct GeodeticCurve

{

private readonly double mEllipsoidalDistance;

private readonly Angle mAzimuth;

private readonly Angle mReverseAzimuth;

public GeodeticCurve(double ellipsoidalDistance,

Angle azimuth, Angle reverseAzimuth)

{

mEllipsoidalDistance = ellipsoidalDistance;

mAzimuth = azimuth;

mReverseAzimuth = reverseAzimuth;

}

public double EllipsoidalDistance

{

get { return mEllipsoidalDistance; }

}

public Angle Azimuth

{

get { return mAzimuth; }

}

public Angle ReverseAzimuth

{

get { return mReverseAzimuth; }

}

}

}

A “geodetic curve” is the solution we’re looking for. It describes how to get from one point on an ellipsoid to another. The ellipsoidal distance is the distance, in meters, between the two points along the surface of the ellipsoid. The azimuth is the direction of travel from the starting point to the ending point. The reverse azimuth, of course, is the direction back from the endpoint (which isn’t necessarily a 180 degree turn on an ellipsoid).

The final input we need to Vincenty’s Formula is an ellipsoid. We hold onto four parameters of an ellipsoid: the length of each semi-axis (in meters), the flattening ratio, and the inverse of the flattening ratio. Technically, we only need the length of one semi-axis and any one of the other three parameters. We’ll record all four for convenience.

namespace Gavaghan.Geodesy

{

public struct Ellipsoid

{

private readonly double mSemiMajorAxis;

private readonly double mSemiMinorAxis;

private readonly double mFlattening;

private readonly double mInverseFlattening;

private Ellipsoid(double semiMajor, double semiMinor,

double flattening, double inverseFlattening)

{

mSemiMajorAxis = semiMajor;

mSemiMinorAxis = semiMinor;

mFlattening = flattening;

mInverseFlattening = inverseFlattening;

}

static public readonly Ellipsoid WGS84

= FromAAndInverseF(6378137.0, 298.257223563);

static public readonly Ellipsoid GRS80

= FromAAndInverseF(6378137.0, 298.257222101);

static public readonly Ellipsoid GRS67

= FromAAndInverseF(6378160.0, 298.25);

static public readonly Ellipsoid ANS

= FromAAndInverseF(6378160.0, 298.25);

static public readonly Ellipsoid Clarke1880

= FromAAndInverseF(6378249.145, 293.465);

static public Ellipsoid FromAAndInverseF(double semiMajor,

double inverseFlattening)

{

double f = 1.0 / inverseFlattening;

double b = (1.0 – f) * semiMajor;

return new Ellipsoid(semiMajor, b, f, inverseFlattening);

}

public double SemiMajorAxis

{

get { return mSemiMajorAxis; }

}

public double SemiMinorAxis

{

get { return mSemiMinorAxis; }

}

public double Flattening

{

get { return mFlattening; }

}

public double InverseFlattening

{

get { return mInverseFlattening; }

}

}

}

Generally, you won’t need to specify ellipsoid parameters. The Ellipsoid type has a number of static instances that define “reference ellipsoids”. Reference ellipsoids represent some organization’s consensus on the “best” ellipsoidal parameters to use to model Earth. Two of the most widely accepted reference ellipsoids are defined above: WGS84 (the 1984 World Geodetic System) and GRS80 (the 1980 Geodetic Reference System).

Finally, we have the class that actually implements Vincenty’s Formula to solve the Geodetic Inverse Problem given a reference ellipsoid and two sets of global coordinates (equation numbers in comments relate directly to Vincenty’s publication):

using System;

namespace Gavaghan.Geodesy

{

public class GeodeticCalculator

{

private const double TwoPi = 2.0 * Math.PI;

public GeodeticCurve CalculateGeodeticCurve(Ellipsoid ellipsoid,

GlobalCoordinates start, GlobalCoordinates end)

{

// get constants

double a = ellipsoid.SemiMajorAxis;

double b = ellipsoid.SemiMinorAxis;

double f = ellipsoid.Flattening;

// get parameters as radians

double phi1 = start.Latitude.Radians;

double lambda1 = start.Longitude.Radians;

double phi2 = end.Latitude.Radians;

double lambda2 = end.Longitude.Radians;

// calculations

double a2 = a * a;

double b2 = b * b;

double a2b2b2 = (a2 – b2) / b2;

double omega = lambda2 – lambda1;

double tanphi1 = Math.Tan(phi1);

double tanU1 = (1.0 – f) * tanphi1;

double U1 = Math.Atan(tanU1);

double sinU1 = Math.Sin(U1);

double cosU1 = Math.Cos(U1);

double tanphi2 = Math.Tan(phi2);

double tanU2 = (1.0 – f) * tanphi2;

double U2 = Math.Atan(tanU2);

double sinU2 = Math.Sin(U2);

double cosU2 = Math.Cos(U2);

double sinU1sinU2 = sinU1 * sinU2;

double cosU1sinU2 = cosU1 * sinU2;

double sinU1cosU2 = sinU1 * cosU2;

double cosU1cosU2 = cosU1 * cosU2;

// eq. 13

double lambda = omega;

// intermediates we’ll need to compute ‘s’

double A = 0.0;

double B = 0.0;

double sigma = 0.0;

double deltasigma = 0.0;

double lambda0;

bool converged = false;

for (int i = 0; i < 10; i++)

{

lambda0 = lambda;

double sinlambda = Math.Sin(lambda);

double coslambda = Math.Cos(lambda);

// eq. 14

double sin2sigma = (cosU2 * sinlambda * cosU2 * sinlambda)

+ (cosU1sinU2 – sinU1cosU2 * coslambda)

* (cosU1sinU2 – sinU1cosU2 * coslambda);

double sinsigma = Math.Sqrt(sin2sigma);

// eq. 15

double cossigma = sinU1sinU2 + (cosU1cosU2 * coslambda);

// eq. 16

sigma = Math.Atan2(sinsigma, cossigma);

// eq. 17 Careful! sin2sigma might be almost 0!

double sinalpha = (sin2sigma == 0) ? 0.0

: cosU1cosU2 * sinlambda / sinsigma;

double alpha = Math.Asin(sinalpha);

double cosalpha = Math.Cos(alpha);

double cos2alpha = cosalpha * cosalpha;

// eq. 18 Careful! cos2alpha might be almost 0!

double cos2sigmam = cos2alpha == 0.0 ? 0.0

: cossigma – 2 * sinU1sinU2 / cos2alpha;

double u2 = cos2alpha * a2b2b2;

double cos2sigmam2 = cos2sigmam * cos2sigmam;

// eq. 3

A = 1.0 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 – 175 * u2)));

// eq. 4

B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 – 47 * u2)));

// eq. 6

deltasigma = B * sinsigma * (cos2sigmam + B / 4

* (cossigma * (-1 + 2 * cos2sigmam2) – B / 6 * cos2sigmam

* (-3 + 4 * sin2sigma) * (-3 + 4 * cos2sigmam2)));

// eq. 10

double C = f / 16 * cos2alpha * (4 + f * (4 – 3 * cos2alpha));

// eq. 11 (modified)

lambda = omega + (1 – C) * f * sinalpha

* (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1 + 2 * cos2sigmam2)));

// see how much improvement we got

double change = Math.Abs((lambda – lambda0) / lambda);

if ((i > 1) && (change < 0.0000000000001))

{

converged = true;

break;

}

}

// eq. 19

double s = b * A * (sigma – deltasigma);

Angle alpha1;

Angle alpha2;

// didn’t converge? must be N/S

if (!converged)

{

if (phi1 > phi2)

{

alpha1 = Angle.Angle180;

alpha2 = Angle.Zero;

}

else if (phi1 < phi2)

{

alpha1 = Angle.Zero;

alpha2 = Angle.Angle180;

}

else

{

alpha1 = new Angle(Double.NaN);

alpha2 = new Angle(Double.NaN);

}

}

// else, it converged, so do the math

else

{

double radians;

alpha1 = new Angle();

alpha2 = new Angle();

// eq. 20

radians = Math.Atan2(cosU2 * Math.Sin(lambda),

(cosU1sinU2 – sinU1cosU2 * Math.Cos(lambda)));

if (radians < 0.0) radians += TwoPi;

alpha1.Radians = radians;

// eq. 21

radians = Math.Atan2(cosU1 * Math.Sin(lambda),

(-sinU1cosU2 + cosU1sinU2 * Math.Cos(lambda))) + Math.PI;

if (radians < 0.0) radians += TwoPi;

alpha2.Radians = radians;

}

return new GeodeticCurve(s, alpha1, alpha2);

}

}

}

There you have it! You have the tools you need to know the answer to the question “How far is it from here to there?”

Here’s an example of using the code to calculate how far it is from the Lincoln Memorial in Washington, D.C. to the Eiffel Tower in Paris:

using System;

using System.Text;

using Gavaghan.Geodesy;

namespace Gavaghan.Geodesy.Example

{

public class Example

{

static void Main()

{

// instantiate the calculator

GeodeticCalculator geoCalc = new GeodeticCalculator();

// select a reference elllipsoid

Ellipsoid reference = Ellipsoid.WGS84;

// set Lincoln Memorial coordinates

GlobalCoordinates lincolnMemorial;

lincolnMemorial = new GlobalCoordinates(

new Angle(38.88922), new Angle(-77.04978)

);

// set Eiffel Tower coordinates

GlobalCoordinates eiffelTower;

eiffelTower = new GlobalCoordinates(

new Angle(48.85889), new Angle(2.29583)

);

// calculate the geodetic curve

GeodeticCurve geoCurve = geoCalc.CalculateGeodeticCurve(

reference, lincolnMemorial, eiffelTower

);

double ellipseKilometers = geoCurve.EllipsoidalDistance / 1000.0;

Console.WriteLine(“Lincoln Memorial to Eiffel Tower using WGS84”);

Console.WriteLine(” Ellipsoidal Distance: {0:0.00} kilometers”,

ellipseKilometers

);

Console.WriteLine(” Azimuth: {0:0.00} degrees”,

geoCurve.Azimuth.Degrees

);

Console.WriteLine(” Reverse Azimuth: {0:0.00} degrees”,

geoCurve.ReverseAzimuth.Degrees

);

}

}

}

The library also supports 3-D geodetic calculations (measurements that account for the elevation above or below the reference ellipsoid). For complete source code, documentation, and examples, download the entire C# library.

Save/Share Google Yahoo! Add to Technorati Favorites Digg It Reddit
del.icio.us My Zimbio

25 Responses to “C#, GPS Receivers, and Geocaching: Vincenty’s Formula”

  1. Jay Allen Says:

    Read this on CodeProject.com. Great stuff – and great blog!

  2. Russ Lankenau Says:

    Mike,

    Great post! I’ve been looking over the Vincenty paper trying to get a handle on this, and I found some FORTAN and C++ code on the web, but your post is explained much more clearly than anything else I’ve found.

  3. Klaus Lenssen Says:

    Mike,

    excellent work – I am new to geodesy and I found your C# class very handy to write an small application to analyse the tracks I recorded with my new GPS mouse. Next thing is write a class to create kml files.

  4. Binaryjam Says:

    Mike, you are an absolute star ! I’m absolutely terrible at maths like this and your easy to use library enabled me to calculate all the adjacent postcodes in the UK for each other.

    Using this library I can improve the legacy car sharing app I wrote to accurately find other car sharers by bounding box filter and distance calculations.

    This is undoubtedly the best bit of C# code/ Complete library I have ever come across.

    Thankyou Soooo much !!!!

  5. Marcelo Ois Lagarde Says:

    Hi Mike, thanks for your excellent explanation. Your link to Vincenty’s original work is very useful too. Reading the GeodeticCalculator class I wonder why do you implement equations 3, 4 and 6 inside the loop. I think if you postpone that after the convergence question you could gain an extra performance.

  6. Mike Gavaghan Says:

    Marcelo,

    Thanks for the kind words.

    Eqs. 3, 4, and 6 must be inside the loop because they change on each iteration. They are used to calculate the next value of lambda – and are derived from the previous value of lambda (albeit, through a complex sequence of calculations). A and B are functions of u2, u2 is a function of alpha, and alpha is a function of lambda. “deltaSigma” is similar (it’s a function of B).

    The equation numbering is admittedly nonintuitive – but they aren’t my numbers. They correlate directly back to Vincenty’s publication.

    –Mike

  7. Kenny Says:

    Hi Mike: Your library saved me loads of time developing an applications I’m writing to track objects. Do you know if there is C# code available that would compute the midpoint between two GPS locations. In my case the two points would always be less than 100 yards apart.

  8. Jacob Says:

    @Kenny:
    To get the midpoint, simply use GeodeticCalculator to
    1. calculate the distance and initial bearing between the two points,
    2. calculate the point at half the distance and at the same bearing from the starting point.

  9. Stephen Says:

    Hi Mike – this looks like a great implementation, with excellent documentation. I’m looking to incorporate some inverse 3D geodetic calcs into a sunrise/sunset type app for photographers. Specifically, I need to be able to calculate the elevation angle (or altitude – I think there’s a subtle difference depending on if you’re an astronomer or a surveyor, but I don’t know what it is…) between two points.

    Taking your Pike’s Peak example in the source code, if you were standing on the plane east of Pike’s Peak and wanted to determine when the run might set behind it, I need the elevation angle between the two points.

    My question is: is it possible to use your implementation to return the zenith distance from which elevation angle/altitude can be derived? I can see the ElevationChange, but am unsure how I’d get to an accurate angle from this.

    Some examples of the problem I mean here: http://photo.net/nature-photography-forum/00SigL (about half way down the page) and here http://www.ngs.noaa.gov/cgi-bin/Inv_Fwd/invers3d.prl.

    Many thanks for making this code available.

    Stephen

  10. ramesh Says:

    excellent stuff for beginners… Thanks a lot…

  11. Daniel Says:

    Hi Mike,
    Thanks for sharing this. It is a great help and timesaver for my little freeware project: Generation of airspace boundaries to be uploaded to an Variometer/GPS unit for Paraglider and Hanglider pilots.
    Many thanks & Regards from Switzerland
    Daniel

  12. Markus Says:

    Hello Mike,

    Thanks for sharing your information. I found it very intriguing, but too much for my simple question. I was wondering if you could shed some light on my problem.

    I need to generate a bounding box of min and max latitude and longitude values so I can return addresses within a given radius.

    I have two func’s below to return the range I need to +/- to the lon lat coordinates to establish the bounding box. However I don’t think the math is 100%

    I am using the Haversine formula to generate my distance calculations and that seems to be working accurately. I investigated the Vincenty formula for distance as well but for my needs that was overkill.

    My problem is that I would like my search results to be as accurate as Haversine in that the range I +/- to the lon lat coordinates takes into account the ellipsoidal curvature or the earth.

    Ok, here are my func’s…please note I am not a Math professor :o)

    static Func GetLatitudeRange = (latitude, distance) => (180.0 / Math.PI) * (distance / EARTH_RADIUS_IN_KM);

    static Func GetLongitudeRange = (longitude, latitude, distance) =>
    (
    Math.Asin(
    Math.Sin(distance / EARTH_RADIUS_IN_KM) / Math.Cos(latitude)
    )
    );

    Many Thanks,
    Markus

  13. Aharon Levine Says:

    Hi Mike

    Thanks so much for sharing this brilliant code, it has proved invaluable to me in several projects.
    I would like to propose a small change which made the code more robust for real applications:
    The function GeodeticCalculator.calculateEndingGlobalCoordinates(…) goes into an infinite loop if its distance parameter is given as 0.
    I added these lines to the beginning of the function:

    if (distance == 0)
    {
    return new GlobalCoordinates(start.getLatitude(), start.getLongitude());
    }

    Thanks again
    Aharon

  14. steve Says:

    Hi Mike,

    Just wanted to say thanks for ur c# implementation of Vincenty’s formula,

    cheers,
    steve

  15. Robert Friedman Says:

    Hi Mike,
    This is great code. It is very well written and the example made it easy for my use. I am actually a retired engineer and I wanted to develop an application for use with my Garmin GPS. One feature was to measure the distance from a reference way point and everything else in the GPS. I started out using the Haversine sine method which gave me fairly good results. I found the distances were a mile or so off when measuring long distance of a few thousand miles across the US. My numbers never complelely agreed with my Garmin GPS when I view distances in the waypoint favorites lists. With a little Google searching, I found your C# code. Within a half hour, I had your distance calculations working. No changes whatsoever were needed. The distance calculations were in complete agreement with my GPS. Orginally I simply included your library code as an include project. This work fine except a .dll was created. I really didn’t want another .dll so I simply took all the c# code in Gavaghan.Geodsy and copied it into my source code. That embedded your code and I then had no need for the .dll;
    Thanks again for such a good piece of code.

  16. Clemens Says:

    Hi Mike,

    I just came across your implementation of Vincenty’s inverse formula.
    Two points are worth mentioning:

    1. Marcelo is right in his comment from June 2008. Equations 3, 4 and 6 do not have to be calculated inside the loop.
    Neither Vincenty in his original paper does it, nor does the pseudo code in Wikipedia (http://en.wikipedia.org/wiki/Vincenty's_formulae) or the online calculator on Movable Type Scripts (http://www.movable-type.co.uk/scripts/latlong-vincenty.html).

    2. In line 268 you should not use the relative change abs((lambda – lambda0) / lambda) as stop criterion, since lambda is a longitude difference, which may be zero.
    Calculating the distance between two point on the same meridian might well result in a division by zero. At least the algorithm iterates much to often.
    Simply use abs(lambda – lambda0) instead.

    Cheers
    Clemens

  17. Roman Says:

    Hi Mike,

    Thank you for sharing this valuable information with others!

    Regards,

    Roman

  18. S Pola Says:

    Hi Mike,

    Your explanation and your C# library itself are brilliant. I’ve made use of your library in a small application I’ve built to make a grid of points. Your code and the example made my task much easier. Thanks very much.

    Cheers
    S Pola

  19. D Pederson Says:

    Wow, wow, wow.

    As a SW engineer, I was looking to make up a project that was interesting and useful to my hobby, Geocaching. I have been thinking of creating a geodetic class library, and was studying others that are often MFC in C++. My hope was to make an organized .NET library in C#. No need now, this is well thought out, modern and well referenced. A textbook example of a design and implementation.

    Make your site a virtual cache so we can log it!

  20. Henri de Feraudy Says:

    Readers my like to know that a recent C++ open source development is GeographicLib, whose author claims is far more accurate than Vincenty’s formula. See http://geographiclib.sourceforge.net/.
    Nevertheless the code you have presented is much easier to get into, and for distances of less than a 100km might be accurate, but I havent done any comparisons.

  21. Andrew Jones Says:

    Mike,

    Great code! Can you confirm my assumption that Google Earth also uses the Vincenty’s formula with WGS84 when calculating the length of a multi point line. I find the distance calculations match exactly.

    Andrew

  22. Mike Gavaghan Says:

    I would assume so. WGS84 is a good model for calculating distances across the entire globe. If I’m not mistaken, the other geoids are either 1) intended to improve accuracy, but only for particular regions, or 2) are based on older measurements that were later refined by WGS84.

  23. online learning Says:

    online learning

    C#, GPS Receivers, and Geocaching: Vincenty’s Formula « Talk Nerdy To Me – Java, C#, .Net « Blog Archive

  24. Anonymous Says:

    Hi Mike,

    Thank you very much for this excellent library.
    Is there a way to :
    – have elevation angle between 2 GlobalPosition points ?
    – add “CalculateEndingGlobalPositions(reference, GlobalPosition_Start, Angle_bearing, Distance)”

    Thank you again.

  25. motus Says:

    Hi Mike,

    Thank you very much for this excellent library.
    Is there a way to :
    – have elevation angle between 2 GlobalPosition points ?
    – add “CalculateEndingGlobalPositions(reference, GlobalPosition_Start, Angle_bearing, Distance)”

    Thank you again.

Leave a Reply