diff --git a/MapControl/Track.cs b/MapControl/Track.cs index 6cf8959..b43bbd5 100644 --- a/MapControl/Track.cs +++ b/MapControl/Track.cs @@ -240,5 +240,108 @@ namespace csmapcontrol return bounds; } + /// + /// Calculate the length of the track. + /// + /// The length in metres. + public double CalculateLength () + { + TrackPoint last = null; + double dist = 0; + foreach (TrackPoint p in Points) { + if (last != null) { + dist += distance (last.Lat, last.Lon, p.Lat, p.Lon); + } + last = p; + } + return dist; + } + + /// + /// Calculate geodesic distance (in m) between two points specified by + /// latitude/longitude (in numeric degrees) using Vincenty inverse + /// formula for ellipsoids. + /// + /// Code based on a Javascript version by © 2002-2008 Chris Veness. + /// + /// That code contains the following: "You are welcome to re-use these scripts + /// [without any warranty express or implied] provided you retain my copyright notice + /// and when possible a link to my website (under a LGPL license)." + /// + /// The originating web page: http://www.movable-type.co.uk/scripts/latlong-vincenty.html + /// + /// In turn, this python conversion was done by Maarten Sneep and released + /// under the GPL V2 at http://www.xs4all.nl/~msneep/software/whereabouts-readme.html + /// + /// In turn, it was converted to C# by Ciaran Gultnieks. + /// + /// Latitude of first point. + /// Longitude of first point. + /// Latitude of second point. + /// Longitude of second point. + /// The distance in metres. + private double distance (double lat1, double lon1, double lat2, double lon2) + { + + double a = 6378137; + // in m + double b = 6356752.3142; + // in m + double f = 1 / 298.257223563; + double deg2rad = Math.PI / 180; + lat1 *= deg2rad; + lon1 *= deg2rad; + lat2 *= deg2rad; + lon2 *= deg2rad; + + double L = lon2 - lon1; + double U1 = Math.Atan ((1 - f) * Math.Tan (lat1)); + double U2 = Math.Atan ((1 - f) * Math.Tan (lat2)); + double sinU1 = Math.Sin (U1); + double cosU1 = Math.Cos (U1); + double sinU2 = Math.Sin (U2); + double cosU2 = Math.Cos (U2); + double Lambda = L; + double LambdaP = 2 * Math.PI; + int iterLimit = 20; + + double sinLambda, cosLambda, sinSigma = 0, cosSigma = 0; + double cosSqAlpha = 0, sinAlpha, cos2SigmaM = 0; + double C, sigma = 0; + + while (Math.Abs (Lambda - LambdaP) > 1e-12 && iterLimit > 0) { + sinLambda = Math.Sin (Lambda); + cosLambda = Math.Cos (Lambda); + sinSigma = Math.Sqrt (Math.Pow (cosU2 * sinLambda, 2) + Math.Pow (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2)); + if (sinSigma == 0) + return 0; + cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; + sigma = Math.Atan2 (sinSigma, cosSigma); + sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma; + cosSqAlpha = 1 - Math.Pow (sinAlpha, 2); + try { + + cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha; + } catch (Exception) { + cos2SigmaM = 0; + } + C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha)); + LambdaP = Lambda; + Lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * Math.Pow (cos2SigmaM, 2)))); + iterLimit -= 1; + + } + + if (iterLimit == 0) + return 0; + + double uSq = cosSqAlpha * (Math.Pow (a, 2) - Math.Pow (b, 2)) / Math.Pow (b, 2); + double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))); + double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); + double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * Math.Pow (cos2SigmaM, 2)) - B / 6 * cos2SigmaM * (-3 + 4 * Math.Pow (sinSigma, 2)) * (-3 + 4 * Math.Pow (cos2SigmaM, 2)))); + return b * A * (sigma - deltaSigma); + + } + } }